fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_att_control: use StickYaw class for Stabilized

Browse Source 
to make yawing including reset and drift handling consistent with
Altitude and Position mode.
This commit is contained in:
Matthias Grob 2025-04-01 22:03:54 +02:00
parent 174147208e
commit d514cb4903
4 changed files with 51 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/lib/stick_yaw/StickYaw.cpp
View File

@ -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;

1
src/modules/mc_att_control/CMakeLists.txt
View File

@ -45,4 +45,5 @@ px4_add_module(
AttitudeControl
mathlib
px4_work_queue
StickYaw
)

24
src/modules/mc_att_control/mc_att_control.hpp
View File

@ -57,6 +57,7 @@
#include <uORB/topics/vehicle_status.h>
#include <lib/mathlib/math/filter/AlphaFilter.hpp>
#include <lib/slew_rate/SlewRate.hpp>
#include <lib/stick_yaw/StickYaw.hpp>
#include <AttitudeControl.hpp>
@ -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<float> _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<px4::params::MC_YAWRATE_MAX>) _param_mc_yawrate_max,
/* Stabilized mode params */
(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) _param_mpc_man_tilt_max, /**< maximum tilt allowed for manual flight */
(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max, /**< scaling factor from stick to yaw rate */
(ParamFloat<px4::params::MPC_MANTHR_MIN>) _param_mpc_manthr_min, /**< minimum throttle for stabilized */
(ParamFloat<px4::params::MPC_THR_MAX>) _param_mpc_thr_max, /**< maximum throttle for stabilized */
(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover, /**< throttle at stationary hover */
(ParamInt<px4::params::MPC_THR_CURVE>) _param_mpc_thr_curve, /**< throttle curve behavior */
(ParamFloat<px4::params::MPC_HOLD_DZ>) _param_mpc_hold_dz,
(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) _param_mpc_man_tilt_max,
(ParamFloat<px4::params::MPC_MANTHR_MIN>) _param_mpc_manthr_min,
(ParamFloat<px4::params::MPC_THR_MAX>) _param_mpc_thr_max,
(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover,
(ParamInt<px4::params::MPC_THR_CURVE>) _param_mpc_thr_curve,
(ParamFloat<px4::params::MPC_YAW_EXPO>) _param_mpc_yaw_expo,
(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time
)

68
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -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);