block_setpoint_generator_3d.c

/*
	Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2007, 2008, 2013, 2014
	Copyright Stichting C.A. Muller Radioastronomiestation, 2007, 2008, 2013

	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/>.

 */
#define _ISOC99_SOURCE

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <float.h>

#include <controller/controller_block.h>
#include <controller/controller_command.h>
#include <controller/controller_time.h>
#include <controller/controller_sample.h>
#include <log/log.h>

/*
       inputs     outputs
       nr name    nr name

     -------------------------------
     |                             |
 ----| 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               |
     |                             |
     -------------------------------

	'setpoint' is the setpoint as the user requested it.
	'x' is the output of the setpoint generator.
	'v', 'a' and 'j' are the first, second and third order derivatives of 'x'.

	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.
 */

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

struct controller_block_private {
	/*
		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;
	bool  *reset;
	float *track_x;
	float *track_v;
	bool  *track_x_cmd;
	bool  *track_v_cmd;
	
	/* 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 */
	float freq2;
	float freq3;
	float tick;
	double t_max_a;
	double v_delta_from_max_a;
	controller_trigger_time period;

	/* 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;

	/* state at last change */
	double start_x;
	double start_v;
	double start_a;
	double start_j;
	int start_t;

	/* 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;
	
	uint32_t id;
	
	bool softreset;
	
	struct controller_command_entry cur_command;
	bool cur_done;
	bool cur_start;
	uint64_t command_t;

	struct controller_command *command;
};


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:
		First halve (untill half the speed difference is reached) is 
		done at max jerk, second half is done at -max jerk.
		
		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;
}

static void setpoint_generator_3d_calculate(struct controller_block *spg)
{
	struct controller_block_private *priv = spg->private;
	double cur_x, cur_v;
	double t_max_a;
	bool ignore_x = false;
	bool must_brake = false;
	bool good_x;
	bool good_v;

	cur_x = priv->cur_x;
	cur_v = priv->cur_v;

	t_max_a = priv->t_max_a;

	if (*priv->reset) {
		struct controller_command_entry entry;
	
		priv->cmd_x = *priv->reset_x;
		cur_x = priv->cmd_x;
		priv->cur_done = true;
		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;
		}
		controller_command_queue_read(priv->command, &entry);
	}
	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;
	}

	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;
			priv->id = priv->cur_command.id;
		} else {
			if (priv->cur_command.type ==
			    COMMAND_PTYPE_SETPOINT_TIME) {
				priv->cmd_v = 0.0;
				priv->cmd_x = priv->cur_command.value.f;
			}
			priv->id = COMMAND_ID_NONE;
		}
	}
	if (!priv->cur_done) {
		switch (priv->cur_command.type) {
			case COMMAND_PTYPE_SETPOINT:
				priv->cmd_x = priv->cur_command.value.f;
				priv->cmd_v = 0.0;
				priv->cur_done = true;
				break;
			case COMMAND_PTYPE_SETPOINT_TRACK:
				priv->cur_done = true;
				break;
			case COMMAND_PTYPE_SPEED:
				priv->cmd_v = priv->cur_command.value.f * controller_time_period_get(spg->time);
				priv->cur_done = true;
				break;
			case COMMAND_PTYPE_SPEED_TRACK:
				priv->cur_done = true;
				break;
			case COMMAND_PTYPE_SETPOINT_TIME:
				if (!priv->cur_start) {
					controller_trigger_time command_t;
					int64_t t;
					
					command_t = priv->cur_command.t;
					
					t = (command_t - controller_sample_timestamp) / priv->period;

					if (t < 0) {
						/* Command is to old, pretend it
						   is a setpoint without time */
						log_send(LOG_T_WARNING, "%s: setpoint's time is in the past, clock skew? (%lld < %lld)",
						    spg->name, (long long)priv->cur_command.t, (long long)controller_sample_timestamp);
						priv->cmd_x = priv->cur_command.value.f;
						priv->cmd_v = 0.0;
						priv->cur_done = true;
					} else {
						priv->cmd_v =
						    (priv->cur_command.value.f -
						    priv->cmd_x) / (t+1.0);
						priv->cur_start = true;
					}
				}
				
				if (priv->cur_command.t <= controller_sample_timestamp) {
					priv->cur_done = true;
					priv->cmd_x =
					   priv->cur_command.value.f -
					   priv->cmd_v;
				}
				break;
			default:
				break;
		}
	}

	if (priv->cur_command.type == COMMAND_PTYPE_SPEED) {
		ignore_x = true;
		priv->cmd_x = cur_x + priv->cmd_v;
	}
	if (priv->cur_command.type == COMMAND_PTYPE_SPEED_TRACK) {
		float track_v = *priv->track_v * priv->tick;
		ignore_x = true;
		priv->cmd_v = track_v;
		priv->cmd_x = cur_x + track_v * controller_time_period_get(spg->time);
	}
	if (priv->cur_command.type == COMMAND_PTYPE_SETPOINT_TRACK) {
		priv->cmd_x = *priv->track_x;
		priv->cmd_v = 0.0;
	}
	
	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;
	
	good_x = almost_equal(cur_x, priv->cmd_x);
	good_v = almost_equal(cur_v, priv->cmd_v);

	if (!good_v || !good_x) {
		double error_x;
		double req_x_1;
		double error_x_jpos;
		double error_x_j0;
		double error_x_jneg;
		double error_v = priv->cmd_v - cur_v;
		double x, v, a, j, t;
		double req_x, req_v, v_delta_from_max_a;
		double error_v_after_a, error_x_at_v;
		double j_from_pos;
		bool state_at_max_a = false;
		bool state_to_max_a = false;
		
		/* procedure: where would we end up if we go to
		   requested speed. 
		 */
		
		x = cur_x;
		v = cur_v;
		a = priv->cur_a;
		req_x = priv->cmd_x;
		req_v = priv->cmd_v;
		
		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;
		}
		
		v_delta_from_max_a = priv->v_delta_from_max_a;

		j = copysign(priv->max_j, error_v);
			
		if (fabs(a) >= priv->max_j) {
			/* Not at constant velocity */
		
			if (signbit(a) != signbit(error_v)) {
				/* We are not accelerating towards cmd speed.
				   Go to constant velocity first
				 */
			
				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;

				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);
				
				} else if (fabs(a_peak) > priv->max_a) {
					/* We are going to hit maximum acc */
					a_peak = copysign(priv->max_a, a_peak);
					
					t = ticks_to_a(priv, 0, a_peak);
					v_start_max_a = v_after_ticks(priv, v_started_a, 0, j, t);
					
					v_end_max_a = req_v - copysign(v_delta_from_max_a, error_v);
					
					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;
				} else {
					t_to_max_a = t_a_to_0 - t_a;
					
					if (t_to_max_a >= 1.0) {
						state_to_max_a = true;
						
						/* accelerate to max a */
						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;
					}
				}
				/* decellerate to a==0 */
				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;
			}
		} else {
			state_to_max_a = true;
		}

		/* 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;
			
			x = x_after_ticks(priv, x, v, 0, j, t_max_a);
			v = v_after_ticks(priv, v, 0, j, t_max_a);
			a = copysign(priv->max_a, j);
			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 
			 
			       /\
			 */
			
			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;
		
		
		if (fabs(error_x_at_v) < fabs(error_x) || 
		    (signbit(error_x) != signbit(error_x_at_v) /*&& !state_to_max_a*/ && !state_at_max_a) ||
		    ignore_x) {
			if (!ignore_x &&
			    signbit(error_x_at_v) == signbit(error_x) ) {
				priv->cur_j = j_from_pos;
			} 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;
				}
			}
		} else {
			/* going to requested speed would make position error
			   bigger, first go to position.
			 */
			priv->cur_j = j_from_pos;
		}
	}
	
	
	/* When moving can we brake before the position limits? */
	if (fabs(cur_v) > 0.0) {
		double t, x, v, a, j;
		bool done = false;
		
		x = cur_x;
		v = cur_v;
		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);

					x = x_after_ticks(priv, x, v, a, -j, t);
					v = v_after_ticks(priv, v, a, -j, t);
					a = 0;

					/* Now at constant v */
				}
			}
		}

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

			t = ticks_to_v(priv, cur_v, 0);
		
			/* will we hit max a? */
			if (fabs(a_after_ticks(priv, a, j, t/2)) > priv->max_a) {
				/* 3-part a profile: /-\ */
			
				double v_start_3, t_2;
			
				t = t_max_a;
			
				x = x_after_ticks(priv, x, v, a, j, t);
				v = v_after_ticks(priv, v, a, j, t);
				
				/* 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: /\ */

				/* first half */
				x = x_after_ticks(priv, x, v, a, j, t/2);
				v = v_after_ticks(priv, v, a, j, t/2);
		
				/* second half, swap j to reduce a and v to 0 */
				x = x_after_ticks(priv, x, v, a, -j, t/2);
			}
		
			/* 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;
				must_brake = true;
			}
		}
	}

	/* If accelerating, can we decelerate before going beyond our max vel */
	if (fabs(priv->cur_a) > 0.0) {
		double t, v, a, j;
		
		v = cur_v;
		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);
		
		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;
			}
			must_brake = true;
		}
	}


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

	/* Is the difference between spg and command small enough?
	   If so, make outputs equal to command */
	if (!must_brake &&
	    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) {
		priv->cur_j = 0.0;
		priv->cur_a = 0.0;
		cur_v = priv->cmd_v;
		cur_x = priv->cmd_x;
	}

	/* new jerk? */
	if (!almost_equal(priv->cur_j, priv->start_j)) {
		priv->start_j = priv->cur_j;
		priv->start_a = priv->cur_a;
		priv->start_v = cur_v;
		priv->start_x = cur_x;
		priv->start_t = 0;
	}
	priv->start_t++;
	
	priv->cur_a = a_after_ticks(priv, 
	    priv->start_a, priv->start_j, priv->start_t);
	cur_v = v_after_ticks(priv, 
	    priv->start_v, priv->start_a, priv->start_j, priv->start_t);
	cur_x = x_after_ticks(priv, 
	    priv->start_x, priv->start_v, priv->start_a, priv->start_j, 
	    priv->start_t);


	if (fabs(priv->cur_a) > priv->max_a) {
		priv->cur_a = copysign(priv->max_a, priv->cur_a);
		priv->cur_j = 0;

		priv->start_j = priv->cur_j;
		priv->start_a = priv->cur_a;
		priv->start_v = cur_v;
		priv->start_x = cur_x;
		priv->start_t = 0;
	}

	if (fabs(cur_v) >= priv->max_v) {
		/* prevent further acceleration beyond max v */
		if (signbit(cur_v) == signbit(priv->cur_a)) {
			priv->cur_a = 0.0;
		}
		if (signbit(cur_v) == signbit(priv->cur_j)) {
			priv->cur_j = 0.0;
		}
		cur_v = copysign(priv->max_v, cur_v);

		priv->start_j = priv->cur_j;
		priv->start_a = priv->cur_a;
		priv->start_v = cur_v;
		priv->start_x = cur_x;
		priv->start_t = 0;
	}
	
	if (cur_x > priv->max_x) {
		cur_x = priv->max_x;
		cur_v = 0;
		priv->cur_a = 0;
		priv->cur_j = 0;

		priv->start_j = priv->cur_j;
		priv->start_a = priv->cur_a;
		priv->start_v = cur_v;
		priv->start_x = cur_x;
		priv->start_t = 0;
	}
	if (cur_x < priv->min_x) {
		cur_x = priv->min_x;
		cur_v = 0;
		priv->cur_a = 0;
		priv->cur_j = 0;

		priv->start_j = priv->cur_j;
		priv->start_a = priv->cur_a;
		priv->start_v = cur_v;
		priv->start_x = cur_x;
		priv->start_t = 0;
	}

