block_setpoint_generator_3d.c 28.8 KB
Newer Older
Jeroen Vreeken's avatar
Jeroen Vreeken committed
1
/*
2
	Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2007, 2008, 2013, 2014
3
	Copyright Stichting C.A. Muller Radioastronomiestation, 2007, 2008, 2013
Jeroen Vreeken's avatar
Jeroen Vreeken committed
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

	This program is free software: you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	This program is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU General Public License for more details.

	You should have received a copy of the GNU General Public License
	along with this program.  If not, see <http://www.gnu.org/licenses/>.

 */
19
#define _ISOC99_SOURCE
Jeroen Vreeken's avatar
Jeroen Vreeken committed
20
21
22
23
24

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
25
#include <float.h>
Jeroen Vreeken's avatar
Jeroen Vreeken committed
26

Jeroen Vreeken's avatar
Jeroen Vreeken committed
27
28
#include <controller/controller_block.h>
#include <controller/controller_command.h>
Jeroen Vreeken's avatar
Jeroen Vreeken committed
29
#include <controller/controller_time.h>
30
#include <controller/controller_sample.h>
Jeroen Vreeken's avatar
Jeroen Vreeken committed
31
#include <log/log.h>
Jeroen Vreeken's avatar
Jeroen Vreeken committed
32
33
34
35
36

/*
       inputs     outputs
       nr name    nr name

Jeroen Vreeken's avatar
Jeroen Vreeken committed
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
     -------------------------------
     |                             |
 ----| 0 reset_x       0  x        |----
     |                             |
 ----| 1 reset         1  v        |----
     |                             |
 ----| 2 track_x       2  a        |----
     |                             |
 ----| 3 track_v       3  j        |----
     |                             |
 ----| 4 track_x_cmd   4  setpoint |----
     |                             |
 ----| 5 track_v_cmd               |
     |                             |
     -------------------------------
Jeroen Vreeken's avatar
Jeroen Vreeken committed
52
53

	'setpoint' is the setpoint as the user requested it.
54
	'x' is the output of the setpoint generator.
Jeroen Vreeken's avatar
Jeroen Vreeken committed
55
	'v', 'a' and 'j' are the first, second and third order derivatives of 'x'.
Jeroen Vreeken's avatar
Jeroen Vreeken committed
56

Jeroen Vreeken's avatar
Jeroen Vreeken committed
57
58
59
60
	While reset is active the reset_x value is taken as new setpoint value.
	(Both x and setpoint are updated and are not limited by v, a, j)
	
	track_x and track_v are the inputs for setpoint and speed tracking commands.
Jeroen Vreeken's avatar
Jeroen Vreeken committed
61
62
63
64
65
66
67
68
69
70
71
72
73
 */

struct spg_command {
	int type;
	float setpoint;
	float speed;
	uint32_t time;
	
	int start;
	int done;
};

struct controller_block_private {
74
75
76
77
78
79
80
81
82
83
84
85
	/*
		In the setpoint generator blocks the following physics notations
		will be used.
	
		x = position	
		v = velocity (1st derivative of x)
		a = acceleration (2nd derivative of x)
		j = jerk  (3th derivative of x)
	*/


	float *reset_x;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
86
	bool  *reset;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
87
	float *track_x;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
88
89
90
	float *track_v;
	bool  *track_x_cmd;
	bool  *track_v_cmd;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
91
	
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
	/* beware: 'samples' is the time unit, not 'seconds', 
	   all parameters and commands are scaled on block entry/exit */
	double cmd_x;
	float  cmd_x_out; /* float version of the internal cmd_x */
	double cmd_v;
	float max_x;
	float min_x;
	float max_v;
	float max_a;
	float inv_max_a;
	float max_j;
	float inv_max_j;
	double precision_x;
	double precision_v;
	double precision_a;

	/* conversion factor for seconds/tick and its inverse */
	float freq;	/* ticks per second */
Jeroen Vreeken's avatar
Jeroen Vreeken committed
110
111
	float freq2;
	float freq3;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
112
	float tick;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
113
114
	double t_max_a;
	double v_delta_from_max_a;
115
	controller_trigger_time period;
116
117
118
119
120
121
122
123
124
125

	/* parameters in real world format (time unit: second) */
	float max_v_sec;
	float max_a_sec;
	float max_j_sec;

	float precision_x_sec;
	float precision_v_sec;
	float precision_a_sec;

126
127
128
129
130
131
132
	/* state at last change */
	double start_x;
	double start_v;
	double start_a;
	double start_j;
	int start_t;

133
134
135
136
137
138
139
140
141
	/* current internal state */
	double cur_x;
	double cur_v;
	double cur_a;
	double cur_j;
	float cur_x_out; /* float version of the internal values */
	float cur_v_out;
	float cur_a_out;
	float cur_j_out;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
142
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
143
	uint32_t id;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
144
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
145
146
	bool softreset;
	
147
	struct controller_command_entry cur_command;
148
149
	bool cur_done;
	bool cur_start;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
150
	uint64_t command_t;
151
152

