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

@@ -64,6 +64,9 @@ int FlightTasks::switchTask(FlightTaskIndex new_task_index)
 		return 0;
 	}
 
+	// Save current setpoints for the nex FlightTask
+	vehicle_local_position_setpoint_s current_state = getPositionSetpoint();
+
 	if (_initTask(new_task_index)) {
 		// invalid task
 		return -1;
@@ -85,7 +88,7 @@ int FlightTasks::switchTask(FlightTaskIndex new_task_index)
 	_subscription_array.forcedUpdate(); // make sure data is available for all new subscriptions
 
 	// activation failed
-	if (!_current_task.task->updateInitialize() || !_current_task.task->activate()) {
+	if (!_current_task.task->updateInitialize() || !_current_task.task->activate(current_state)) {
 		_current_task.task->~FlightTask();
 		_current_task.task = nullptr;
 		_current_task.index = FlightTaskIndex::None;

src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp

@@ -77,9 +77,9 @@ bool FlightTaskAuto::initializeSubscriptions(SubscriptionArray &subscription_arr
 	return true;
 }
 
-bool FlightTaskAuto::activate()
+bool FlightTaskAuto::activate(vehicle_local_position_setpoint_s state_prev)
 {
-	bool ret = FlightTask::activate();
+	bool ret = FlightTask::activate(state_prev);
 	_position_setpoint = _position;
 	_velocity_setpoint = _velocity;
 	_yaw_setpoint = _yaw_sp_prev = _yaw;

src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp

@@ -78,7 +78,7 @@ public:
 
 	virtual ~FlightTaskAuto() = default;
 	bool initializeSubscriptions(SubscriptionArray &subscription_array) override;
-	bool activate() override;
+	bool activate(vehicle_local_position_setpoint_s state_prev) override;
 	bool updateInitialize() override;
 	bool updateFinalize() override;
 

src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp

@@ -41,12 +41,17 @@
 
 using namespace matrix;
 
-bool FlightTaskAutoLineSmoothVel::activate()
+bool FlightTaskAutoLineSmoothVel::activate(vehicle_local_position_setpoint_s state_prev)
 {
-	bool ret = FlightTaskAutoMapper2::activate();
+	bool ret = FlightTaskAutoMapper2::activate(state_prev);
+
+	checkSetpoints(state_prev);
+	const Vector3f accel_prev{state_prev.acc_x, state_prev.acc_y, state_prev.acc_z};
+	const Vector3f vel_prev = Vector3f(state_prev.vx, state_prev.vy, state_prev.vz);
+	const Vector3f pos_prev = Vector3f(state_prev.x, state_prev.y, state_prev.z);
 
 	for (int i = 0; i < 3; ++i) {
-		_trajectory[i].reset(0.f, _velocity(i), _position(i));
+		_trajectory[i].reset(accel_prev(i), vel_prev(i), pos_prev(i));
 	}
 
 	_yaw_sp_prev = _yaw;

src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp

@@ -49,7 +49,7 @@ public:
 	FlightTaskAutoLineSmoothVel() = default;
 	virtual ~FlightTaskAutoLineSmoothVel() = default;
 
-	bool activate() override;
+	bool activate(vehicle_local_position_setpoint_s state_prev) override;
 	void reActivate() override;
 
 protected:

src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp

@@ -40,9 +40,9 @@
 
 using namespace matrix;
 
-bool FlightTaskAutoMapper::activate()
+bool FlightTaskAutoMapper::activate(vehicle_local_position_setpoint_s state_prev)
 {
-	bool ret = FlightTaskAuto::activate();
+	bool ret = FlightTaskAuto::activate(state_prev);
 	_reset();
 	return ret;
 }

src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp

@@ -47,7 +47,7 @@ class FlightTaskAutoMapper : public FlightTaskAuto
 public:
 	FlightTaskAutoMapper() = default;
 	virtual ~FlightTaskAutoMapper() = default;
-	bool activate() override;
+	bool activate(vehicle_local_position_setpoint_s state_prev) override;
 	bool update() override;
 
 protected:

src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp

@@ -40,9 +40,9 @@
 
 using namespace matrix;
 
-bool FlightTaskAutoMapper2::activate()
+bool FlightTaskAutoMapper2::activate(vehicle_local_position_setpoint_s state_prev)
 {
-	bool ret = FlightTaskAuto::activate();
+	bool ret = FlightTaskAuto::activate(state_prev);
 	_reset();
 	return ret;
 }

src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp

@@ -47,7 +47,7 @@ class FlightTaskAutoMapper2 : public FlightTaskAuto
 public:
 	FlightTaskAutoMapper2() = default;
 	virtual ~FlightTaskAutoMapper2() = default;
-	bool activate() override;
+	bool activate(vehicle_local_position_setpoint_s state_prev) override;
 	bool update() override;
 
 protected:

src/lib/FlightTasks/tasks/Failsafe/FlightTaskFailsafe.cpp

@@ -36,9 +36,9 @@
 
 #include "FlightTaskFailsafe.hpp"
 
-bool FlightTaskFailsafe::activate()
+bool FlightTaskFailsafe::activate(vehicle_local_position_setpoint_s state_prev)
 {
-	bool ret = FlightTask::activate();
+	bool ret = FlightTask::activate(state_prev);
 	_position_setpoint = _position;
 	_velocity_setpoint.zero();
 	_thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, -_param_mpc_thr_hover.get() * 0.6f);

src/lib/FlightTasks/tasks/Failsafe/FlightTaskFailsafe.hpp

@@ -47,7 +47,7 @@ public:
 
 	virtual ~FlightTaskFailsafe() = default;
 	bool update() override;
-	bool activate() override;
+	bool activate(vehicle_local_position_setpoint_s state_prev) override;
 
 private:
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,

src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp

@@ -22,7 +22,7 @@ bool FlightTask::initializeSubscriptions(SubscriptionArray &subscription_array)
 	return true;
 }
 
-bool FlightTask::activate()
+bool FlightTask::activate(vehicle_local_position_setpoint_s state_prev)
 {
 	_resetSetpoints();
 	_setDefaultConstraints();
@@ -34,7 +34,7 @@ bool FlightTask::activate()
 
 void FlightTask::reActivate()
 {
-	activate();
+	activate(getPositionSetpoint());
 }
 
 bool FlightTask::updateInitialize()
@@ -47,6 +47,47 @@ bool FlightTask::updateInitialize()
 	return true;
 }
 
+void FlightTask::checkSetpoints(vehicle_local_position_setpoint_s &setpoints)
+{
+	// If the position setpoint is unknown, set to the current postion
+	if (!PX4_ISFINITE(setpoints.x)) { setpoints.x = _position(0); }
+
+	if (!PX4_ISFINITE(setpoints.y)) { setpoints.y = _position(1); }
+
+	if (!PX4_ISFINITE(setpoints.z)) { setpoints.z = _position(2); }
+
+	// If the velocity setpoint is unknown, set to the current velocity
+	if (!PX4_ISFINITE(setpoints.vx)) { setpoints.vx = _velocity(0); }
+
+	if (!PX4_ISFINITE(setpoints.vy)) { setpoints.vy = _velocity(1); }
+
+	if (!PX4_ISFINITE(setpoints.vz)) { setpoints.vz = _velocity(2); }
+
+	// No acceleration estimate available, set to zero if the setpoint is NAN
+	if (!PX4_ISFINITE(setpoints.acc_x)) { setpoints.acc_x = 0.f; }
+
+	if (!PX4_ISFINITE(setpoints.acc_y)) { setpoints.acc_y = 0.f; }
+
+	if (!PX4_ISFINITE(setpoints.acc_z)) { setpoints.acc_z = 0.f; }
+
+	// No jerk estimate available, set to zero if the setpoint is NAN
+	if (!PX4_ISFINITE(setpoints.jerk_x)) { setpoints.jerk_x = 0.f; }
+
+	if (!PX4_ISFINITE(setpoints.jerk_y)) { setpoints.jerk_y = 0.f; }
+
+	if (!PX4_ISFINITE(setpoints.jerk_z)) { setpoints.jerk_z = 0.f; }
+
+	if (!PX4_ISFINITE(setpoints.thrust[0])) { setpoints.thrust[0] = 0.f; }
+
+	if (!PX4_ISFINITE(setpoints.thrust[1])) { setpoints.thrust[1] = 0.f; }
+
+	if (!PX4_ISFINITE(setpoints.thrust[2])) { setpoints.thrust[2] = -0.5f; }
+
+	if (!PX4_ISFINITE(setpoints.yaw)) { setpoints.yaw = _yaw; }
+
+	if (!PX4_ISFINITE(setpoints.yawspeed)) { setpoints.yawspeed = 0.f; }
+}
+
 const vehicle_local_position_setpoint_s FlightTask::getPositionSetpoint()
 {
 	/* fill position setpoint message */

src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp

@@ -76,9 +76,10 @@ public:
 
 	/**
 	 * Call once on the event where you switch to the task
+	 * @param state of the previous task
 	 * @return true on success, false on error
 	 */
-	virtual bool activate();
+	virtual bool activate(vehicle_local_position_setpoint_s state_prev);
 
 	/**
 	 * Call this to reset an active Flight Task
@@ -119,6 +120,8 @@ public:
 	 */
 	const vehicle_local_position_setpoint_s getPositionSetpoint();
 
+	void checkSetpoints(vehicle_local_position_setpoint_s &setpoints);
+
 	/**
 	 * Get vehicle constraints.
 	 * The constraints can vary with task.

src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

@@ -61,9 +61,9 @@ bool FlightTaskManualAltitude::updateInitialize()
 	return ret && PX4_ISFINITE(_position(2)) && PX4_ISFINITE(_velocity(2)) && PX4_ISFINITE(_yaw);
 }
 
-bool FlightTaskManualAltitude::activate()
+bool FlightTaskManualAltitude::activate(vehicle_local_position_setpoint_s state_prev)
 {
-	bool ret = FlightTaskManual::activate();
+	bool ret = FlightTaskManual::activate(state_prev);
 	_yaw_setpoint = NAN;
 	_yawspeed_setpoint = 0.0f;
 	_thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, NAN); // altitude is controlled from position/velocity

src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp

@@ -48,7 +48,7 @@ public:
 	FlightTaskManualAltitude() = default;
 	virtual ~FlightTaskManualAltitude() = default;
 	bool initializeSubscriptions(SubscriptionArray &subscription_array) override;
-	bool activate() override;
+	bool activate(vehicle_local_position_setpoint_s state_prev) override;
 	bool updateInitialize() override;
 	bool update() override;
 

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;

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

@@ -48,7 +48,7 @@ public:
 	FlightTaskManualAltitudeSmoothVel() = default;
 	virtual ~FlightTaskManualAltitudeSmoothVel() = default;
 
-	bool activate() override;
+	bool activate(vehicle_local_position_setpoint_s state_prev) override;
 	void reActivate() override;
 
 protected:
@@ -63,10 +63,7 @@ protected:
 
 private:
 
-	/**
-	 * Reset the required axes. when force_z_zero is set to true, the z derivatives are set to sero and not to the estimated states
-	 */
-	void _reset(bool force_vz_zero = false);
+	void _resetPositionLock();
 	void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */
 
 	VelocitySmoothing _smoothing; ///< Smoothing in z direction

src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp

@@ -65,10 +65,10 @@ bool FlightTaskManualPosition::updateInitialize()
 	       && PX4_ISFINITE(_velocity(1));
 }
 
-bool FlightTaskManualPosition::activate()
+bool FlightTaskManualPosition::activate(vehicle_local_position_setpoint_s state_prev)
 {
 	// all requirements from altitude-mode still have to hold
-	bool ret = FlightTaskManualAltitude::activate();
+	bool ret = FlightTaskManualAltitude::activate(state_prev);
 
 	_constraints.tilt = math::radians(_param_mpc_tiltmax_air.get());
 

src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp

@@ -50,7 +50,7 @@ public:
 
 	virtual ~FlightTaskManualPosition() = default;
 	bool initializeSubscriptions(SubscriptionArray &subscription_array) override;
-	bool activate() override;
+	bool activate(vehicle_local_position_setpoint_s state_prev) override;
 	bool updateInitialize() override;
 
 	/**

src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp

@@ -38,11 +38,21 @@
 
 using namespace matrix;
 
-bool FlightTaskManualPositionSmoothVel::activate()
+bool FlightTaskManualPositionSmoothVel::activate(vehicle_local_position_setpoint_s state_prev)
 {
-	bool ret = FlightTaskManualPosition::activate();
+	bool ret = FlightTaskManualPosition::activate(state_prev);
 
-	reset(Axes::XYZ);
+	// Check if the previous FlightTask provided setpoints
+	checkSetpoints(last_setpoint);
+	const Vector3f accel_prev(last_setpoint.acc_x, last_setpoint.acc_y, last_setpoint.acc_z);
+	const Vector3f vel_prev(last_setpoint.vx, last_setpoint.vy, last_setpoint.vz);
+	const Vector3f pos_prev(last_setpoint.x, last_setpoint.y, last_setpoint.z);
+
+	for (int i = 0; i < 3; ++i) {
+		_smoothing[i].reset(accel_prev(i), vel_prev(i), pos_prev(i));
+	}
+
+	_resetPositionLock();
 
 	return ret;
 }
@@ -51,37 +61,18 @@ void FlightTaskManualPositionSmoothVel::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(Axes::XYZ, true);
-}
-
-void FlightTaskManualPositionSmoothVel::reset(Axes axes, bool force_z_zero)
-{
-	int count;
-
-	switch (axes) {
-	case Axes::XY:
-		count = 2;
-		break;
-
-	case Axes::XYZ:
-		count = 3;
-		break;
-
-	default:
-		count = 0;
-		break;
-	}
-
-	// TODO: get current accel
-	for (int i = 0; i < count; ++i) {
+	for (int i = 0; i < 2; ++i) {
 		_smoothing[i].reset(0.f, _velocity(i), _position(i));
 	}
 
-	// Set the z derivatives to zero
-	if (force_z_zero) {
-		_smoothing[2].reset(0.f, 0.f, _position(2));
-	}
+	_smoothing[2].reset(0.f, 0.f, _position(2));
 
+	_initEkfResetCounters();
+	_resetPositionLock();
+}
+
+void FlightTaskManualPositionSmoothVel::_resetPositionLock()
+{
 	_position_lock_xy_active = false;
 	_position_lock_z_active = false;
 	_position_setpoint_xy_locked(0) = NAN;

src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp

@@ -49,7 +49,7 @@ public:
 
 	virtual ~FlightTaskManualPositionSmoothVel() = default;
 
-	bool activate() override;
+	bool activate(vehicle_local_position_setpoint_s state_prev) override;
 	void reActivate() override;
 
 protected:
@@ -62,13 +62,7 @@ protected:
 					(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max
 				       )
 private:
-
-	enum class Axes {XY, XYZ};
-
-	/**
-	 * Reset the required axes. when force_z_zero is set to true, the z derivatives are set to sero and not to the estimated states
-	 */
-	void reset(Axes axes, bool force_z_zero = false);
+	void _resetPositionLock();
 	void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */
 
 	VelocitySmoothing _smoothing[3]; ///< Smoothing in x, y and z directions

src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.cpp

@@ -64,9 +64,9 @@ bool FlightTaskOffboard::updateInitialize()
 	       && PX4_ISFINITE(_velocity(1));
 }
 
-bool FlightTaskOffboard::activate()
+bool FlightTaskOffboard::activate(vehicle_local_position_setpoint_s state_prev)
 {
-	bool ret = FlightTask::activate();
+	bool ret = FlightTask::activate(state_prev);
 	_position_setpoint = _position;
 	_velocity_setpoint.setZero();
 	_position_lock.setAll(NAN);

src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.hpp

@@ -49,7 +49,7 @@ public:
 	virtual ~FlightTaskOffboard() = default;
 	bool initializeSubscriptions(SubscriptionArray &subscription_array) override;
 	bool update() override;
-	bool activate() override;
+	bool activate(vehicle_local_position_setpoint_s state_prev) override;
 	bool updateInitialize() override;
 
 protected:

src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp

@@ -147,9 +147,9 @@ bool FlightTaskOrbit::checkAcceleration(float r, float v, float a)
 	return v * v < a * r;
 }
 
-bool FlightTaskOrbit::activate()
+bool FlightTaskOrbit::activate(vehicle_local_position_setpoint_s state_prev)
 {
-	bool ret = FlightTaskManualAltitudeSmooth::activate();
+	bool ret = FlightTaskManualAltitudeSmooth::activate(state_prev);
 	_r = _radius_min;
 	_v =  1.f;
 	_center = Vector2f(_position);

src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.hpp

@@ -51,7 +51,7 @@ public:
 	virtual ~FlightTaskOrbit();
 
 	bool applyCommandParameters(const vehicle_command_s &command) override;
-	bool activate() override;
+	bool activate(vehicle_local_position_setpoint_s state_prev) override;
 	bool update() override;
 
 	/**

src/lib/FlightTasks/tasks/Sport/FlightTaskSport.hpp

@@ -52,9 +52,9 @@ public:
 
 	virtual ~FlightTaskSport() = default;
 
-	bool activate() override
+	bool activate(vehicle_local_position_setpoint_s state_prev) override
 	{
-		bool ret = FlightTaskManualPosition::activate();
+		bool ret = FlightTaskManualPosition::activate(state_prev);
 
 		// default constraints already are the maximum allowed limits
 		_setDefaultConstraints();

src/lib/FlightTasks/tasks/Transition/FlightTaskTransition.cpp

@@ -42,14 +42,29 @@ bool FlightTaskTransition::updateInitialize()
 	return FlightTask::updateInitialize();
 }
 
-bool FlightTaskTransition::activate()
+bool FlightTaskTransition::activate(vehicle_local_position_setpoint_s state_prev)
 {
-	// transition at the current altitude and current yaw at the time of activation
-	// it would be better to use the last setpoint from the previous running flighttask but that interface
-	// is not available
-	_transition_altitude = _position(2);
-	_transition_yaw = _yaw;
-	return FlightTask::activate();
+	checkSetpoints(state_prev);
+	_transition_altitude = state_prev.z;
+	_transition_yaw = state_prev.yaw;
+	_acceleration_setpoint.setAll(0.f);
+	_velocity_prev = _velocity;
+	return FlightTask::activate(state_prev);
+}
+
+void FlightTaskTransition::updateAccelerationEstimate()
+{
+	// Estimate the acceleration by filtering the raw derivative of the velocity estimate
+	// This is done to provide a good estimate of the current acceleration to the next flight task after back-transition
+	_acceleration_setpoint = 0.9f * _acceleration_setpoint + 0.1f * (_velocity - _velocity_prev) / _deltatime;
+
+	if (!PX4_ISFINITE(_acceleration_setpoint(0)) ||
+	    !PX4_ISFINITE(_acceleration_setpoint(1)) ||
+	    !PX4_ISFINITE(_acceleration_setpoint(2))) {
+		_acceleration_setpoint.setAll(0.f);
+	}
+
+	_velocity_prev = _velocity;
 }
 
 bool FlightTaskTransition::update()
@@ -61,6 +76,8 @@ bool FlightTaskTransition::update()
 	_velocity_setpoint *= NAN;
 	_position_setpoint(2) = _transition_altitude;
 
+	updateAccelerationEstimate();
+
 	_yaw_setpoint = _transition_yaw;
 	return true;
 }

src/lib/FlightTasks/tasks/Transition/FlightTaskTransition.hpp

@@ -47,11 +47,14 @@ public:
 	FlightTaskTransition() = default;
 
 	virtual ~FlightTaskTransition() = default;
-	bool activate() override;
+	bool activate(vehicle_local_position_setpoint_s state_prev) override;
 	bool updateInitialize() override;
 	bool update() override;
 
 private:
+	void updateAccelerationEstimate();
+
 	float _transition_altitude = 0.0f;
 	float _transition_yaw = 0.0f;
+	matrix::Vector3f _velocity_prev{};
 };