diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-08-23 12:13:30 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-08-23 12:13:30 +0200 |
commit | 9f5004be2d282a0bb8f2618545d0c6174df2e29a (patch) | |
tree | e47b679a214872420c4cc88a7d6de51f92e2da41 | |
parent | 40d47fb069cb60303102f3bfba21f6dc5e0aa5a9 (diff) | |
download | px4-firmware-9f5004be2d282a0bb8f2618545d0c6174df2e29a.tar.gz px4-firmware-9f5004be2d282a0bb8f2618545d0c6174df2e29a.tar.bz2 px4-firmware-9f5004be2d282a0bb8f2618545d0c6174df2e29a.zip |
fw pos control: set default roll, pitch while waiting for launch
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 8 |
1 files changed, 7 insertions, 1 deletions
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 6dd458516..9cbe5483b 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1143,7 +1143,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi TECS_MODE_TAKEOFF); /* limit roll motion to ensure enough lift */ - _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), + math::radians(15.0f)); } else { tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, @@ -1159,6 +1160,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _global_pos.alt, ground_speed); } + } else { + /* Set default roll and pitch setpoints during detection phase */ + _att_sp.roll_body = 0.0f; + _att_sp.pitch_body = math::max(math::radians(pos_sp_triplet.current.pitch_min), + math::radians(10.0f)); } } |