	struct controller_command *command;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
153
154
};

155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180

static bool almost_equal(double d1, double d2)
{
	if (fabs(d1 - d2) <= 16 * DBL_EPSILON * fmax(fabs(d1), fabs(d2)) )
		return true;
	else
		return false;
}

/* How many time ticks are needed to go from a to 0 */
static double ticks_to_a(struct controller_block_private *priv, double a_start, double a_at_t)
{
	double t;
	
	t = fabs(a_start - a_at_t) * priv->inv_max_j;

	return ceil(t);
}

/* time to a constant speed */
static double ticks_to_v(struct controller_block_private *priv, 
    double v_start, double v_at_t)
{
	double t;
	
	/* 	From a constant speed to a constant speed is done in two halves:
Jeroen Vreeken's avatar
Jeroen Vreeken committed
181
182
		First halve (untill half the speed difference is reached) is 
		done at max jerk, second half is done at -max jerk.
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
		
		1/2 v = 1/2 j t^2   ->   v = j t^2   ->  t = sqrt(v/j) 
		
		Second half takes just as long.... return 2*t
	*/
	
	t = sqrt(fabs(v_at_t - v_start) * priv->inv_max_j);
	
	return ceil(t) * 2;
}

static double a_after_ticks(struct controller_block_private *priv, 
    double a, double j, double t)
{
	double a_at_ticks;
	
	a_at_ticks = 
	   a +
	   j * t;

	return a_at_ticks;
}

static double v_after_ticks(struct controller_block_private *priv, 
    double v, double a, double j, double t)
{
	double v_at_ticks;
	
	v_at_ticks = 
	    v +
	    a * t + 
	    1.0/2.0 * j * t * t;

	return v_at_ticks;
}

static double x_after_ticks(struct controller_block_private *priv, 
    double x, double v, double a, double j, double t)
{
	double x_at_t;
	
	x_at_t =
	    x +
	    v * t +
	    1.0/2.0 * a * t * t +
	    1.0/6.0 * j * t * t * t;

	return x_at_t;
}

