fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 07:33:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'launchpitchlimit' into launchpitchlimit_swissfang

Browse Source
This commit is contained in:
Thomas Gubler
2014-09-08 13:01:02 +02:00
parent 33c0cd2e56 df181455eb
commit 98d643fdf8
1 changed files with 15 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+15 -5
View File
@@ -383,7 +383,8 @@ private:
bool climbout_mode, float climbout_pitch_min_rad, bool climbout_mode, float climbout_pitch_min_rad,
float altitude, float altitude,
const math::Vector<3> &ground_speed, 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> &current_positi
/* select maximum pitch: the launchdetector may impose another limit for the pitch /* select maximum pitch: the launchdetector may impose another limit for the pitch
* depending on the state of the launch */ * depending on the state of the launch */
float takeoff_pitch_max_rad = math::radians( float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max);
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 /* apply minimum pitch and limit roll if target altitude is not within climbout_diff
* meters */ * meters */
@@ -1142,7 +1143,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
math::radians(10.0f)), math::radians(10.0f)),
_global_pos.alt, _global_pos.alt,
ground_speed, ground_speed,
TECS_MODE_TAKEOFF); TECS_MODE_TAKEOFF,
takeoff_pitch_max_deg != _parameters.pitch_limit_max);
/* limit roll motion to ensure enough lift */ /* limit roll motion to ensure enough lift */
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), _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, bool climbout_mode, float climbout_pitch_min_rad,
float altitude, float altitude,
const math::Vector<3> &ground_speed, const math::Vector<3> &ground_speed,
tecs_mode mode) tecs_mode mode, bool pitch_max_special)
{ {
if (_mTecs.getEnabled()) { if (_mTecs.getEnabled()) {
/* Using mtecs library: prepare arguments for mtecs call */ /* Using mtecs library: prepare arguments for mtecs call */
@@ -1411,6 +1413,14 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
} else { } else {
limitOverride.disablePitchMinOverride(); 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, _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
limitOverride); limitOverride);
} else { } else {