fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 05:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

FW att control: Robustify for throttle scaling

Browse Source 
PX4 can support negative (reverse) throttle and the fixed wing controller is not expecting this input range. This hardens it against it.
This commit is contained in:
Lorenz Meier
2021-02-14 11:17:28 +01:00
parent 0d3676e5b0
commit 6d489a4b4d
1 changed files with 3 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+3 -3
View File
@@ -165,7 +165,7 @@ FixedwingAttitudeControl::vehicle_manual_poll()
-radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get()));
_att_sp.yaw_body = 0.0f;
_att_sp.thrust_body[0] = _manual_control_setpoint.z;
_att_sp.thrust_body[0] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f);
Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
q.copyTo(_att_sp.q_d);
@@ -182,7 +182,7 @@ FixedwingAttitudeControl::vehicle_manual_poll()
_rates_sp.roll = _manual_control_setpoint.y * radians(_param_fw_acro_x_max.get());
_rates_sp.pitch = -_manual_control_setpoint.x * radians(_param_fw_acro_y_max.get());
_rates_sp.yaw = _manual_control_setpoint.r * radians(_param_fw_acro_z_max.get());
_rates_sp.thrust_body[0] = _manual_control_setpoint.z;
_rates_sp.thrust_body[0] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f);
_rate_sp_pub.publish(_rates_sp);
@@ -194,7 +194,7 @@ FixedwingAttitudeControl::vehicle_manual_poll()
-_manual_control_setpoint.x * _param_fw_man_p_sc.get() + _param_trim_pitch.get();
_actuators.control[actuator_controls_s::INDEX_YAW] =
_manual_control_setpoint.r * _param_fw_man_y_sc.get() + _param_trim_yaw.get();
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual_control_setpoint.z;
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f);
}
}
}