Use the correct time when staring execution of a timed command

Do not override a brake.

controller/controller/block_setpoint_generator.c

@@ -207,6 +207,7 @@ static void calculate(struct controller_block *spg)
 {
 	struct controller_block_private *priv = spg->private;
 	bool ignore_x = false;
+	bool must_brake = false;
 
 	if (*priv->reset) {
 		priv->cmd_x = *priv->reset_x;
@@ -251,7 +252,7 @@ static void calculate(struct controller_block *spg)
 				if (!priv->current_command.start) {
 					t = (double)(priv->current_command.time
 					    -controller_time_seconds) * priv->freq -
-					    -(double)controller_time_samplenr;
+					    (double)controller_time_samplenr;
 					if (t < 1.0 ||
 					    priv->current_command.time <
 					    controller_time_seconds) {
@@ -579,6 +580,7 @@ static void calculate(struct controller_block *spg)
 			/* 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;
 			}
 		}
 	}
@@ -609,19 +611,20 @@ static void calculate(struct controller_block *spg)
 			} else {
 				priv->cur_j = j;
 			}
+			must_brake = true;
 		}
 	}
 
 	/* Is the difference between spg and command small enough?
 	   If so, make outputs equal to command */
-	if (fabs(priv->cmd_x - priv->cur_x) < priv->precision_x) {
-		if (fabs(priv->cmd_v - priv->cur_v) < priv->precision_v) {
-			if (fabs(priv->cur_a) < priv->precision_a)
-				priv->cur_j = 0.0;
-				priv->cur_a = 0.0;
-				priv->cur_v = priv->cmd_v;
-				priv->cur_x = priv->cmd_x;
-		}
+	if (!must_brake &&
+	    fabs(priv->cmd_x - priv->cur_x) < priv->precision_x &&
+	    fabs(priv->cmd_v - priv->cur_v) < priv->precision_v &&
+	    fabs(priv->cur_a) < priv->precision_a) {
+		priv->cur_j = 0.0;
+		priv->cur_a = 0.0;
+		priv->cur_v = priv->cmd_v;
+		priv->cur_x = priv->cmd_x;
 	}
 
 	priv->cur_a += priv->cur_j;