set_output:
	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;
	priv->cmd_x_out = priv->cmd_x;
	priv->cur_x = cur_x;
	priv->cur_v = cur_v;
}

static int block_setpoint_generator_command_filter(struct controller_command *command,
    struct command_entry  *entry)
{
	struct controller_block_private *priv = command->block->private;
	int r = 0;
	
	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;
		}
		case COMMAND_PTYPE_SETPOINT_TRACK:
		case COMMAND_PTYPE_SPEED_TRACK:
			break;
		default:
			r = -1;
			break;
	}

	return r;
}

static void scale(struct controller_block *spg)
{
	double t_max_a;
	double tick = controller_time_period_get(spg->time);
	struct timespec ts;
	
	/* Scale all settings to sample time unit */
	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;
	spg->private->precision_x = spg->private->precision_x_sec;
	spg->private->precision_v = spg->private->precision_v_sec * tick;
	spg->private->precision_a = spg->private->precision_a_sec * tick * tick;

	spg->private->tick = tick;
	spg->private->freq = 1.0 / tick;
	spg->private->freq2 = spg->private->freq * spg->private->freq;
	spg->private->freq3 = spg->private->freq2 * spg->private->freq;
	spg->private->inv_max_j = 1.0 / spg->private->max_j;
	spg->private->inv_max_a = 1.0 / spg->private->max_a;
	ts.tv_sec = tick;
	ts.tv_nsec = (uint64_t)(tick * 1000000000) % 1000000000;
	spg->private->period = controller_sample_trigger->timespec2timestamp(&ts);

	/* 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;
}

static int param_set_setpoint(struct controller_block *spg, char *param,
    int argc, va_list val)
{
	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;
}

static int param_set_max_x(struct controller_block *spg, char *param,
    int argc, va_list val)
{
	spg->private->max_x = va_arg(val, double);
	scale(spg);
	return 0;
}

static int param_set_min_x(struct controller_block *spg, char *param,
    int argc, va_list val)
{
	spg->private->min_x = va_arg(val, double);
	scale(spg);
	return 0;
}

static int param_set_max_v(struct controller_block *spg, char *param,
    int argc, va_list val)
{
	spg->private->max_v_sec = va_arg(val, double);
	scale(spg);
	return 0;
}

static int param_set_max_a(struct controller_block *spg, char *param,
    int argc, va_list val)
{
	spg->private->max_a_sec = va_arg(val, double);
	scale(spg);
	return 0;
}

static int param_set_max_j(struct controller_block *spg, char *param,
    int argc, va_list val)
{
	spg->private->max_j_sec = va_arg(val, double);
	scale(spg);
	return 0;
}

static int param_set_precision_x(struct controller_block *spg, char *param,
    int argc, va_list val)
{
	spg->private->precision_x_sec = va_arg(val, double);
	scale(spg);
	return 0;
}

static int param_set_precision_v(struct controller_block *spg, char *param,
    int argc, va_list val)
{
	spg->private->precision_v_sec = va_arg(val, double);
	scale(spg);
	return 0;
}

static int param_set_precision_a(struct controller_block *spg, char *param,
    int argc, va_list val)
{
	spg->private->precision_a_sec = va_arg(val, double);
	scale(spg);
	return 0;
}

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;
}


static struct controller_block_param_list params[] = {
	{ "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 } },
	{ "softreset",   false, param_set_softreset, .args = { "int", NULL } },
	{ NULL },
};


static struct controller_block_interm_list interms[] = {
	{ "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) },
	{ 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) },
	{ "id",       CONTROLLER_BLOCK_TERM_UINT32, offsetof(struct controller_block_private, id)       },
	{ NULL }