Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-04 21:23:57 +08:00
Code Issues Actions 4 Packages Projects Releases Wiki Activity

MC att control: Robustify for throttle scaling

Browse Source 
PX4 can support negative (reverse) throttle and the multicopter controller is not expecting this input range. This hardens it against it.
This commit is contained in:
Lorenz Meier
2021-02-13 20:34:44 +01:00
parent 548e070b91
commit 15219fbfe5
1 changed files with 3 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/mc_att_control_main.cpp
+3 -2
View File
@@ -127,7 +127,8 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt,
if (reset_yaw_sp) { if (reset_yaw_sp) {
_man_yaw_sp = yaw; _man_yaw_sp = yaw;
} else if (_manual_control_setpoint.z > 0.05f || _param_mc_airmode.get() == (int32_t)Mixer::Airmode::roll_pitch_yaw) { } else if (math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f) > 0.05f
|| _param_mc_airmode.get() == (int32_t)Mixer::Airmode::roll_pitch_yaw) {
const float yaw_rate = math::radians(_param_mpc_man_y_max.get()); const float yaw_rate = math::radians(_param_mpc_man_y_max.get());
attitude_setpoint.yaw_sp_move_rate = _manual_control_setpoint.r * yaw_rate; attitude_setpoint.yaw_sp_move_rate = _manual_control_setpoint.r * yaw_rate;
@@ -210,7 +211,7 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt,
Quatf q_sp = Eulerf(attitude_setpoint.roll_body, attitude_setpoint.pitch_body, attitude_setpoint.yaw_body); Quatf q_sp = Eulerf(attitude_setpoint.roll_body, attitude_setpoint.pitch_body, attitude_setpoint.yaw_body);
q_sp.copyTo(attitude_setpoint.q_d); q_sp.copyTo(attitude_setpoint.q_d);
attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_setpoint.z); attitude_setpoint.thrust_body[2] = -throttle_curve(math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f));
attitude_setpoint.timestamp = hrt_absolute_time(); attitude_setpoint.timestamp = hrt_absolute_time();
_vehicle_attitude_setpoint_pub.publish(attitude_setpoint); _vehicle_attitude_setpoint_pub.publish(attitude_setpoint);