diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index c1e4e096e8..b584c5b56d 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/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() diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp index c4b9649046..24d26abb59 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp +++ b/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() diff --git a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp index c1d1edaff4..4c2e11d889 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp +++ b/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); +} diff --git a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp index 694bcb682f..b6dceba037 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp +++ b/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; diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp index 1466d2dcb4..b2515b1d52 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp +++ b/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); } diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp index fe0aee7acb..6c5d38f34d 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp +++ b/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); diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index b2f88d4c65..2df5825e0e 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/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) { diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index 10aa105d06..d24adfd647 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/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; + } +} diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp index 124fd08b03..d828a30d91 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp +++ b/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. */