[WIP] multicopter adjust all velocity/acceleration states and setpoints on yaw reset

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp

@@ -706,6 +706,28 @@ void FlightTaskAuto::_ekfResetHandlerVelocityZ(float delta_vz)
 void FlightTaskAuto::_ekfResetHandlerHeading(float delta_psi)
 {
 	_yaw_sp_prev += delta_psi;
+
+	const Quatf q(Eulerf(0.f, 0.f, delta_psi));
+
+	if (_velocity_setpoint.isAllFinite()) {
+		_velocity_setpoint = q.rotateVector(_velocity_setpoint);
+	}
+
+	if (_velocity_setpoint_feedback.isAllFinite()) {
+		_velocity_setpoint_feedback = q.rotateVector(_velocity_setpoint_feedback);
+	}
+
+	if (_acceleration_setpoint.isAllFinite()) {
+		_acceleration_setpoint = q.rotateVector(_acceleration_setpoint);
+	}
+
+	if (_acceleration_setpoint_feedback.isAllFinite()) {
+		_acceleration_setpoint_feedback = q.rotateVector(_acceleration_setpoint_feedback);
+	}
+
+	if (_jerk_setpoint.isAllFinite()) {
+		_jerk_setpoint = q.rotateVector(_jerk_setpoint);
+	}
 }
 
 void FlightTaskAuto::_checkEmergencyBraking()

src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp

@@ -48,14 +48,25 @@ bool FlightTask::update()
 void FlightTask::_checkEkfResetCounters()
 {
 	// Check if a reset event has happened
+
+	bool yaw_reset = false;
+
+	if (_sub_vehicle_local_position.get().heading_reset_counter != _reset_counters.heading) {
+		_ekfResetHandlerHeading(_sub_vehicle_local_position.get().delta_heading);
+		_reset_counters.heading = _sub_vehicle_local_position.get().heading_reset_counter;
+		yaw_reset = true;
+	}
+
 	if (_sub_vehicle_local_position.get().xy_reset_counter != _reset_counters.xy) {
 		_ekfResetHandlerPositionXY(matrix::Vector2f{_sub_vehicle_local_position.get().delta_xy});
 		_reset_counters.xy = _sub_vehicle_local_position.get().xy_reset_counter;
 	}
 
-	if (_sub_vehicle_local_position.get().vxy_reset_counter != _reset_counters.vxy) {
-		_ekfResetHandlerVelocityXY(matrix::Vector2f{_sub_vehicle_local_position.get().delta_vxy});
-		_reset_counters.vxy = _sub_vehicle_local_position.get().vxy_reset_counter;
+	if (!yaw_reset) {
+		if (_sub_vehicle_local_position.get().vxy_reset_counter != _reset_counters.vxy) {
+			_ekfResetHandlerVelocityXY(matrix::Vector2f{_sub_vehicle_local_position.get().delta_vxy});
+			_reset_counters.vxy = _sub_vehicle_local_position.get().vxy_reset_counter;
+		}
 	}
 
 	if (_sub_vehicle_local_position.get().z_reset_counter != _reset_counters.z) {
@@ -67,11 +78,6 @@ void FlightTask::_checkEkfResetCounters()
 		_ekfResetHandlerVelocityZ(_sub_vehicle_local_position.get().delta_vz);
 		_reset_counters.vz = _sub_vehicle_local_position.get().vz_reset_counter;
 	}
-
-	if (_sub_vehicle_local_position.get().heading_reset_counter != _reset_counters.heading) {
-		_ekfResetHandlerHeading(_sub_vehicle_local_position.get().delta_heading);
-		_reset_counters.heading = _sub_vehicle_local_position.get().heading_reset_counter;
-	}
 }
 
 const trajectory_setpoint_s FlightTask::getTrajectorySetpoint()

src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp

@@ -98,3 +98,10 @@ void FlightTaskManualAcceleration::_ekfResetHandlerVelocityXY(const matrix::Vect
 {
 	_stick_acceleration_xy.resetVelocity(_velocity.xy());
 }
+
+void FlightTaskManualAcceleration::_ekfResetHandlerHeading(float delta_psi)
+{
+	FlightTaskManualAltitude::_ekfResetHandlerHeading(delta_psi);
+
+	_stick_acceleration_xy.resetHeading(delta_psi);
+}

src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp

@@ -56,6 +56,7 @@ public:
 private:
 	void _ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) override;
 	void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) override;
+	void _ekfResetHandlerHeading(float delta_psi) override;
 
 	StickAccelerationXY _stick_acceleration_xy{this};
 	StickYaw _stick_yaw;

src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp

@@ -64,6 +64,22 @@ void StickAccelerationXY::resetVelocity(const matrix::Vector2f &velocity)
 	_velocity_setpoint = velocity;
 }
 