233
static void setpoint_generator_3d_calculate(struct controller_block *spg)
Jeroen Vreeken's avatar
Jeroen Vreeken committed
234
235
{
	struct controller_block_private *priv = spg->private;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
236
	double cur_x, cur_v;
237
	double t_max_a;
238
	bool ignore_x = false;
239
	bool must_brake = false;
240
241
	bool good_x;
	bool good_v;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
242

Jeroen Vreeken's avatar
Jeroen Vreeken committed
243
244
245
	cur_x = priv->cur_x;
	cur_v = priv->cur_v;

246
247
	t_max_a = priv->t_max_a;

Jeroen Vreeken's avatar
Jeroen Vreeken committed
248
	if (*priv->reset) {
249
		struct controller_command_entry entry;
250
	
251
		priv->cmd_x = *priv->reset_x;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
252
		cur_x = priv->cmd_x;
253
		priv->cur_done = true;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
		if (!priv->softreset) {
			priv->cur_command.type = COMMAND_PTYPE_SETPOINT;
			priv->cmd_v = 0.0;
			cur_v = 0.0;
			priv->cur_a = 0.0;
			priv->cur_j = 0.0;
			priv->start_x = cur_x;
			priv->start_v = 0.0;
			priv->start_a = 0.0;
			priv->start_j = 0.0;
			priv->start_t = 0;
			priv->id = COMMAND_ID_NONE;
	
			goto set_output;
		}
269
		controller_command_queue_read(priv->command, &entry);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
270
	}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
271
272
273
274
275
276
277
	if (*priv->track_x_cmd) {
		priv->cur_done = false;
		priv->cur_command.type = COMMAND_PTYPE_SETPOINT_TRACK;
	} else if (*priv->track_v_cmd) {
		priv->cur_done = false;
		priv->cur_command.type = COMMAND_PTYPE_SPEED_TRACK;
	}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
278

279
280
281
282
283
284
285
	if (priv->cur_done) {
		int r;
		
		r = controller_command_queue_read(priv->command, &priv->cur_command);
		if (r == 0) {
			priv->cur_done = false;
			priv->cur_start = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
286
			priv->id = priv->cur_command.id;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
287
		} else {
288
289
			if (priv->cur_command.type ==
			    COMMAND_PTYPE_SETPOINT_TIME) {
290
				priv->cmd_v = 0.0;
291
				priv->cmd_x = priv->cur_command.value.f;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
292
			}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
293
			priv->id = COMMAND_ID_NONE;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
294
295
		}
	}
296
297
298
299
	if (!priv->cur_done) {
		switch (priv->cur_command.type) {
			case COMMAND_PTYPE_SETPOINT:
				priv->cmd_x = priv->cur_command.value.f;
300
				priv->cmd_v = 0.0;
301
302
				priv->cur_done = true;
				break;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
303
304
305
			case COMMAND_PTYPE_SETPOINT_TRACK:
				priv->cur_done = true;
				break;
306
			case COMMAND_PTYPE_SPEED:
Jeroen Vreeken's avatar
Jeroen Vreeken committed
307
				priv->cmd_v = priv->cur_command.value.f * controller_time_period_get(spg->time);
308
309
				priv->cur_done = true;
				break;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
310
311
312
			case COMMAND_PTYPE_SPEED_TRACK:
				priv->cur_done = true;
				break;
313
314
			case COMMAND_PTYPE_SETPOINT_TIME:
				if (!priv->cur_start) {
315
					controller_trigger_time command_t;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
316
317
					int64_t t;
					
318
					command_t = priv->cur_command.t;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
319
					
320
					t = (command_t - controller_sample_timestamp) / priv->period;
321

Jeroen Vreeken's avatar
Jeroen Vreeken committed
322
					if (t < 0) {
Jeroen Vreeken's avatar
Jeroen Vreeken committed
323
324
						/* Command is to old, pretend it
						   is a setpoint without time */
Jeroen Vreeken's avatar
Jeroen Vreeken committed
325
						log_send(LOG_T_WARNING, "%s: setpoint's time is in the past, clock skew? (%lld < %lld)",
326
						    spg->name, (long long)priv->cur_command.t, (long long)controller_sample_timestamp);
327
						priv->cmd_x = priv->cur_command.value.f;
328
						priv->cmd_v = 0.0;
329
						priv->cur_done = true;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
330
					} else {
331
						priv->cmd_v =
332
						    (priv->cur_command.value.f -
Jeroen Vreeken's avatar
Jeroen Vreeken committed
333
						    priv->cmd_x) / (t+1.0);
334
						priv->cur_start = true;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
335
336
337
					}
				}
				
338
				if (priv->cur_command.t <= controller_sample_timestamp) {
339
					priv->cur_done = true;
340
					priv->cmd_x =
341
					   priv->cur_command.value.f -
342
					   priv->cmd_v;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
343
				}
344
345
346
				break;
			default:
				break;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
347
348
349
		}
	}

350
	if (priv->cur_command.type == COMMAND_PTYPE_SPEED) {
351
		ignore_x = true;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
352
		priv->cmd_x = cur_x + priv->cmd_v;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
353
	}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
354
	if (priv->cur_command.type == COMMAND_PTYPE_SPEED_TRACK) {
Jeroen Vreeken's avatar
Jeroen Vreeken committed
355
		float track_v = *priv->track_v * priv->tick;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
356
357
358
359
		ignore_x = true;
		priv->cmd_v = track_v;
		priv->cmd_x = cur_x + track_v * controller_time_period_get(spg->time);
	}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
360
361
	if (priv->cur_command.type == COMMAND_PTYPE_SETPOINT_TRACK) {
		priv->cmd_x = *priv->track_x;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
362
		priv->cmd_v = 0.0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
363
	}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
364
	
365
366
367
368
	if (priv->cmd_x > priv->max_x)
		priv->cmd_x = priv->max_x;
	if (priv->cmd_x < priv->min_x)
		priv->cmd_x = priv->min_x;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
369
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
370
371
	good_x = almost_equal(cur_x, priv->cmd_x);
	good_v = almost_equal(cur_v, priv->cmd_v);
372
373
374
375
376
377
378

	if (!good_v || !good_x) {
		double error_x;
		double req_x_1;
		double error_x_jpos;
		double error_x_j0;
		double error_x_jneg;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
379
		double error_v = priv->cmd_v - cur_v;
380
		double x, v, a, j, t;
381
		double req_x, req_v, v_delta_from_max_a;
382
		double error_v_after_a, error_x_at_v;
383
		double j_from_pos;
384
385
386
387
388
389
390
		bool state_at_max_a = false;
		bool state_to_max_a = false;
		
		/* procedure: where would we end up if we go to
		   requested speed. 
		 */
		
Jeroen Vreeken's avatar
Jeroen Vreeken committed
391
392
		x = cur_x;
		v = cur_v;
393
394
395
396
		a = priv->cur_a;
		req_x = priv->cmd_x;
		req_v = priv->cmd_v;
		
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
		req_x_1 = req_x + req_v;
		
		error_x_jpos = req_x_1 - x_after_ticks(priv, x, v, a, priv->max_j, 1);
		error_x_j0 = req_x_1 - x_after_ticks(priv, x, v, a, 0, 1);
		error_x_jneg = req_x_1 - x_after_ticks(priv, x, v, a, -priv->max_j, 1);
		if (fabs(error_x_jpos) < fabs(error_x_jneg)) {
			error_x = error_x_jpos;
			j_from_pos = priv->max_j;
		} else {
			error_x = error_x_jneg;
			j_from_pos = -priv->max_j;
		}
		if (fabs(error_x_j0) < fabs(error_x)) {
			error_x = error_x_j0;
			j_from_pos = 0;
		}
		
Jeroen Vreeken's avatar
Jeroen Vreeken committed
414
		v_delta_from_max_a = priv->v_delta_from_max_a;
415
416

		j = copysign(priv->max_j, error_v);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
417
			
418
		if (fabs(a) >= priv->max_j) {
419
420
421
422
423
424
			/* Not at constant velocity */
		
			if (signbit(a) != signbit(error_v)) {
				/* We are not accelerating towards cmd speed.
				   Go to constant velocity first
				 */
Jeroen Vreeken's avatar
Jeroen Vreeken committed
425
			
426
427
428
429
430
				t = ticks_to_a(priv, a, 0);
				x = x_after_ticks(priv, x, v, a, j, t);
				v = v_after_ticks(priv, v, a, j, t);
				a = a_after_ticks(priv, a, j, t);
				req_x = req_x + req_v * t;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
431

432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
				state_to_max_a = true;
			} else {
				/* We are accelerating towards cmd speed. */
				double a_peak;
				double t_to_max_a, t_a, t_at_max_a, t_a_to_0;
				double v_end_max_a, v_start_max_a, v_started_a, v_delta_a;

				/* speed we had when starting acceleration */
				t_a = ticks_to_a(priv, 0, a);
				v_delta_a = v_after_ticks(priv, 0, 0, priv->max_j, t_a);
				v_started_a = v - copysign(v_delta_a, a);

				/* What is the highest a we get? */
				t_a_to_0 = ticks_to_v(priv, v_started_a, req_v) * 0.5;
				a_peak = a_after_ticks(priv, 0, j, t_a_to_0);
				
				if (fabs(a) > fabs(a_peak)) {
					/* We are accelerating much faster than needed acc down first */
					t_a_to_0 = ticks_to_a(priv, a, 0);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
451
				
452
				} else if (fabs(a_peak) > priv->max_a) {
453
					/* We are going to hit maximum acc */
454
					a_peak = copysign(priv->max_a, a_peak);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
455
					
456
457
					t = ticks_to_a(priv, 0, a_peak);
					v_start_max_a = v_after_ticks(priv, v_started_a, 0, j, t);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
458
					
459
					v_end_max_a = req_v - copysign(v_delta_from_max_a, error_v);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
460
					
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
					t_at_max_a = fabs(v_end_max_a - v_start_max_a) * priv->inv_max_a;
					t_to_max_a = t - t_a;
					
					if (t_to_max_a >= 1.0) {
						state_to_max_a = true;
						
						x = x_after_ticks(priv, x, v, a, j, t_to_max_a);
						v = v_after_ticks(priv, v, a, j, t_to_max_a);
						a = a_after_ticks(priv, a, j, t_to_max_a);
						req_x = req_x + req_v * t_to_max_a;
					} else {
						state_at_max_a = true;
					}
					x = x_after_ticks(priv, x, v, a, 0, t_at_max_a);
					v = v_after_ticks(priv, v, a, 0, t_at_max_a);
					a = a_after_ticks(priv, a, 0, t_at_max_a);
					req_x = req_x + req_v * t_at_max_a;
					
					t_a_to_0 = t_max_a;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
480
				} else {
481
482
483
484
485
					t_to_max_a = t_a_to_0 - t_a;
					
					if (t_to_max_a >= 1.0) {
						state_to_max_a = true;
						
486
						/* accelerate to max a */
487
488
489
490
491
492
493
						x = x_after_ticks(priv, x, v, a, j, t_to_max_a);
						v = v_after_ticks(priv, v, a, j, t_to_max_a);
						a = a_after_ticks(priv, a, j, t_to_max_a);
						req_x = req_x + req_v * t_to_max_a;
					} else {
						state_at_max_a = true;
					}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
494
				}
495
				/* decellerate to a==0 */
496
497
498
499
				x = x_after_ticks(priv, x, v, a, -j, t_a_to_0);
				v = v_after_ticks(priv, v, a, -j, t_a_to_0);
				req_x = req_x + req_v * t_a_to_0;
				a = 0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
500
			}
501
502
		} else {
			state_to_max_a = true;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
503
		}
504
505
506
507
508
509
510
511
512
513
514
515
516

		/* We are at constant velocity and need to go to another
		   constant velocity
		 */
		
		error_v_after_a = priv->cmd_v - v;
		
		if (fabs(error_v_after_a) > v_delta_from_max_a * 2) {
			/* Going to hit max a: trapeze 
			        ___
			       /   \
			 */
			double t_at_max_a;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
517
			
518
519
			x = x_after_ticks(priv, x, v, 0, j, t_max_a);
			v = v_after_ticks(priv, v, 0, j, t_max_a);
520
			a = copysign(priv->max_a, j);
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
			req_x = req_x + req_v * t_max_a;

			t_at_max_a = (fabs(error_v_after_a) - v_delta_from_max_a*2) * priv->inv_max_a;
			x = x_after_ticks(priv, x, v, a, 0, t_at_max_a);
			v = v_after_ticks(priv, v, a, 0, t_at_max_a);
			a = a_after_ticks(priv, a, 0, t_at_max_a);
			req_x = req_x + req_v * t_at_max_a;

			x = x_after_ticks(priv, x, v, a, -j, t_max_a);
			v = v_after_ticks(priv, v, a, -j, t_max_a);
			req_x = req_x + req_v * t_max_a;
			a = 0;
		} else {
			/* Simple profile: triangle 
			 
			       /\
			 */
Jeroen Vreeken's avatar
Jeroen Vreeken committed
538
			
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
			t = ticks_to_v(priv, 0, error_v_after_a) * 0.5;

			x = x_after_ticks(priv, x, v, 0, j, t);
			v = v_after_ticks(priv, v, 0, j, t);
			a = a_after_ticks(priv, 0, j, t);
			req_x = req_x + req_v * t;

			x = x_after_ticks(priv, x, v, a, -j, t);
			v = v_after_ticks(priv, v, a, -j, t);
			a = a_after_ticks(priv, a, -j, t);
			req_x = req_x + req_v * t;
			a = 0;
		}
		
		error_x_at_v = req_x - x;
		
555
556
		
		if (fabs(error_x_at_v) < fabs(error_x) || 
Jeroen Vreeken's avatar
Jeroen Vreeken committed
557
		    (signbit(error_x) != signbit(error_x_at_v) /*&& !state_to_max_a*/ && !state_at_max_a) ||
558
559
		    ignore_x) {
			if (!ignore_x &&
560
			    signbit(error_x_at_v) == signbit(error_x) ) {
561
				priv->cur_j = j_from_pos;
562
563
564
565
566
567
568
			} else {
				if (state_to_max_a) {
					priv->cur_j = j;
				} else if (state_at_max_a) {
					priv->cur_j = 0;
				} else {
					priv->cur_j = -j;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
569
570
571
				}
			}
		} else {
572
573
574
			/* going to requested speed would make position error
			   bigger, first go to position.
			 */
575
			priv->cur_j = j_from_pos;
576
577
578
		}
	}
	
579
	
580
	/* When moving can we brake before the position limits? */
Jeroen Vreeken's avatar
Jeroen Vreeken committed
581
	if (fabs(cur_v) > 0.0) {
582
583
584
		double t, x, v, a, j;
		bool done = false;
		
Jeroen Vreeken's avatar
Jeroen Vreeken committed
585
586
		x = cur_x;
		v = cur_v;
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
		a = priv->cur_a;

		/* add 1 tick, we want to know if we are still safe untill the next
		   tick, not only this one 
		 */
		x = x_after_ticks(priv, x, v, a, priv->cur_j, 1);
		v = v_after_ticks(priv, v, a, priv->cur_j, 1);
		a = a_after_ticks(priv, a, priv->cur_j, 1);
		
		j = -copysign(priv->max_j, v);
		
		/* accelerating? */
		if (fabs(priv->cur_a) > 0.0) {
			/* Is our speed still increasing? */
			if (signbit(a) == signbit(v)) {
				/* Reduce a to zero first,
				   then use normal constant v to zero checks */
			
				t = ticks_to_a(priv, a, 0);
			
				x = x_after_ticks(priv, x, v, a, j, t);
				v = v_after_ticks(priv, v, a, j, t);
				a = 0;
			} else {
				/* Speed already going down, 
				   should we try to go down faster? */
				if (almost_equal(priv->cur_j, j)) {
					/* We can't do any better... */
					done = true;
				} else if (fabs(a) > priv->max_a - priv->max_j) {
					/* Already at max a, can't do better */
					done = true;
				} else {
					/* reducing a */
					t = ticks_to_a(priv, 0, a);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
622

623
624
625
					x = x_after_ticks(priv, x, v, a, -j, t);
					v = v_after_ticks(priv, v, a, -j, t);
					a = 0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
626

627
					/* Now at constant v */
Jeroen Vreeken's avatar
Jeroen Vreeken committed
628
629
630
631
				}
			}
		}

632
633
634
635
		if (!done) {
			/* Assuming we have a constant v,
			   check if we have to start deceleration now. */

Jeroen Vreeken's avatar
Jeroen Vreeken committed
636
			t = ticks_to_v(priv, cur_v, 0);
637
638
639
640
		
			/* will we hit max a? */
			if (fabs(a_after_ticks(priv, a, j, t/2)) > priv->max_a) {
				/* 3-part a profile: /-\ */
Jeroen Vreeken's avatar
Jeroen Vreeken committed
641
			
642
				double v_start_3, t_2;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
643
			
Jeroen Vreeken's avatar
Jeroen Vreeken committed
644
				t = t_max_a;
645
646
647
			
				x = x_after_ticks(priv, x, v, a, j, t);
				v = v_after_ticks(priv, v, a, j, t);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
648
				
649
650
651
652
653
654
655
656
657
658
659
660
661
662
				/* v at start of 3th period */
				v_start_3 = v_after_ticks(priv, 0, a, j, t);
				
				/* length of 2nd period */
				t_2 = fabs(v - v_start_3) * priv->inv_max_a;
			
				/* period 2 */
				x = x_after_ticks(priv, x, v, a, 0, t_2);
				v = v_after_ticks(priv, v, a, 0, t_2);
			
				/* period 3 */
				x = x_after_ticks(priv, x, v, a, -j, t);
			} else {
				/* 2-part profile: /\ */
Jeroen Vreeken's avatar
Jeroen Vreeken committed
663

664
665
666
				/* first half */
				x = x_after_ticks(priv, x, v, a, j, t/2);
				v = v_after_ticks(priv, v, a, j, t/2);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
667
		
668
669
670
				/* second half, swap j to reduce a and v to 0 */
				x = x_after_ticks(priv, x, v, a, -j, t/2);
			}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
671
		
672
673
674
			/* once we are still, would we still be within limits? */
			if ((x > priv->max_x || x < priv->min_x) && priv->cur_j != j) {
				priv->cur_j = j;
675
				must_brake = true;
676
			}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
677
678
679
		}
	}

680
	/* If accelerating, can we decelerate before going beyond our max vel */
681
682
	if (fabs(priv->cur_a) > 0.0) {
		double t, v, a, j;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
683
		
Jeroen Vreeken's avatar
Jeroen Vreeken committed
684
		v = cur_v;
685
686
687
688
689
690
691
		a = priv->cur_a;

		/* add 1 tick, we want to know if we are still safe untill the next
		   tick, not only this one 
		 */
		v = v_after_ticks(priv, v, a, priv->cur_j, 1);
		a = a_after_ticks(priv, a, priv->cur_j, 1);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
692
		
693
694
695
696
697
698
699
700
701
702
703
704
705
706
		j = -copysign(priv->max_j, a);
		
		t = ticks_to_a(priv, a, 0);
		
		v = v_after_ticks(priv, v, a, j, t);
		
		/* If not already at the right jerk, set it */
		if (fabs(v) > priv->max_v && signbit(priv->cur_j) != signbit(j)) {
			if (t <= 1) {
				priv->cur_j = 0;
				priv->cur_a = 0;
			} else {
				priv->cur_j = j;
			}
707
			must_brake = true;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
708
709
710
		}
	}

711
712
713
714
715

	if (!ignore_x) {
		priv->cmd_x += priv->cmd_v;
	}

716
717
	/* Is the difference between spg and command small enough?
	   If so, make outputs equal to command */
718
	if (!must_brake &&
Jeroen Vreeken's avatar
Jeroen Vreeken committed
719
720
721
	    fabs(priv->cmd_x - cur_x) <= priv->precision_x &&
	    fabs(priv->cmd_v - cur_v) <= priv->precision_v &&
	    fabs(priv->cur_a) <= priv->precision_a) {
722
723
		priv->cur_j = 0.0;
		priv->cur_a = 0.0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
724
725
		cur_v = priv->cmd_v;
		cur_x = priv->cmd_x;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
726
727
	}

728
729
730
731
	/* new jerk? */
	if (!almost_equal(priv->cur_j, priv->start_j)) {
		priv->start_j = priv->cur_j;
		priv->start_a = priv->cur_a;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
732
733
		priv->start_v = cur_v;
		priv->start_x = cur_x;
734
735
736
737
738
739
		priv->start_t = 0;
	}
	priv->start_t++;
	
	priv->cur_a = a_after_ticks(priv, 
	    priv->start_a, priv->start_j, priv->start_t);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
740
	cur_v = v_after_ticks(priv, 
741
	    priv->start_v, priv->start_a, priv->start_j, priv->start_t);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
742
	cur_x = x_after_ticks(priv, 
743
744
745
	    priv->start_x, priv->start_v, priv->start_a, priv->start_j, 
	    priv->start_t);

Jeroen Vreeken's avatar
Jeroen Vreeken committed
746

747
748
749
	if (fabs(priv->cur_a) > priv->max_a) {
		priv->cur_a = copysign(priv->max_a, priv->cur_a);
		priv->cur_j = 0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
750

751
752
		priv->start_j = priv->cur_j;
		priv->start_a = priv->cur_a;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
753
754
		priv->start_v = cur_v;
		priv->start_x = cur_x;
755
756
		priv->start_t = 0;
	}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
757

Jeroen Vreeken's avatar
Jeroen Vreeken committed
758
	if (fabs(cur_v) >= priv->max_v) {
759
		/* prevent further acceleration beyond max v */
Jeroen Vreeken's avatar
Jeroen Vreeken committed
760
		if (signbit(cur_v) == signbit(priv->cur_a)) {
761
			priv->cur_a = 0.0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
762
		}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
763
		if (signbit(cur_v) == signbit(priv->cur_j)) {
764
			priv->cur_j = 0.0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
765
		}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
766
		cur_v = copysign(priv->max_v, cur_v);
767
768
769

		priv->start_j = priv->cur_j;
		priv->start_a = priv->cur_a;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
770
771
		priv->start_v = cur_v;
		priv->start_x = cur_x;
772
		priv->start_t = 0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
773
774
	}
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
775
776
777
	if (cur_x > priv->max_x) {
		cur_x = priv->max_x;
		cur_v = 0;
778
779
		priv->cur_a = 0;
		priv->cur_j = 0;
780
781
782

		priv->start_j = priv->cur_j;
		priv->start_a = priv->cur_a;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
783
784
		priv->start_v = cur_v;
		priv->start_x = cur_x;
785
		priv->start_t = 0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
786
	}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
787
788
789
	if (cur_x < priv->min_x) {
		cur_x = priv->min_x;
		cur_v = 0;
790
791
		priv->cur_a = 0;
		priv->cur_j = 0;
792
793
794

		priv->start_j = priv->cur_j;
		priv->start_a = priv->cur_a;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
795
796
		priv->start_v = cur_v;
		priv->start_x = cur_x;
797
		priv->start_t = 0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
798
799
	}

Jeroen Vreeken's avatar
Jeroen Vreeken committed
800
set_output:
Jeroen Vreeken's avatar
Jeroen Vreeken committed
801
802
803
804
	priv->cur_x_out = cur_x;
	priv->cur_v_out = cur_v * priv->freq;
	priv->cur_a_out = priv->cur_a * priv->freq2;
	priv->cur_j_out = priv->cur_j * priv->freq3;
805
	priv->cmd_x_out = priv->cmd_x;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
806
807
	priv->cur_x = cur_x;
	priv->cur_v = cur_v;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
808
809
}

