FlightTask - When switching task, pass the last setpoints of the previous task to the new task

This is done to allow proper initialization of the new FlightTask and
give it a chance to continue the setpoints without discontinuity. The
function checkSetpoints replaces the setpoints containing NANs with an
estimate of the state. The estimate is usually the current estimate of
the EKF or zero.
The transition FlightTask also provides an estimate of the current
acceleration to properly initialize the next FlightTask after
back-transition. This avoid having to initialize the accelerations to
zero knowing that the actual acceleration is usually far from zero.

src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp

@@ -41,11 +41,16 @@
 
 using namespace matrix;
 
-bool FlightTaskManualAltitudeSmoothVel::activate()
+bool FlightTaskManualAltitudeSmoothVel::activate(vehicle_local_position_setpoint_s state_prev)
 {
-	bool ret = FlightTaskManualAltitude::activate();
+	bool ret = FlightTaskManualAltitude::activate(state_prev);
 
-	_reset();
+	// Check if the previous FlightTask provided setpoints
+	checkSetpoints(state_prev);
+
+	_smoothing.reset(state_prev.acc_z, state_prev.vz, state_prev.z);
+
+	_resetPositionLock();
 
 	return ret;
 }
@@ -54,20 +59,13 @@ void FlightTaskManualAltitudeSmoothVel::reActivate()
 {
 	// The task is reacivated while the vehicle is on the ground. To detect takeoff in mc_pos_control_main properly
 	// using the generated jerk, reset the z derivatives to zero
-	_reset(true);
+	_smoothing.reset(0.f, 0.f, _position(2));
+
+	_resetPositionLock();
 }
 
-void FlightTaskManualAltitudeSmoothVel::_reset(bool force_vz_zero)
+void FlightTaskManualAltitudeSmoothVel::_resetPositionLock()
 {
-	// Set the z derivatives to zero
-	if (force_vz_zero) {
-		_smoothing.reset(0.f, 0.f, _position(2));
-
-	} else {
-		// TODO: get current accel
-		_smoothing.reset(0.f, _velocity(2), _position(2));
-	}
-
 	// Always start unlocked
 	_position_lock_z_active = false;
 	_position_setpoint_z_locked = NAN;