diff options
| author | Thomas Gubler <thomasgubler@gmail.com> | 2014-05-28 00:14:56 +0200 |
|---|---|---|
| committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-05-28 00:14:56 +0200 |
| commit | bfb27ff7bd27e3c89eb0eebc923c9ab7bd7c04fe (patch) | |
| tree | 3d3994da36c8b1e10ae56f3638ba3afc38e5adb5 /src | |
| parent | 36c938a187e63fa5ec6d72b9ac5628334b23b837 (diff) | |
| download | px4-firmware-bfb27ff7bd27e3c89eb0eebc923c9ab7bd7c04fe.tar.gz px4-firmware-bfb27ff7bd27e3c89eb0eebc923c9ab7bd7c04fe.tar.bz2 px4-firmware-bfb27ff7bd27e3c89eb0eebc923c9ab7bd7c04fe.zip | |
fw pos control: set takeoff min pitch for mtecs
Diffstat (limited to 'src')
| -rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 11 |
1 files changed, 10 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 e3b6eb261..e43469b70 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 @@ -1452,13 +1452,22 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ fwPosctrl::mTecs::tecs_mode mode) { if (_mTecs.getEnabled()) { + /* Using mtecs library: prepare arguments for mtecs call */ float flightPathAngle = 0.0f; float ground_speed_length = ground_speed.length(); if (ground_speed_length > FLT_EPSILON) { flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); } - _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode); + fwPosctrl::mTecs::LimitOverride limitOverride; + if (climbout_mode) { + limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); + } else { + limitOverride.disablePitchMinOverride(); + } + _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, + limitOverride); } else { + /* Using tecs library */ _tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp, _airspeed.indicated_airspeed_m_s, eas2tas, climbout_mode, climbout_pitch_min_rad, |