810
811
static int block_setpoint_generator_command_filter(struct controller_command *command,
    struct command_entry  *entry)
Jeroen Vreeken's avatar
Jeroen Vreeken committed
812
{
813
814
	struct controller_block_private *priv = command->block->private;
	int r = 0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
815
	
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
	switch (entry->type) {
		case COMMAND_PTYPE_SETPOINT_TIME:
		case COMMAND_PTYPE_SETPOINT: {
			float value = entry->value.f;
			
			if (value > priv->max_x)
				value = priv->max_x;
			if (value < priv->min_x)
				value = priv->min_x;
				
			entry->value.f = value;
			break;
		}
		case COMMAND_PTYPE_SPEED: {
			float value = entry->value.f;
			
			if (fabs(value) > priv->max_v_sec)
				value = copysign(priv->max_v_sec, value);
			
			entry->value.f = value;
			
			break;
		}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
839
		case COMMAND_PTYPE_SETPOINT_TRACK:
Jeroen Vreeken's avatar
Jeroen Vreeken committed
840
		case COMMAND_PTYPE_SPEED_TRACK:
Jeroen Vreeken's avatar
Jeroen Vreeken committed
841
			break;
842
843
844
845
		default:
			r = -1;
			break;
	}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
846

847
	return r;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
848
849
}

