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

fw_att_control move angular rate limits to body angular rates (#9023)

Browse Source 
* ecl: attitude_fw update
- move angular rate limits to body angular rates
- handle rattitude/acro vs attitude mode rate limits
This commit is contained in:
Thomas Stastny
2018-03-08 22:55:45 -05:00
committed by Daniel Agar
parent 2ce8b6a984
commit 74730273c1
3 changed files with 21 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/ecl
+1 -1
Submodule src/lib/ecl updated: 78e983073a...041886a289
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+18 -4
View File
@@ -199,8 +199,6 @@ FixedwingAttitudeControl::parameters_update()
_pitch_ctrl.set_k_i(_parameters.p_i);
_pitch_ctrl.set_k_ff(_parameters.p_ff);
_pitch_ctrl.set_integrator_max(_parameters.p_integrator_max);
_pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos));
_pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg));
/* roll control parameters */
_roll_ctrl.set_time_constant(_parameters.r_tc);
@@ -208,14 +206,12 @@ FixedwingAttitudeControl::parameters_update()
_roll_ctrl.set_k_i(_parameters.r_i);
_roll_ctrl.set_k_ff(_parameters.r_ff);
_roll_ctrl.set_integrator_max(_parameters.r_integrator_max);
_roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax));
/* yaw control parameters */
_yaw_ctrl.set_k_p(_parameters.y_p);
_yaw_ctrl.set_k_i(_parameters.y_i);
_yaw_ctrl.set_k_ff(_parameters.y_ff);
_yaw_ctrl.set_integrator_max(_parameters.y_integrator_max);
_yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax));
/* wheel control parameters */
_wheel_ctrl.set_k_p(_parameters.w_p);
@@ -638,6 +634,24 @@ void FixedwingAttitudeControl::run()
control_input.groundspeed = groundspeed;
control_input.groundspeed_scaler = groundspeed_scaler;
/* reset body angular rate limits on mode change */
if ((_vcontrol_mode.flag_control_attitude_enabled != _flag_control_attitude_enabled_last) || params_updated) {
if (_vcontrol_mode.flag_control_attitude_enabled) {
_roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax));
_pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos));
_pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg));
_yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax));
} else {
_roll_ctrl.set_max_rate(_parameters.acro_max_x_rate_rad);
_pitch_ctrl.set_max_rate_pos(_parameters.acro_max_y_rate_rad);
_pitch_ctrl.set_max_rate_neg(_parameters.acro_max_y_rate_rad);
_yaw_ctrl.set_max_rate(_parameters.acro_max_z_rate_rad);
}
}
_flag_control_attitude_enabled_last = _vcontrol_mode.flag_control_attitude_enabled;
/* Run attitude controllers */
if (_vcontrol_mode.flag_control_attitude_enabled) {
if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) {
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
+2
View File
@@ -137,6 +137,8 @@ private:
float _battery_scale{1.0f};
bool _flag_control_attitude_enabled_last{false};
struct {
float p_tc;
float p_p;