diff options
| author | Thomas Gubler <thomasgubler@gmail.com> | 2014-05-28 19:45:41 +0200 |
|---|---|---|
| committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-05-28 19:45:41 +0200 |
| commit | eed1b685373ee8e81a182ade937e25f2f7b3a3fe (patch) | |
| tree | eda51ed9ed457ad99e04e96f70bf9bd46f663e87 /src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | |
| parent | ad2fccdf9b48e5e619a11e6a8ce88eed0b914098 (diff) | |
| parent | bfb27ff7bd27e3c89eb0eebc923c9ab7bd7c04fe (diff) | |
| download | px4-firmware-eed1b685373ee8e81a182ade937e25f2f7b3a3fe.tar.gz px4-firmware-eed1b685373ee8e81a182ade937e25f2f7b3a3fe.tar.bz2 px4-firmware-eed1b685373ee8e81a182ade937e25f2f7b3a3fe.zip | |
Merge branch 'mtecs_takeofflimits' into mtecs
Diffstat (limited to 'src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp')
| -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, |