850
static void scale(struct controller_block *spg)
Jeroen Vreeken's avatar
Jeroen Vreeken committed
851
{
Jeroen Vreeken's avatar
Jeroen Vreeken committed
852
	double t_max_a;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
853
	double tick = controller_time_period_get(spg->time);
854
	struct timespec ts;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
855
	
856
	/* Scale all settings to sample time unit */
Jeroen Vreeken's avatar
Jeroen Vreeken committed
857
858
859
	spg->private->max_v = spg->private->max_v_sec * tick;
	spg->private->max_a = spg->private->max_a_sec * tick * tick;
	spg->private->max_j = spg->private->max_j_sec * tick * tick * tick;
860
	spg->private->precision_x = spg->private->precision_x_sec;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
861
862
	spg->private->precision_v = spg->private->precision_v_sec * tick;
	spg->private->precision_a = spg->private->precision_a_sec * tick * tick;
863

Jeroen Vreeken's avatar
Jeroen Vreeken committed
864
	spg->private->tick = tick;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
865
	spg->private->freq = 1.0 / tick;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
866
867
	spg->private->freq2 = spg->private->freq * spg->private->freq;
	spg->private->freq3 = spg->private->freq2 * spg->private->freq;
868
869
	spg->private->inv_max_j = 1.0 / spg->private->max_j;
	spg->private->inv_max_a = 1.0 / spg->private->max_a;
870
871
872
	ts.tv_sec = tick;
	ts.tv_nsec = (uint64_t)(tick * 1000000000) % 1000000000;
	spg->private->period = controller_sample_trigger->timespec2timestamp(&ts);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
873
874
875
876
877
878

	/* Calculate delta v when comming from max_a */
	t_max_a = ticks_to_a(spg->private, 0, spg->private->max_a);
	spg->private->v_delta_from_max_a = 
	    v_after_ticks(spg->private, 0, 0, spg->private->max_j, t_max_a);
	spg->private->t_max_a = t_max_a;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
879
880
}

