From f751dd2242a1c7a7d4acaa9f7f2158cd02eb72b0 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 20 Sep 2021 15:36:47 +0200 Subject: [PATCH] FlightTask: set yaw_setpoint to NAN when yaw should not be controlled --- msg/vehicle_local_position.msg | 1 + src/modules/ekf2/EKF2.cpp | 1 + .../tasks/Auto/FlightTaskAuto.cpp | 16 +++++++++++----- .../tasks/FlightTask/FlightTask.cpp | 1 + .../tasks/FlightTask/FlightTask.hpp | 1 + .../FlightTaskManualAcceleration.cpp | 6 ++++-- .../ManualAltitude/FlightTaskManualAltitude.cpp | 4 ++-- .../tasks/Utility/StickYaw.cpp | 9 +++++---- .../tasks/Utility/StickYaw.hpp | 6 +++--- 9 files changed, 29 insertions(+), 16 deletions(-) diff --git a/msg/vehicle_local_position.msg b/msg/vehicle_local_position.msg index 9b8bbca3e0..e8ac85ba98 100644 --- a/msg/vehicle_local_position.msg +++ b/msg/vehicle_local_position.msg @@ -41,6 +41,7 @@ float32 az # Down velocity derivative in NED earth-fixed frame, (metres/s float32 heading # Euler yaw angle transforming the tangent plane relative to NED earth-fixed frame, -PI..+PI, (radians) float32 delta_heading uint8 heading_reset_counter +bool heading_good_for_control # Position of reference point (local NED frame origin) in global (GPS / WGS84) frame bool xy_global # true if position (x, y) has a valid global reference (ref_lat, ref_lon) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 196f13fa46..ef01f031d8 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -888,6 +888,7 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) lpos.heading = Eulerf(_ekf.getQuaternion()).psi(); lpos.delta_heading = Eulerf(delta_q_reset).psi(); + lpos.heading_good_for_control = _ekf.isYawFinalAlignComplete(); // Distance to bottom surface (ground) in meters // constrain the distance to ground to _rng_gnd_clearance diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 5219fe4977..a71f4f334e 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -401,20 +401,24 @@ bool FlightTaskAuto::_evaluateTriplets() _yaw_setpoint = NAN; } else { - if ((_type != WaypointType::takeoff || _sub_triplet_setpoint.get().current.disable_weather_vane) - && _sub_triplet_setpoint.get().current.yaw_valid) { + if (!_is_yaw_good_for_control) { + _yaw_lock = false; + _yaw_setpoint = NAN; + _yawspeed_setpoint = 0.f; + + } else if ((_type != WaypointType::takeoff || _sub_triplet_setpoint.get().current.disable_weather_vane) + && _sub_triplet_setpoint.get().current.yaw_valid) { // Use the yaw computed in Navigator except during takeoff because // Navigator is not handling the yaw reset properly. // But: use if from Navigator during takeoff if disable_weather_vane is true, // because we're then aligning to the transition waypoint. // TODO: fix in navigator _yaw_setpoint = _sub_triplet_setpoint.get().current.yaw; + _yawspeed_setpoint = NAN; } else { _set_heading_from_mode(); } - - _yawspeed_setpoint = NAN; } return true; @@ -471,6 +475,8 @@ void FlightTaskAuto::_set_heading_from_mode() _yaw_lock = false; _yaw_setpoint = NAN; } + + _yawspeed_setpoint = NAN; } bool FlightTaskAuto::_isFinite(const position_setpoint_s &sp) @@ -809,7 +815,7 @@ void FlightTaskAuto::_prepareLandSetpoints() land_speed *= (1 + _sticks.getPositionExpo()(2)); _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading, - _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _deltatime); + _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _is_yaw_good_for_control, _deltatime); _stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _land_heading, _position, _velocity_setpoint_feedback.xy(), _deltatime); _stick_acceleration_xy.getSetpoints(_land_position, _velocity_setpoint, _acceleration_setpoint); diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp index 22a723d8c1..93279da86c 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp @@ -124,6 +124,7 @@ void FlightTask::_evaluateVehicleLocalPosition() // yaw _yaw = _sub_vehicle_local_position.get().heading; + _is_yaw_good_for_control = _sub_vehicle_local_position.get().heading_good_for_control; // position if (_sub_vehicle_local_position.get().xy_valid) { diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp index 36e70e0aed..b5e0b3b9e7 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp @@ -231,6 +231,7 @@ protected: matrix::Vector3f _velocity; /**< current vehicle velocity */ float _yaw{}; /**< current vehicle yaw heading */ + 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 */ float _dist_to_ground{}; /**< equals _dist_to_bottom if valid, height above home otherwise */ diff --git a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp index df6902a032..f503b866d7 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp @@ -68,8 +68,10 @@ bool FlightTaskManualAcceleration::update() bool ret = FlightTaskManualAltitudeSmoothVel::update(); _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, - _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _deltatime); - _stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _yaw_setpoint, _position, + _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _is_yaw_good_for_control, _deltatime); + + _stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _yaw_setpoint, + _position, _velocity_setpoint_feedback.xy(), _deltatime); _stick_acceleration_xy.getSetpoints(_position_setpoint, _velocity_setpoint, _acceleration_setpoint); diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 95cc9630d7..168dfd0177 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -288,7 +288,7 @@ void FlightTaskManualAltitude::_respectGroundSlowdown() void FlightTaskManualAltitude::_rotateIntoHeadingFrame(Vector2f &v) { - float yaw_rotate = PX4_ISFINITE(_yaw_setpoint) ? _yaw_setpoint : _yaw; + const float yaw_rotate = PX4_ISFINITE(_yaw_setpoint) ? _yaw_setpoint : _yaw; Vector3f v_r = Vector3f(Dcmf(Eulerf(0.0f, 0.0f, yaw_rotate)) * Vector3f(v(0), v(1), 0.0f)); v(0) = v_r(0); v(1) = v_r(1); @@ -296,7 +296,7 @@ void FlightTaskManualAltitude::_rotateIntoHeadingFrame(Vector2f &v) void FlightTaskManualAltitude::_updateHeadingSetpoints() { - if (_isYawInput()) { + if (_isYawInput() || !_is_yaw_good_for_control) { _unlockYaw(); } else { diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp index 4a467a8ef8..bddb48c09c 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp @@ -45,16 +45,17 @@ StickYaw::StickYaw() } void StickYaw::generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, const float desired_yawspeed, - const float yaw, const float deltatime) + const float yaw, const bool is_yaw_good_for_control, const float deltatime) { yawspeed_setpoint = _yawspeed_slew_rate.update(desired_yawspeed, deltatime); - yaw_setpoint = updateYawLock(yaw, yawspeed_setpoint, yaw_setpoint); + yaw_setpoint = updateYawLock(yaw, yawspeed_setpoint, yaw_setpoint, is_yaw_good_for_control); } -float StickYaw::updateYawLock(const float yaw, const float yawspeed_setpoint, const float yaw_setpoint) +float StickYaw::updateYawLock(const float yaw, const float yawspeed_setpoint, const float yaw_setpoint, + const bool is_yaw_good_for_control) { // Yaw-lock depends on desired yawspeed input. If not locked, yaw_sp is set to NAN. - if (fabsf(yawspeed_setpoint) > FLT_EPSILON) { + if ((fabsf(yawspeed_setpoint) > FLT_EPSILON) || !is_yaw_good_for_control) { // no fixed heading when rotating around yaw by stick return NAN; diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp index a6d71e0759..8e666b551d 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp @@ -47,8 +47,8 @@ public: StickYaw(); ~StickYaw() = default; - void generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, const float desired_yawspeed, const float yaw, - const float deltatime); + void generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, float desired_yawspeed, float yaw, + bool is_yaw_good_for_control, float deltatime); private: SlewRate _yawspeed_slew_rate; @@ -64,5 +64,5 @@ 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(const float yaw, const float yawspeed_setpoint, const float yaw_setpoint); + static float updateYawLock(float yaw, float yawspeed_setpoint, float yaw_setpoint, bool is_yaw_good_for_control); };