From 23b31cc5fdc33fe1f43dcf12bb6fe4a5afbbc24d Mon Sep 17 00:00:00 2001 From: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)> Date: Mon, 21 Aug 2023 10:56:08 +0200 Subject: [PATCH] manual_yaw: compensate for yaw estimate convergence When the yaw estimate is converging, the controller makes the drone yaw in order to follow the current setpoint. This is unintuitive for the pilot and it is preferable if the drone continues to fly towards the same physical direction. --- src/drivers/ins/vectornav/VectorNav.cpp | 1 + .../tasks/Auto/FlightTaskAuto.cpp | 3 +- .../tasks/Descend/FlightTaskDescend.cpp | 3 +- .../tasks/FlightTask/FlightTask.cpp | 1 + .../tasks/FlightTask/FlightTask.hpp | 1 + .../FlightTaskManualAltitude.cpp | 8 ++- .../FlightTaskManualAltitude.hpp | 3 +- .../tasks/Utility/StickYaw.cpp | 69 +++++++++++++++++-- .../tasks/Utility/StickYaw.hpp | 17 ++++- .../BlockLocalPositionEstimator.cpp | 5 +- src/modules/mavlink/mavlink_receiver.cpp | 2 + src/modules/simulation/simulator_sih/sih.cpp | 1 + 12 files changed, 96 insertions(+), 18 deletions(-) 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);