+void StickAccelerationXY::resetHeading(const float delta_heading)
+{
+	const Quatf q(Eulerf(0.f, 0.f, delta_heading));
+
+	_velocity_setpoint = q.rotateVector(Vector3f(_velocity_setpoint(0), _velocity_setpoint(1), 0.f)).xy();
+
+	_acceleration_setpoint = q.rotateVector(Vector3f(_acceleration_setpoint(0), _acceleration_setpoint(1), 0.f)).xy();
+	_acceleration_setpoint_prev = q.rotateVector(
+					      Vector3f(_acceleration_setpoint_prev(0), _acceleration_setpoint_prev(1), 0.f)).xy();
+
+	Vector2f acc_slew_rate = q.rotateVector(
+					 Vector3f(_acceleration_slew_rate_x.getState(), _acceleration_slew_rate_y.getState(), 0.f)).xy();
+	_acceleration_slew_rate_x.setForcedValue(acc_slew_rate(0));
+	_acceleration_slew_rate_y.setForcedValue(acc_slew_rate(1));
+}
+
 void StickAccelerationXY::resetAcceleration(const matrix::Vector2f &acceleration)
 {
 	_acceleration_slew_rate_x.setForcedValue(acceleration(0));
@@ -154,7 +170,7 @@ Vector2f StickAccelerationXY::calculateDrag(Vector2f drag_coefficient, const flo
 
 	drag_coefficient *= _brake_boost_filter.getState();
 
-	// increase drag with sqareroot function when velocity is lower than 1m/s
+	// increase drag with squareroot function when velocity is lower than 1m/s
 	const Vector2f velocity_with_sqrt_boost = vel_sp.unit_or_zero() * math::sqrt_linear(vel_sp.norm());
 	return drag_coefficient.emult(velocity_with_sqrt_boost);
 }

src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp

@@ -57,6 +57,8 @@ public:
 	void resetPosition(const matrix::Vector2f &position);
 	void resetVelocity(const matrix::Vector2f &velocity);
 	void resetAcceleration(const matrix::Vector2f &acceleration);
+	void resetHeading(const float delta_heading);
+
 	void generateSetpoints(matrix::Vector2f stick_xy, const float yaw, const float yaw_sp, const matrix::Vector3f &pos,
 			       const matrix::Vector2f &vel_sp_feedback, const float dt);
 	void getSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, matrix::Vector3f &acc_sp);

src/modules/mc_pos_control/MulticopterPositionControl.cpp

@@ -372,7 +372,28 @@ void MulticopterPositionControl::Run()
 
 		// adjust existing (or older) setpoint with any EKF reset deltas
 		if ((_setpoint.timestamp != 0) && (_setpoint.timestamp < vehicle_local_position.timestamp)) {
-			if (vehicle_local_position.vxy_reset_counter != _vxy_reset_counter) {
+
+			if (vehicle_local_position.heading_reset_counter != _heading_reset_counter) {
+				_setpoint.yaw = wrap_pi(_setpoint.yaw + vehicle_local_position.delta_heading);
+
+				// rotate setpoints
+				const Quatf q(Eulerf(0.f, 0.f, vehicle_local_position.delta_heading));
+
+				// velocity
+				const Vector3f vel_sp{_setpoint.velocity};
+
+				if (vel_sp.isAllFinite()) {
+					q.rotateVector(vel_sp).copyTo(_setpoint.velocity);
+				}
+
+				// acceleration
+				const Vector3f acc_sp{_setpoint.acceleration};
+
+				if (acc_sp.isAllFinite()) {
+					q.rotateVector(acc_sp).copyTo(_setpoint.acceleration);
+				}
+
+			} else if (vehicle_local_position.vxy_reset_counter != _vxy_reset_counter) {
 				_setpoint.velocity[0] += vehicle_local_position.delta_vxy[0];
 				_setpoint.velocity[1] += vehicle_local_position.delta_vxy[1];
 			}
@@ -390,9 +411,10 @@ void MulticopterPositionControl::Run()
 				_setpoint.position[2] += vehicle_local_position.delta_z;
 			}
 
-			if (vehicle_local_position.heading_reset_counter != _heading_reset_counter) {
-				_setpoint.yaw = wrap_pi(_setpoint.yaw + vehicle_local_position.delta_heading);
-			}
+		}
+
+		if (vehicle_local_position.heading_reset_counter != _heading_reset_counter) {
+			_control.resetHeading(vehicle_local_position.delta_heading);
 		}
 
 		if (vehicle_local_position.vxy_reset_counter != _vxy_reset_counter) {

src/modules/mc_pos_control/PositionControl/PositionControl.cpp

@@ -260,3 +260,38 @@ void PositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_
 	ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, attitude_setpoint);
 	attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp;
 }
+
+void PositionControl::resetHeading(const float delta_heading)
+{
+	const Quatf q(Eulerf(0.f, 0.f, delta_heading));
+
+	// States
+	if (_vel.isAllFinite()) {
+		_vel = q.rotateVector(_vel);
+	}
+
+	if (_vel_dot.isAllFinite()) {
+		_vel_dot = q.rotateVector(_vel_dot);
+	}
+
+	if (_vel_int.isAllFinite()) {
+		_vel_int = q.rotateVector(_vel_int);
+	}
+
+	if (PX4_ISFINITE(_yaw)) {
+		_yaw += delta_heading;
+	}
+
+	// Setpoints
+	if (_vel_sp.isAllFinite()) {
+		_vel_sp = q.rotateVector(_vel_sp);
+	}
+
+	if (_acc_sp.isAllFinite()) {
+		_acc_sp = q.rotateVector(_acc_sp);
+	}
+
+	if (PX4_ISFINITE(_yaw_sp)) {
+		_yaw_sp += delta_heading;
+	}
+}

src/modules/mc_pos_control/PositionControl/PositionControl.hpp

@@ -179,6 +179,8 @@ public:
 	 */
 	void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const;
 
+	void resetHeading(const float delta_heading);
+
 	/**
 	 * All setpoints are set to NAN (uncontrolled). Timestampt zero.
 	 */