diff --git a/src/lib/stick_yaw/StickYaw.cpp b/src/lib/stick_yaw/StickYaw.cpp index 0118e53cd4..2978d8e37b 100644 --- a/src/lib/stick_yaw/StickYaw.cpp +++ b/src/lib/stick_yaw/StickYaw.cpp @@ -113,7 +113,7 @@ float StickYaw::updateYawLock(const float yaw, const float yawspeed_setpoint, co return NAN; } else { - // break down and hold the current heading when no more rotation commanded + // slow down and hold the current heading when no more rotation commanded if (!PX4_ISFINITE(yaw_setpoint)) { return yaw; diff --git a/src/modules/mc_att_control/CMakeLists.txt b/src/modules/mc_att_control/CMakeLists.txt index abd0f4ff9c..d08d100b1d 100644 --- a/src/modules/mc_att_control/CMakeLists.txt +++ b/src/modules/mc_att_control/CMakeLists.txt @@ -45,4 +45,5 @@ px4_add_module( AttitudeControl mathlib px4_work_queue + StickYaw ) diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index dbf40990e1..f2632279ab 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -57,6 +57,7 @@ #include #include #include +#include #include @@ -93,9 +94,10 @@ private: /** * Generate & publish an attitude setpoint from stick inputs */ - void generate_attitude_setpoint(const matrix::Quatf &q, float dt, bool reset_yaw_sp); + void generate_attitude_setpoint(const matrix::Quatf &q, float dt); AttitudeControl _attitude_control; /**< class for attitude control calculations */ + StickYaw _stick_yaw{this}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -119,10 +121,11 @@ private: perf_counter_t _loop_perf; /**< loop duration performance counter */ matrix::Vector3f _thrust_setpoint_body; /**< body frame 3D thrust vector */ - float _hover_thrust{NAN}; - float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ + float _yaw_setpoint_stabilized{0.f}; + bool _heading_good_for_control{true}; // initialized true to have heading lock when local position never published + float _unaided_heading{NAN}; // initialized NAN to not distract heading lock when local position never published float _man_tilt_max{0.f}; /**< maximum tilt allowed for manual flight [rad] */ SlewRate _manual_throttle_minimum{0.f}; ///< 0 when landed and ramped to MPC_MANTHR_MIN in air @@ -135,8 +138,6 @@ private: bool _spooled_up{false}; ///< used to make sure the vehicle cannot take off during the spoolup time bool _landed{true}; - bool _reset_yaw_sp{true}; - bool _heading_good_for_control{true}; ///< initialized true to have heading lock when local position never published bool _vehicle_type_rotary_wing{true}; bool _vtol{false}; bool _vtol_tailsitter{false}; @@ -158,12 +159,13 @@ private: (ParamFloat) _param_mc_yawrate_max, /* Stabilized mode params */ - (ParamFloat) _param_mpc_man_tilt_max, /**< maximum tilt allowed for manual flight */ - (ParamFloat) _param_mpc_man_y_max, /**< scaling factor from stick to yaw rate */ - (ParamFloat) _param_mpc_manthr_min, /**< minimum throttle for stabilized */ - (ParamFloat) _param_mpc_thr_max, /**< maximum throttle for stabilized */ - (ParamFloat) _param_mpc_thr_hover, /**< throttle at stationary hover */ - (ParamInt) _param_mpc_thr_curve, /**< throttle curve behavior */ + (ParamFloat) _param_mpc_hold_dz, + (ParamFloat) _param_mpc_man_tilt_max, + (ParamFloat) _param_mpc_manthr_min, + (ParamFloat) _param_mpc_thr_max, + (ParamFloat) _param_mpc_thr_hover, + (ParamInt) _param_mpc_thr_curve, + (ParamFloat) _param_mpc_yaw_expo, (ParamFloat) _param_com_spoolup_time ) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 22b78d519c..15f3f6ef40 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -135,25 +135,22 @@ MulticopterAttitudeControl::throttle_curve(float throttle_stick_input) } void -MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, bool reset_yaw_sp) +MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt) { vehicle_attitude_setpoint_s attitude_setpoint{}; + + // Avoid accumulating absolute yaw error with arming stick gesture + const bool arming_gesture = (_manual_control_setpoint.throttle < -.9f) && (_param_mc_airmode.get() != 2); + + if (arming_gesture || !_heading_good_for_control) { + _yaw_setpoint_stabilized = NAN; + } + const float yaw = Eulerf(q).psi(); - - attitude_setpoint.yaw_sp_move_rate = _manual_control_setpoint.yaw * math::radians(_param_mpc_man_y_max.get()); - - // Avoid accumulating absolute yaw error with arming stick gesture in case heading_good_for_control stays true - if ((_manual_control_setpoint.throttle < -.9f) && (_param_mc_airmode.get() != 2)) { - reset_yaw_sp = true; - } - - // Make sure not absolute heading error builds up - if (reset_yaw_sp) { - _man_yaw_sp = yaw; - - } else { - _man_yaw_sp = wrap_pi(_man_yaw_sp + attitude_setpoint.yaw_sp_move_rate * dt); - } + const float yaw_stick_input = math::expo_deadzone(_manual_control_setpoint.yaw, _param_mpc_yaw_expo.get(), + _param_mpc_hold_dz.get()); + _stick_yaw.generateYawSetpoint(attitude_setpoint.yaw_sp_move_rate, _yaw_setpoint_stabilized, yaw_stick_input, yaw, dt, + _unaided_heading); /* * Input mapping for roll & pitch setpoints @@ -178,11 +175,13 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, } Quatf q_sp_rp = AxisAnglef(v(0), v(1), 0.f); + // Make sure there's a valid attitude quaternion with no yaw error when yaw is unlocked (NAN) + const float yaw_setpoint = PX4_ISFINITE(_yaw_setpoint_stabilized) ? _yaw_setpoint_stabilized : yaw; // The axis angle can change the yaw as well (noticeable at higher tilt angles). // This is the formula by how much the yaw changes: // let a := tilt angle, b := atan(y/x) (direction of maximum tilt) // yaw = atan(-2 * sin(b) * cos(b) * sin^2(a/2) / (1 - 2 * cos^2(b) * sin^2(a/2))). - const Quatf q_sp_yaw(cosf(_man_yaw_sp / 2.f), 0.f, 0.f, sinf(_man_yaw_sp / 2.f)); + const Quatf q_sp_yaw(cosf(yaw_setpoint / 2.f), 0.f, 0.f, sinf(yaw_setpoint / 2.f)); if (_vtol) { // Modify the setpoints for roll and pitch such that they reflect the user's intention even @@ -267,33 +266,31 @@ MulticopterAttitudeControl::Run() if (_vehicle_local_position_sub.copy(&vehicle_local_position)) { _heading_good_for_control = vehicle_local_position.heading_good_for_control; + _unaided_heading = vehicle_local_position.unaided_heading; } } - bool attitude_setpoint_generated = false; - + // during transitions VTOL module generates attitude setpoints const bool is_hovering = (_vehicle_type_rotary_wing && !_vtol_in_transition_mode); - - // vehicle is a tailsitter in transition mode const bool is_tailsitter_transition = (_vtol_tailsitter && _vtol_in_transition_mode); - const bool run_att_ctrl = _vehicle_control_mode.flag_control_attitude_enabled && (is_hovering - || is_tailsitter_transition); + const bool run_att_ctrl = _vehicle_control_mode.flag_control_attitude_enabled + && (is_hovering || is_tailsitter_transition); if (run_att_ctrl) { - // Generate the attitude setpoint from stick inputs if we are in Manual/Stabilized mode if (_vehicle_control_mode.flag_control_manual_enabled && !_vehicle_control_mode.flag_control_altitude_enabled && !_vehicle_control_mode.flag_control_velocity_enabled && !_vehicle_control_mode.flag_control_position_enabled) { - generate_attitude_setpoint(q, dt, _reset_yaw_sp); - attitude_setpoint_generated = true; + generate_attitude_setpoint(q, dt); } else { _man_roll_input_filter.reset(0.f); _man_pitch_input_filter.reset(0.f); + _yaw_setpoint_stabilized = NAN; + _stick_yaw.reset(Eulerf(q).psi(), _unaided_heading); } // Check for new attitude setpoint @@ -312,9 +309,14 @@ MulticopterAttitudeControl::Run() // Check for a heading reset if (_quat_reset_counter != v_att.quat_reset_counter) { const Quatf delta_q_reset(v_att.delta_q_reset); + const float delta_psi = Eulerf(delta_q_reset).psi(); - // for stabilized attitude generation only extract the heading change from the delta quaternion - _man_yaw_sp = wrap_pi(_man_yaw_sp + Eulerf(delta_q_reset).psi()); + // Only offset the yaw setpoint when the heading is locked + if (PX4_ISFINITE(_yaw_setpoint_stabilized)) { + _yaw_setpoint_stabilized = wrap_pi(_yaw_setpoint_stabilized + delta_psi); + } + + _stick_yaw.ekfResetHandler(delta_psi); if (v_att.timestamp > _last_attitude_setpoint) { // adapt existing attitude setpoint unless it was generated after the current attitude estimate @@ -348,6 +350,12 @@ MulticopterAttitudeControl::Run() rates_setpoint.timestamp = hrt_absolute_time(); _vehicle_rates_setpoint_pub.publish(rates_setpoint); + + } else { + _man_roll_input_filter.reset(0.f); + _man_pitch_input_filter.reset(0.f); + _yaw_setpoint_stabilized = NAN; + _stick_yaw.reset(Eulerf(q).psi(), _unaided_heading); } if (_landed) { @@ -363,10 +371,6 @@ MulticopterAttitudeControl::Run() } else { _manual_throttle_maximum.setForcedValue(0.f); } - - // reset yaw setpoint during transitions, tailsitter.cpp generates - // attitude setpoint for the transition - _reset_yaw_sp = !attitude_setpoint_generated || !_heading_good_for_control || (_vtol && _vtol_in_transition_mode); } perf_end(_loop_perf);