881
882
static int param_set_setpoint(struct controller_block *spg, char *param,
    int argc, va_list val)
883
884
885
886
887
888
889
890
891
892
893
894
895
{
	spg->private->cmd_x = va_arg(val, double);
	spg->private->cur_x = spg->private->cmd_x;
	spg->private->cur_done = true;
	spg->private->cmd_v = 0.0;
	spg->private->cur_v = 0.0;
	spg->private->cur_a = 0.0;
	spg->private->cur_j = 0.0;

	scale(spg);
	return 0;
}

896
897
static int param_set_max_x(struct controller_block *spg, char *param,
    int argc, va_list val)
898
899
900
901
902
903
{
	spg->private->max_x = va_arg(val, double);
	scale(spg);
	return 0;
}

904
905
static int param_set_min_x(struct controller_block *spg, char *param,
    int argc, va_list val)
906
907
908
909
910
911
{
	spg->private->min_x = va_arg(val, double);
	scale(spg);
	return 0;
}

912
913
static int param_set_max_v(struct controller_block *spg, char *param,
    int argc, va_list val)
914
915
916
917
918
919
{
	spg->private->max_v_sec = va_arg(val, double);
	scale(spg);
	return 0;
}

920
921
static int param_set_max_a(struct controller_block *spg, char *param,
    int argc, va_list val)
