FlightTask smoothVel - Initialize ekf reset counters when task is activated

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

@@ -56,6 +56,7 @@ bool FlightTaskAutoLineSmoothVel::activate(vehicle_local_position_setpoint_s sta
 
 	_yaw_sp_prev = _yaw;
 	_updateTrajConstraints();
+	_initEkfResetCounters();
 
 	return ret;
 }
@@ -68,6 +69,7 @@ void FlightTaskAutoLineSmoothVel::reActivate()
 	}
 
 	_trajectory[2].reset(0.f, 0.7f, _position(2));
+	_initEkfResetCounters();
 }
 
 void FlightTaskAutoLineSmoothVel::_generateSetpoints()
@@ -115,6 +117,14 @@ inline float FlightTaskAutoLineSmoothVel::_constrainOneSide(float val, float con
 	return math::constrain(val, min, 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.

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

@@ -70,6 +70,7 @@ protected:
 	bool _checkTakeoff() override { return _want_takeoff; };
 
 	inline float _constrainOneSide(float val, float constrain);
+	void _initEkfResetCounters();
 	void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */
 	void _generateHeading();
 	bool _generateHeadingAlongTraj(); /**< Generates heading along trajectory. */

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

@@ -50,6 +50,7 @@ bool FlightTaskManualAltitudeSmoothVel::activate(vehicle_local_position_setpoint
 
 	_smoothing.reset(state_prev.acc_z, state_prev.vz, state_prev.z);
 
+	_initEkfResetCounters();
 	_resetPositionLock();
 
 	return ret;
@@ -61,6 +62,7 @@ void FlightTaskManualAltitudeSmoothVel::reActivate()
 	// using the generated jerk, reset the z derivatives to zero
 	_smoothing.reset(0.f, 0.f, _position(2));
 
+	_initEkfResetCounters();
 	_resetPositionLock();
 }
 
@@ -71,6 +73,12 @@ void FlightTaskManualAltitudeSmoothVel::_resetPositionLock()
 	_position_setpoint_z_locked = NAN;
 }
 
+void FlightTaskManualAltitudeSmoothVel::_initEkfResetCounters()
+{
+	_reset_counters.z = _sub_vehicle_local_position->get().z_reset_counter;
+	_reset_counters.vz = _sub_vehicle_local_position->get().vz_reset_counter;
+}
+
 void FlightTaskManualAltitudeSmoothVel::_checkEkfResetCounters()
 {
 	if (_sub_vehicle_local_position->get().z_reset_counter != _reset_counters.z) {

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

@@ -64,6 +64,7 @@ protected:
 private:
 
 	void _resetPositionLock();
+	void _initEkfResetCounters();
 	void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */
 
 	VelocitySmoothing _smoothing; ///< Smoothing in z direction

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

@@ -52,6 +52,7 @@ bool FlightTaskManualPositionSmoothVel::activate(vehicle_local_position_setpoint
 		_smoothing[i].reset(accel_prev(i), vel_prev(i), pos_prev(i));
 	}
 
+	_initEkfResetCounters();
 	_resetPositionLock();
 
 	return ret;
@@ -80,6 +81,14 @@ void FlightTaskManualPositionSmoothVel::_resetPositionLock()
 	_position_setpoint_z_locked = NAN;
 }
 
+void FlightTaskManualPositionSmoothVel::_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 FlightTaskManualPositionSmoothVel::_checkEkfResetCounters()
 {
 	// Check if a reset event has happened.

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

@@ -63,6 +63,7 @@ protected:
 				       )
 private:
 	void _resetPositionLock();
+	void _initEkfResetCounters();
 	void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */
 
 	VelocitySmoothing _smoothing[3]; ///< Smoothing in x, y and z directions