diff --git a/src/drivers/ins/vectornav/VectorNav.cpp b/src/drivers/ins/vectornav/VectorNav.cpp index eae3dc5486..7dfe173a29 100644 --- a/src/drivers/ins/vectornav/VectorNav.cpp +++ b/src/drivers/ins/vectornav/VectorNav.cpp @@ -250,6 +250,7 @@ void VectorNav::sensorCallback(VnUartPacket *packet) local_position.evv = velocityUncertaintyEstimated; local_position.xy_valid = true; local_position.heading_good_for_control = mode_tracking; + local_position.unaided_heading = NAN; local_position.timestamp = hrt_absolute_time(); _local_position_pub.publish(local_position); diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 6c34aae7a3..8168f0bca5 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -221,8 +221,7 @@ void FlightTaskAuto::rcHelpModifyYaw(float &yaw_sp) { // Only set a yawrate setpoint if weather vane is not active or the yaw stick is out of its dead-zone if (!_weathervane.isActive() || fabsf(_sticks.getYawExpo()) > FLT_EPSILON) { - _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, yaw_sp, _sticks.getYawExpo(), _yaw, _is_yaw_good_for_control, - _deltatime); + _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, yaw_sp, _sticks.getYawExpo(), _yaw, _deltatime); // Hack to make sure the MPC_YAW_MODE 4 alignment doesn't stop the vehicle from descending when there's yaw input _yaw_sp_aligned = true; diff --git a/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp b/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp index 3ebc783581..85ba49ea0b 100644 --- a/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp +++ b/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp @@ -60,8 +60,7 @@ bool FlightTaskDescend::update() // Nudging if (_param_mpc_land_rc_help.get() && _sticks.checkAndUpdateStickInputs()) { - _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw, - _is_yaw_good_for_control, _deltatime); + _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw, _deltatime); _acceleration_setpoint.xy() = _stick_tilt_xy.generateAccelerationSetpoints(_sticks.getPitchRoll(), _deltatime, _yaw, _yaw_setpoint); diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp index c4b9649046..063b78dcf6 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp @@ -112,6 +112,7 @@ void FlightTask::_evaluateVehicleLocalPosition() // yaw _yaw = _sub_vehicle_local_position.get().heading; + _unaided_yaw = _sub_vehicle_local_position.get().unaided_heading; _is_yaw_good_for_control = _sub_vehicle_local_position.get().heading_good_for_control; // position diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp index 95021fed59..3b9ddbd466 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp @@ -218,6 +218,7 @@ protected: matrix::Vector3f _velocity; /**< current vehicle velocity */ float _yaw{}; /**< current vehicle yaw heading */ + float _unaided_yaw{}; bool _is_yaw_good_for_control{}; /**< true if the yaw estimate can be used for yaw control */ float _dist_to_bottom{}; /**< current height above ground level if dist_bottom is valid */ float _dist_to_ground{}; /**< equals _dist_to_bottom if available, height above home otherwise */ diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 0a2704d68a..ab90d10e12 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -64,6 +64,7 @@ bool FlightTaskManualAltitude::activate(const trajectory_setpoint_s &last_setpoi _acceleration_setpoint = Vector3f(0.f, 0.f, NAN); // altitude is controlled from position/velocity _position_setpoint(2) = _position(2); _velocity_setpoint(2) = 0.f; + _stick_yaw.reset(_yaw, _unaided_yaw); _setDefaultConstraints(); _updateConstraintsFromEstimator(); @@ -273,14 +274,15 @@ 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; + _yaw_setpoint = wrap_pi(_yaw_setpoint + delta_psi); } + + _stick_yaw.ekfResetHandler(delta_psi); } void FlightTaskManualAltitude::_updateSetpoints() { - _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw, - _is_yaw_good_for_control, _deltatime); + _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw, _deltatime, _unaided_yaw); _acceleration_setpoint.xy() = _stick_tilt_xy.generateAccelerationSetpoints(_sticks.getPitchRoll(), _deltatime, _yaw, _yaw_setpoint); _updateAltitudeLock(); diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index fdac174e37..bf7ec2dc49 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -40,7 +40,6 @@ #pragma once #include "FlightTask.hpp" -#include #include "Sticks.hpp" #include "StickTiltXY.hpp" #include "StickYaw.hpp" @@ -115,6 +114,8 @@ private: void setGearAccordingToSwitch(); + bool _updateYawCorrection(); + uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */ bool _terrain_follow{false}; /**< true when the vehicle is following the terrain height */ bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */ diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp index 8a2512279c..edc82d6c98 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp @@ -35,23 +35,80 @@ #include +using matrix::wrap_pi; + StickYaw::StickYaw(ModuleParams *parent) : ModuleParams(parent) {} -void StickYaw::generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, const float stick_yaw, - const float yaw, const bool is_yaw_good_for_control, const float deltatime) +void StickYaw::reset(const float yaw, const float unaided_yaw) { + if (PX4_ISFINITE(unaided_yaw)) { + _yaw_error_lpf.reset(wrap_pi(yaw - unaided_yaw)); + } +} + +void StickYaw::ekfResetHandler(const float delta_yaw) +{ + _yaw_error_lpf.reset(wrap_pi(_yaw_error_lpf.getState() + delta_yaw)); + _yaw_error_ref = wrap_pi(_yaw_error_ref + delta_yaw); +} + +void StickYaw::generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, const float stick_yaw, + const float yaw, const float deltatime, const float unaided_yaw) +{ + _yaw_error_lpf.setParameters(deltatime, _kYawErrorTimeConstant); + const float yaw_correction_prev = _yaw_correction; + const bool reset_setpoint = updateYawCorrection(yaw, unaided_yaw); + + if (reset_setpoint) { + yaw_setpoint = NAN; + } + _yawspeed_filter.setParameters(deltatime, _param_mpc_man_y_tau.get()); yawspeed_setpoint = _yawspeed_filter.update(stick_yaw * math::radians(_param_mpc_man_y_max.get())); - yaw_setpoint = updateYawLock(yaw, yawspeed_setpoint, yaw_setpoint, is_yaw_good_for_control); + yaw_setpoint = updateYawLock(yaw, yawspeed_setpoint, yaw_setpoint, yaw_correction_prev); +} + +bool StickYaw::updateYawCorrection(const float yaw, const float unaided_yaw) +{ + if (!PX4_ISFINITE(unaided_yaw)) { + _yaw_correction = 0.f; + return false; + } + + // Detect the convergence phase of the yaw estimate by monitoring its relative + // distance from an unaided yaw source. + const float yaw_error = wrap_pi(yaw - unaided_yaw); + + // Run it through a high-pass filter to detect transients + const float yaw_error_hpf = wrap_pi(yaw_error - _yaw_error_lpf.getState()); + _yaw_error_lpf.update(yaw_error); + + const bool was_converging = _yaw_estimate_converging; + _yaw_estimate_converging = fabsf(yaw_error_hpf) > _kYawErrorChangeThreshold; + + bool reset_setpoint = false; + + if (!_yaw_estimate_converging) { + _yaw_error_ref = yaw_error; + + if (was_converging) { + // Force a reset of the locking mechanism + reset_setpoint = true; + } + } + + _yaw_correction = wrap_pi(yaw_error - _yaw_error_ref); + + return reset_setpoint; } float StickYaw::updateYawLock(const float yaw, const float yawspeed_setpoint, const float yaw_setpoint, - const bool is_yaw_good_for_control) + const float yaw_correction_prev) const { // Yaw-lock depends on desired yawspeed input. If not locked, yaw_sp is set to NAN. - if ((fabsf(yawspeed_setpoint) > FLT_EPSILON) || !is_yaw_good_for_control) { + if (fabsf(yawspeed_setpoint) > FLT_EPSILON) { // no fixed heading when rotating around yaw by stick return NAN; @@ -61,7 +118,7 @@ float StickYaw::updateYawLock(const float yaw, const float yawspeed_setpoint, co return yaw; } else { - return yaw_setpoint; + return wrap_pi(yaw_setpoint - yaw_correction_prev + _yaw_correction); } } } diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp index 8e9e4aea1e..185c490110 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp @@ -48,12 +48,23 @@ public: StickYaw(ModuleParams *parent); ~StickYaw() = default; - void generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, float stick_yaw, float yaw, - bool is_yaw_good_for_control, float deltatime); + void reset(float yaw, float unaided_yaw = NAN); + void ekfResetHandler(float delta_yaw); + void generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, float stick_yaw, float yaw, float deltatime, + float unaided_yaw = NAN); private: AlphaFilter _yawspeed_filter; + float _yaw_error_ref{0.f}; + float _yaw_correction{0.f}; + bool _yaw_estimate_converging{false}; + AlphaFilter _yaw_error_lpf{0.01f}; ///< used to create a high-pass filter + static constexpr float _kYawErrorTimeConstant{1.f}; ///< time constant of the high-pass filter used to detect yaw convergence + static constexpr float _kYawErrorChangeThreshold{radians(1.f)}; ///< we consider the yaw estimate as "converging" when above this threshold + + bool updateYawCorrection(float yaw, float unaided_yaw); + /** * Lock yaw when not currently turning * When applying a yawspeed the vehicle is turning, when the speed is @@ -65,7 +76,7 @@ private: * @param yaw current yaw setpoint which then will be overwritten by the return value * @return yaw setpoint to execute to have a yaw lock at the correct moment in time */ - static float updateYawLock(float yaw, float yawspeed_setpoint, float yaw_setpoint, bool is_yaw_good_for_control); + float updateYawLock(float yaw, float yawspeed_setpoint, float yaw_setpoint, float yaw_correction_prev) const; DEFINE_PARAMETERS( (ParamFloat) _param_mpc_man_y_max, ///< Maximum yaw speed with full stick deflection diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 508a542754..9ece702888 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -600,7 +600,10 @@ void BlockLocalPositionEstimator::publishLocalPos() _pub_lpos.get().z = xLP(X_z); // down } - _pub_lpos.get().heading = matrix::Eulerf(matrix::Quatf(_sub_att.get().q)).psi(); + const float heading = matrix::Eulerf(matrix::Quatf(_sub_att.get().q)).psi(); + _pub_lpos.get().heading = heading; + _pub_lpos.get().heading_good_for_control = PX4_ISFINITE(heading); + _pub_lpos.get().unaided_heading = NAN; _pub_lpos.get().vx = xLP(X_vx); // north _pub_lpos.get().vy = xLP(X_vy); // east diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 767ec7d02f..de603c6b17 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2713,6 +2713,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) matrix::Eulerf euler{matrix::Quatf(hil_state.attitude_quaternion)}; hil_local_pos.heading = euler.psi(); + hil_local_pos.heading_good_for_control = PX4_ISFINITE(euler.psi()); + hil_local_pos.unaided_heading = NAN; hil_local_pos.xy_global = true; hil_local_pos.z_global = true; hil_local_pos.vxy_max = INFINITY; diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index fd8da11891..1fa076568b 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -546,6 +546,7 @@ void Sih::publish_ground_truth(const hrt_abstime &time_now_us) local_position.heading = Eulerf(_q).psi(); local_position.heading_good_for_control = true; + local_position.unaided_heading = NAN; local_position.timestamp = hrt_absolute_time(); _local_position_ground_truth_pub.publish(local_position);