922
923
924
925
926
927
{
	spg->private->max_a_sec = va_arg(val, double);
	scale(spg);
	return 0;
}

928
929
static int param_set_max_j(struct controller_block *spg, char *param,
    int argc, va_list val)
930
931
932
933
934
935
{
	spg->private->max_j_sec = va_arg(val, double);
	scale(spg);
	return 0;
}

936
937
static int param_set_precision_x(struct controller_block *spg, char *param,
    int argc, va_list val)
938
939
940
941
942
943
{
	spg->private->precision_x_sec = va_arg(val, double);
	scale(spg);
	return 0;
}

944
945
static int param_set_precision_v(struct controller_block *spg, char *param,
    int argc, va_list val)
946
947
948
949
950
951
{
	spg->private->precision_v_sec = va_arg(val, double);
	scale(spg);
	return 0;
}

952
953
static int param_set_precision_a(struct controller_block *spg, char *param,
    int argc, va_list val)
954
955
956
957
958
959
{
	spg->private->precision_a_sec = va_arg(val, double);
	scale(spg);
	return 0;
}

Jeroen Vreeken's avatar
Jeroen Vreeken committed
960
961
962
963
964
965
966
967
static int param_set_softreset(struct controller_block *spg, char *param,
    int argc, va_list val)
{
	spg->private->softreset = va_arg(val, int);
	return 0;
}


