mirror of https://github.com/ArduPilot/ardupilot
APM: allow for landing pitch control until final stage
This commit is contained in:
parent
7a49515ee3
commit
972b8df052
|
@ -1111,11 +1111,16 @@ static void update_current_flight_mode(void)
|
||||||
nav_roll_cd = 0;
|
nav_roll_cd = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!alt_control_airspeed() || land_complete) {
|
if (land_complete) {
|
||||||
// hold pitch constant in final approach
|
// hold pitch constant in final approach
|
||||||
nav_pitch_cd = g.land_pitch_cd;
|
nav_pitch_cd = g.land_pitch_cd;
|
||||||
} else {
|
} else {
|
||||||
calc_nav_pitch();
|
calc_nav_pitch();
|
||||||
|
if (!alt_control_airspeed()) {
|
||||||
|
// when not under airspeed control, don't allow
|
||||||
|
// down pitch in landing
|
||||||
|
nav_pitch_cd = constrain(nav_pitch_cd, 0, nav_pitch_cd);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
calc_throttle();
|
calc_throttle();
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue