diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-08-25 12:48:14 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-08-25 12:48:14 +0200 |
commit | c591444c1e2e4124d28a03653c0ccdbcd305c1c2 (patch) | |
tree | 7506e0824346519402427a8366fb6966170c8b64 | |
parent | cfbbe60291028c60a5dd0a8f2b8bad265a7ee839 (diff) | |
download | px4-firmware-c591444c1e2e4124d28a03653c0ccdbcd305c1c2.tar.gz px4-firmware-c591444c1e2e4124d28a03653c0ccdbcd305c1c2.tar.bz2 px4-firmware-c591444c1e2e4124d28a03653c0ccdbcd305c1c2.zip |
fw pos control: set pitch sp correctly while waiting for launch
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 10 |
1 files changed, 8 insertions, 2 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 da81b0dba..b3a78ad82 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 @@ -1192,7 +1192,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } /* Copy thrust and pitch values from tecs - * making sure again that the correct thrust is used, without depending on library calls */ + * making sure again that the correct thrust is used, + * without depending on library calls */ if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); @@ -1200,7 +1201,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi else { _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : _tecs.get_throttle_demand(), throttle_max); } - _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); + /* During a takeoff waypoint while waiting for launch the pitch sp is set + * already (not by tecs) */ + if (!(pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && + launch_detection_state == LAUNCHDETECTION_RES_NONE)) { + _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); + } if (_control_mode.flag_control_position_enabled) { last_manual = false; |