diff options
| author | Johan Jansen <jnsn.johan@gmail.com> | 2015-02-16 14:48:49 +0100 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2015-02-18 16:56:53 +0100 |
| commit | 51fcb440d0c416f1689cf4f80fea4e05f45b3c14 (patch) | |
| tree | 114c752ca78634369275af103ff45299d121301d | |
| parent | 3f45f51d7a4cc988e000473916a7ddeb22beaebe (diff) | |
| download | px4-firmware-51fcb440d0c416f1689cf4f80fea4e05f45b3c14.tar.gz px4-firmware-51fcb440d0c416f1689cf4f80fea4e05f45b3c14.tar.bz2 px4-firmware-51fcb440d0c416f1689cf4f80fea4e05f45b3c14.zip | |
mc_pos_control: Fix autonomous landing without GPS
Due to a regression bug in #1741 the autonomous landing without GPS used manual RC
input to determine setpoints which broke auto landing without GPS.
| -rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 31 |
1 files changed, 17 insertions, 14 deletions
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 0d9cc8157..5f76b714c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1351,18 +1351,25 @@ MulticopterPositionControl::task_main() } if(!_control_mode.flag_control_velocity_enabled) { + /* generate attitude setpoint from manual controls */ + if(_control_mode.flag_control_manual_enabled) { + /* move yaw setpoint */ + float yaw_sp_move_rate = _manual.r * _params.man_yaw_max; + _att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + yaw_sp_move_rate * dt); + float yaw_offs_max = _params.man_yaw_max / _params.mc_att_yaw_p; + float yaw_offs = _wrap_pi(_att_sp.yaw_body - _att.yaw); + if (yaw_offs < - yaw_offs_max) { + _att_sp.yaw_body = _wrap_pi(_att.yaw - yaw_offs_max); + + } else if (yaw_offs > yaw_offs_max) { + _att_sp.yaw_body = _wrap_pi(_att.yaw + yaw_offs_max); + } - /* move yaw setpoint */ - float yaw_sp_move_rate = _manual.r * _params.man_yaw_max; - _att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + yaw_sp_move_rate * dt); - float yaw_offs_max = _params.man_yaw_max / _params.mc_att_yaw_p; - float yaw_offs = _wrap_pi(_att_sp.yaw_body - _att.yaw); - if (yaw_offs < - yaw_offs_max) { - _att_sp.yaw_body = _wrap_pi(_att.yaw - yaw_offs_max); - - } else if (yaw_offs > yaw_offs_max) { - _att_sp.yaw_body = _wrap_pi(_att.yaw + yaw_offs_max); + _att_sp.roll_body = _manual.y * _params.man_roll_max; + _att_sp.pitch_body = -_manual.x * _params.man_pitch_max; + _att_sp.yaw_sp_move_rate = yaw_sp_move_rate; + _att_sp.thrust = _control_mode.flag_control_altitude_enabled ? _att_sp.thrust : _manual.z; } /* reset yaw setpoint to current position if needed */ @@ -1371,10 +1378,6 @@ MulticopterPositionControl::task_main() _att_sp.yaw_body = _att.yaw; } - _att_sp.roll_body = _manual.y * _params.man_roll_max; - _att_sp.pitch_body = -_manual.x * _params.man_pitch_max; - _att_sp.thrust = _control_mode.flag_control_altitude_enabled ? _att_sp.thrust : _manual.z; - _att_sp.yaw_sp_move_rate = yaw_sp_move_rate; math::Matrix<3,3> R_sp; R_sp.from_euler(_att_sp.roll_body,_att_sp.pitch_body,_att_sp.yaw_body); memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body)); |