968
static struct controller_block_param_list params[] = {
969
970
971
972
973
974
975
976
977
	{ "setpoint",    true, param_set_setpoint, .args = { "double", NULL } },
	{ "max_x",       true, param_set_max_x, .args = { "double", NULL } },
	{ "min_x",       true, param_set_min_x, .args = { "double", NULL } },
	{ "max_v",       true, param_set_max_v, .args = { "double", NULL } },
	{ "max_a",       true, param_set_max_a, .args = { "double", NULL } },
	{ "max_j",       true, param_set_max_j, .args = { "double", NULL } },
	{ "precision_x", true, param_set_precision_x, .args = { "double", NULL } },
	{ "precision_v", true, param_set_precision_v, .args = { "double", NULL } },
	{ "precision_a", true, param_set_precision_a, .args = { "double", NULL } },
Jeroen Vreeken's avatar
Jeroen Vreeken committed
978
	{ "softreset",   false, param_set_softreset, .args = { "int", NULL } },
979
980
981
982
	{ NULL },
};


983
static struct controller_block_interm_list interms[] = {
Jeroen Vreeken's avatar
Jeroen Vreeken committed
984
985
986
987
988
989
	{ "reset_x",     CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, reset_x)     },
	{ "reset",       CONTROLLER_BLOCK_TERM_BOOL,  offsetof(struct controller_block_private, reset)       },
	{ "track_x",     CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, track_x)     },
	{ "track_v",     CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, track_v)     },
	{ "track_x_cmd", CONTROLLER_BLOCK_TERM_BOOL,  offsetof(struct controller_block_private, track_x_cmd) },
	{ "track_v_cmd", CONTROLLER_BLOCK_TERM_BOOL,  offsetof(struct controller_block_private, track_v_cmd) },
990
991
992
993
994
995
996
997
998
	{ NULL }
};

static struct controller_block_outterm_list outterms[] = {
	{ "x",        CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cur_x_out) },
	{ "v",        CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cur_v_out) },
	{ "a",        CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cur_a_out) },
	{ "j",        CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cur_j_out) },
	{ "setpoint", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cmd_x_out) },
Jeroen Vreeken's avatar
Jeroen Vreeken committed
999
	{ "id",       CONTROLLER_BLOCK_TERM_UINT32, offsetof(struct controller_block_private, id)       },
1000
	{ NULL }
For faster browsing, not all history is shown. View entire blame