diff options
| author | Thomas Gubler <thomasgubler@gmail.com> | 2014-09-08 13:01:02 +0200 |
|---|---|---|
| committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-09-08 13:01:02 +0200 |
| commit | 98d643fdf85e4cc49bf3f5e4bb1195839f7d67fa (patch) | |
| tree | 0829922df848cb683926ec6a5fb93512aefcaac1 /src/modules/fw_pos_control_l1 | |
| parent | 33c0cd2e569782ed68931b6ffd6be4a45c6b1b54 (diff) | |
| parent | df181455ebdcfacda73615adba40fa224b4d074c (diff) | |
| download | px4-firmware-98d643fdf85e4cc49bf3f5e4bb1195839f7d67fa.tar.gz px4-firmware-98d643fdf85e4cc49bf3f5e4bb1195839f7d67fa.tar.bz2 px4-firmware-98d643fdf85e4cc49bf3f5e4bb1195839f7d67fa.zip | |
Merge branch 'launchpitchlimit' into launchpitchlimit_swissfang
Diffstat (limited to 'src/modules/fw_pos_control_l1')
| -rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 20 |
1 files changed, 15 insertions, 5 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 86fde2b38..23167eef4 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 @@ -383,7 +383,8 @@ private: bool climbout_mode, float climbout_pitch_min_rad, float altitude, const math::Vector<3> &ground_speed, - tecs_mode mode = TECS_MODE_NORMAL); + tecs_mode mode = TECS_MODE_NORMAL, + bool pitch_max_special = false); }; @@ -1122,8 +1123,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* select maximum pitch: the launchdetector may impose another limit for the pitch * depending on the state of the launch */ - float takeoff_pitch_max_rad = math::radians( - launchDetector.getPitchMax(_parameters.pitch_limit_max)); + float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max); + float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); /* apply minimum pitch and limit roll if target altitude is not within climbout_diff * meters */ @@ -1142,7 +1143,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::radians(10.0f)), _global_pos.alt, ground_speed, - TECS_MODE_TAKEOFF); + TECS_MODE_TAKEOFF, + takeoff_pitch_max_deg != _parameters.pitch_limit_max); /* limit roll motion to ensure enough lift */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), @@ -1396,7 +1398,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ bool climbout_mode, float climbout_pitch_min_rad, float altitude, const math::Vector<3> &ground_speed, - tecs_mode mode) + tecs_mode mode, bool pitch_max_special) { if (_mTecs.getEnabled()) { /* Using mtecs library: prepare arguments for mtecs call */ @@ -1411,6 +1413,14 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ } else { limitOverride.disablePitchMinOverride(); } + + if (pitch_max_special) { + /* Use the maximum pitch from the argument */ + limitOverride.enablePitchMaxOverride(M_RAD_TO_DEG_F * pitch_max_rad); + } else { + /* use pitch max set by MT param */ + limitOverride.disablePitchMaxOverride(); + } _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, limitOverride); } else { |