aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: move target land_airspeed logic to top layer

Browse Source 
- this is to allow min groundspeed to be enforced which is otherwise lost when TECS assigns a target airspeed at the lower level
This commit is contained in:
Tom Pittenger 2016-04-13 09:12:26 -07:00
parent 0af878703f
commit ec3e9014e4
1 changed files with 25 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
ArduPlane/navigation.cpp
View File

@ -98,6 +98,31 @@ void Plane::calc_airspeed_errors()
((int32_t)aparm.airspeed_min * 100);
}
// Landing airspeed target
if (control_mode == AUTO && ahrs.airspeed_sensor_enabled()) {
float land_airspeed = SpdHgt_Controller->get_land_airspeed();
switch (flight_stage) {
case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
if (land_airspeed >= 0) {
target_airspeed_cm = land_airspeed * 100;
}
break;
case AP_SpdHgtControl::FLIGHT_LAND_PREFLARE:
case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
if (auto_state.land_pre_flare && aparm.land_pre_flare_airspeed > 0) {
// if we just preflared then continue using the pre-flare airspeed during final flare
target_airspeed_cm = aparm.land_pre_flare_airspeed * 100;
} else if (land_airspeed >= 0) {
target_airspeed_cm = land_airspeed * 100;
}
break;
default:
break;
}
}
// Set target to current airspeed + ground speed undershoot,
// but only when this is faster than the target airspeed commanded
// above.