FlightTask - Move ekf reset counter monitoring logic in the base Flih…

…tTask

Each child FlightTask can simply implement what it wants to do in case
of an EKF reset event by overriding one of the _ekfResetHandler functions

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

@@ -56,7 +56,6 @@ bool FlightTaskAutoLineSmoothVel::activate(vehicle_local_position_setpoint_s las
 
 	_yaw_sp_prev = last_setpoint.yaw;
 	_updateTrajConstraints();
-	_initEkfResetCounters();
 
 	return ret;
 }
@@ -69,7 +68,6 @@ void FlightTaskAutoLineSmoothVel::reActivate()
 	}
 
 	_trajectory[2].reset(0.f, 0.7f, _position(2));
-	_initEkfResetCounters();
 }
 
 void FlightTaskAutoLineSmoothVel::checkSetpoints(vehicle_local_position_setpoint_s &setpoints)
@@ -98,6 +96,38 @@ void FlightTaskAutoLineSmoothVel::checkSetpoints(vehicle_local_position_setpoint
 	if (!PX4_ISFINITE(setpoints.yaw)) { setpoints.yaw = _yaw; }
 }
 
+/**
+ * EKF reset handling functions
+ * Those functions are called by the base FlightTask in
+ * case of an EKF reset event
+ */
+void FlightTaskAutoLineSmoothVel::_ekfResetHandlerPositionXY()
+{
+	_trajectory[0].setCurrentPosition(_position(0));
+	_trajectory[1].setCurrentPosition(_position(1));
+}
+
+void FlightTaskAutoLineSmoothVel::_ekfResetHandlerVelocityXY()
+{
+	_trajectory[0].setCurrentVelocity(_velocity(0));
+	_trajectory[1].setCurrentVelocity(_velocity(1));
+}
+
+void FlightTaskAutoLineSmoothVel::_ekfResetHandlerPositionZ()
+{
+	_trajectory[2].setCurrentPosition(_position(2));
+}
+
+void FlightTaskAutoLineSmoothVel::_ekfResetHandlerVelocityZ()
+{
+	_trajectory[2].setCurrentVelocity(_velocity(2));
+}
+
+void FlightTaskAutoLineSmoothVel::_ekfResetHandlerHeading(float delta_psi)
+{
+	_yaw_sp_prev += delta_psi;
+}
+
 void FlightTaskAutoLineSmoothVel::_generateSetpoints()
 {
 	_prepareSetpoints();
@@ -151,40 +181,6 @@ float FlightTaskAutoLineSmoothVel::_constrainAbs(float val, float min, float max
 	return math::sign(val) * math::constrain(fabsf(val), fabsf(min), fabsf(max));
 }
 
-void FlightTaskAutoLineSmoothVel::_initEkfResetCounters()
-{
-	_reset_counters.xy = _sub_vehicle_local_position->get().xy_reset_counter;
-	_reset_counters.vxy = _sub_vehicle_local_position->get().vxy_reset_counter;
-	_reset_counters.z = _sub_vehicle_local_position->get().z_reset_counter;
-	_reset_counters.vz = _sub_vehicle_local_position->get().vz_reset_counter;
-}
-
-void FlightTaskAutoLineSmoothVel::_checkEkfResetCounters()
-{
-	// Check if a reset event has happened.
-	if (_sub_vehicle_local_position->get().xy_reset_counter != _reset_counters.xy) {
-		_trajectory[0].setCurrentPosition(_position(0));
-		_trajectory[1].setCurrentPosition(_position(1));
-		_reset_counters.xy = _sub_vehicle_local_position->get().xy_reset_counter;
-	}
-
-	if (_sub_vehicle_local_position->get().vxy_reset_counter != _reset_counters.vxy) {
-		_trajectory[0].setCurrentVelocity(_velocity(0));
-		_trajectory[1].setCurrentVelocity(_velocity(1));
-		_reset_counters.vxy = _sub_vehicle_local_position->get().vxy_reset_counter;
-	}
-
-	if (_sub_vehicle_local_position->get().z_reset_counter != _reset_counters.z) {
-		_trajectory[2].setCurrentPosition(_position(2));
-		_reset_counters.z = _sub_vehicle_local_position->get().z_reset_counter;
-	}
-
-	if (_sub_vehicle_local_position->get().vz_reset_counter != _reset_counters.vz) {
-		_trajectory[2].setCurrentVelocity(_velocity(2));
-		_reset_counters.vz = _sub_vehicle_local_position->get().vz_reset_counter;
-	}
-}
-
 float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget()
 {
 	// Compute the maximum allowed speed at the waypoint assuming that we want to
@@ -239,7 +235,6 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
 	// that one is used as a velocity limit.
 	// If the position setpoints are set to NAN, the values in the velocity setpoints are used as velocity targets: nothing to do here.
 
-	_checkEkfResetCounters();
 	_want_takeoff = false;
 
 	if (_param_mpc_yaw_mode.get() == 4 && !_yaw_sp_aligned) {

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

@@ -71,12 +71,16 @@ class FlightTaskAutoLineSmoothVel : public FlightTaskAutoMapper2
 
 	/** determines when to trigger a takeoff (ignored in flight) */
 	bool _checkTakeoff() override { return _want_takeoff; };
+	/** Reset position or velocity setpoints in case of EKF reset event */
+	void _ekfResetHandlerPositionXY() override;
+	void _ekfResetHandlerVelocityXY() override;
+	void _ekfResetHandlerPositionZ() override;
+	void _ekfResetHandlerVelocityZ() override;
+	void _ekfResetHandlerHeading(float delta_psi) override;
 
 	inline float _constrainOneSide(float val, float constraint); /**< Constrain val between INF and constraint */
 	inline float _constrainAbs(float val, float min, float max); /**< Constrain absolute value of val between min and max */
 
-	void _initEkfResetCounters();
-	void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */
 	void _generateHeading();
 	bool _generateHeadingAlongTraj(); /**< Generates heading along trajectory. */
 	void _updateTrajConstraints();
@@ -86,11 +90,4 @@ class FlightTaskAutoLineSmoothVel : public FlightTaskAutoMapper2
 
 	bool _want_takeoff{false};
 
-	/* counters for estimator local position resets */
-	struct {
-		uint8_t xy;
-		uint8_t vxy;
-		uint8_t z;
-		uint8_t vz;
-	} _reset_counters{0, 0, 0, 0};
 };

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

@@ -26,8 +26,8 @@ bool FlightTask::activate(vehicle_local_position_setpoint_s last_setpoint)
 {
 	_resetSetpoints();
 	_setDefaultConstraints();
+	_initEkfResetCounters();
 	_time_stamp_activate = hrt_absolute_time();
-	_heading_reset_counter = _sub_attitude->get().quat_reset_counter;
 	_gear = empty_landing_gear_default_keep;
 	return true;
 }
@@ -44,9 +44,49 @@ bool FlightTask::updateInitialize()
 	_deltatime  = math::min((_time_stamp_current - _time_stamp_last), _timeout) / 1e6f;
 	_time_stamp_last = _time_stamp_current;
 	_evaluateVehicleLocalPosition();
+	_checkEkfResetCounters();
 	return true;
 }
 
+void FlightTask::_initEkfResetCounters()
+{
+	_reset_counters.xy = _sub_vehicle_local_position->get().xy_reset_counter;
+	_reset_counters.vxy = _sub_vehicle_local_position->get().vxy_reset_counter;
+	_reset_counters.z = _sub_vehicle_local_position->get().z_reset_counter;
+	_reset_counters.vz = _sub_vehicle_local_position->get().vz_reset_counter;
+	_reset_counters.quat = _sub_attitude->get().quat_reset_counter;
+}
+
+void FlightTask::_checkEkfResetCounters()
+{
+	// Check if a reset event has happened
+	if (_sub_vehicle_local_position->get().xy_reset_counter != _reset_counters.xy) {
+		_ekfResetHandlerPositionXY();
+		_reset_counters.xy = _sub_vehicle_local_position->get().xy_reset_counter;
+	}
+
+	if (_sub_vehicle_local_position->get().vxy_reset_counter != _reset_counters.vxy) {
+		_ekfResetHandlerVelocityXY();
+		_reset_counters.vxy = _sub_vehicle_local_position->get().vxy_reset_counter;
+	}
+
+	if (_sub_vehicle_local_position->get().z_reset_counter != _reset_counters.z) {
+		_ekfResetHandlerPositionZ();
+		_reset_counters.z = _sub_vehicle_local_position->get().z_reset_counter;
+	}
+
+	if (_sub_vehicle_local_position->get().vz_reset_counter != _reset_counters.vz) {
+		_ekfResetHandlerVelocityZ();
+		_reset_counters.vz = _sub_vehicle_local_position->get().vz_reset_counter;
+	}
+
+	if (_sub_attitude->get().quat_reset_counter != _reset_counters.quat) {
+		float delta_psi = matrix::Eulerf(matrix::Quatf(_sub_attitude->get().delta_q_reset)).psi();
+		_ekfResetHandlerHeading(delta_psi);
+		_reset_counters.quat = _sub_attitude->get().quat_reset_counter;
+	}
+}
+
 const vehicle_local_position_setpoint_s FlightTask::getPositionSetpoint()
 {
 	/* fill position setpoint message */

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

@@ -178,7 +178,6 @@ class FlightTask : public ModuleParams
 
 	uORB::SubscriptionPollable<vehicle_local_position_s> *_sub_vehicle_local_position{nullptr};
 	uORB::SubscriptionPollable<vehicle_attitude_s> *_sub_attitude{nullptr};
-	uint8_t _heading_reset_counter{0}; /**< estimator heading reset */
 
 	/** Reset all setpoints to NAN */
 	void _resetSetpoints();
@@ -189,9 +188,21 @@ class FlightTask : public ModuleParams
 	/** Set constraints to default values */
 	virtual void _setDefaultConstraints();
 
-	/** determines when to trigger a takeoff (ignored in flight) */
+	/** Determine when to trigger a takeoff (ignored in flight) */
 	virtual bool _checkTakeoff();
 
+	/**
+	 * Monitor the EKF reset counters and
+	 * call the appropriate handling functions in case of a reset event
+	 */
+	void _initEkfResetCounters();
+	void _checkEkfResetCounters();
+	virtual void _ekfResetHandlerPositionXY() {};
+	virtual void _ekfResetHandlerVelocityXY() {};
+	virtual void _ekfResetHandlerPositionZ() {};
+	virtual void _ekfResetHandlerVelocityZ() {};
+	virtual void _ekfResetHandlerHeading(float delta_psi) {};
+
 	/* Time abstraction */
 	static constexpr uint64_t _timeout = 500000; /**< maximal time in us before a loop or data times out */
 	float _time = 0; /**< passed time in seconds since the task was activated */
@@ -225,6 +236,15 @@ class FlightTask : public ModuleParams
 	matrix::Vector3f _velocity_setpoint_feedback;
 	matrix::Vector3f _thrust_setpoint_feedback;
 
+	/* Counters for estimator local position resets */
+	struct {
+		uint8_t xy;
+		uint8_t vxy;
+		uint8_t z;
+		uint8_t vz;
+		uint8_t quat;
+	} _reset_counters{0, 0, 0, 0, 0};
+
 	/**
 	 * Vehicle constraints.
 	 * The constraints can vary with tasks.

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

@@ -310,17 +310,18 @@ void FlightTaskManualAltitude::_updateHeadingSetpoints()
 		// hold the current heading when no more rotation commanded
 		if (!PX4_ISFINITE(_yaw_setpoint)) {
 			_yaw_setpoint = _yaw;
-
-		} else {
-			// check reset counter and update yaw setpoint if necessary
-			if (_sub_attitude->get().quat_reset_counter != _heading_reset_counter) {
-				_yaw_setpoint += matrix::Eulerf(matrix::Quatf(_sub_attitude->get().delta_q_reset)).psi();
-				_heading_reset_counter = _sub_attitude->get().quat_reset_counter;
-			}
 		}
 	}
 }
 
+void FlightTaskManualAltitude::_ekfResetHandlerHeading(float delta_psi)
+{
+	// Only reset the yaw setpoint when the heading is locked
+	if (PX4_ISFINITE(_yaw_setpoint)) {
+		_yaw_setpoint += delta_psi;
+	}
+}
+
 void FlightTaskManualAltitude::_updateSetpoints()
 {
 	_updateHeadingSetpoints(); // get yaw setpoint

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

@@ -54,6 +54,7 @@ class FlightTaskManualAltitude : public FlightTaskManual
 
 protected:
 	void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */
+	void _ekfResetHandlerHeading(float delta_psi) override; /**< adjust heading setpoint in case of EKF reset event */
 	virtual void _updateSetpoints(); /**< updates all setpoints */
 	virtual void _scaleSticks(); /**< scales sticks to velocity in z */
 	bool _checkTakeoff() override;

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

@@ -50,8 +50,6 @@ bool FlightTaskManualAltitudeSmoothVel::activate(vehicle_local_position_setpoint
 
 	_smoothing.reset(last_setpoint.acc_z, last_setpoint.vz, last_setpoint.z);
 
-	_initEkfResetCounters();
-
 	return ret;
 }
 
@@ -60,8 +58,6 @@ 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
 	_smoothing.reset(0.f, 0.f, _position(2));
-
-	_initEkfResetCounters();
 }
 
 void FlightTaskManualAltitudeSmoothVel::checkSetpoints(vehicle_local_position_setpoint_s &setpoints)
@@ -76,17 +72,18 @@ void FlightTaskManualAltitudeSmoothVel::checkSetpoints(vehicle_local_position_se
 	if (!PX4_ISFINITE(setpoints.acc_z)) { setpoints.acc_z = 0.f; }
 }
 
-void FlightTaskManualAltitudeSmoothVel::_initEkfResetCounters()
+void FlightTaskManualAltitudeSmoothVel::_ekfResetHandlerPositionZ()
 {
-	_reset_counters.z = _sub_vehicle_local_position->get().z_reset_counter;
-	_reset_counters.vz = _sub_vehicle_local_position->get().vz_reset_counter;
+	_smoothing.setCurrentPosition(_position(2));
 }
 
-void FlightTaskManualAltitudeSmoothVel::_updateSetpoints()
+void FlightTaskManualAltitudeSmoothVel::_ekfResetHandlerVelocityZ()
 {
-	// Reset trajectories if EKF did a reset
-	_checkEkfResetCounters();
+	_smoothing.setCurrentVelocity(_velocity(2));
+}
 
+void FlightTaskManualAltitudeSmoothVel::_updateSetpoints()
+{
 	// Set max accel/vel/jerk
 	// Has to be done before _updateTrajectories()
 	_updateTrajConstraints();
@@ -104,19 +101,6 @@ void FlightTaskManualAltitudeSmoothVel::_updateSetpoints()
 	_setOutputState();
 }
 
-void FlightTaskManualAltitudeSmoothVel::_checkEkfResetCounters()
-{
-	if (_sub_vehicle_local_position->get().z_reset_counter != _reset_counters.z) {
-		_smoothing.setCurrentPosition(_position(2));
-		_reset_counters.z = _sub_vehicle_local_position->get().z_reset_counter;
-	}
-
-	if (_sub_vehicle_local_position->get().vz_reset_counter != _reset_counters.vz) {
-		_smoothing.setCurrentVelocity(_velocity(2));
-		_reset_counters.vz = _sub_vehicle_local_position->get().vz_reset_counter;
-	}
-}
-
 void FlightTaskManualAltitudeSmoothVel::_updateTrajConstraints()
 {
 	_smoothing.setMaxJerk(_param_mpc_jerk_max.get());

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

@@ -55,6 +55,10 @@ class FlightTaskManualAltitudeSmoothVel : public FlightTaskManualAltitude
 
 	virtual void _updateSetpoints() override;
 
+	/** Reset position or velocity setpoints in case of EKF reset event */
+	void _ekfResetHandlerPositionZ() override;
+	void _ekfResetHandlerVelocityZ() override;
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max,
 		(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
@@ -65,16 +69,8 @@ class FlightTaskManualAltitudeSmoothVel : public FlightTaskManualAltitude
 
 	void checkSetpoints(vehicle_local_position_setpoint_s &setpoints);
 
-	void _initEkfResetCounters();
-	void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */
 	void _updateTrajConstraints();
 	void _setOutputState();
 
 	ManualVelocitySmoothingZ _smoothing; ///< Smoothing in z direction
-
-	/* counters for estimator local position resets */
-	struct {
-		uint8_t z;
-		uint8_t vz;
-	} _reset_counters{0, 0};
 };