Plane: always setup target airspeed

this sets up a target airspeed even when flying without an airspeed
sensor. This is needed for quadplanes without airspeed sensors where
we use synthetic airspeed during the transition in TECS
This commit is contained in:
Andrew Tridgell 2017-07-31 19:58:19 +10:00
parent 4064a23b30
commit 5c10d81f60
2 changed files with 14 additions and 6 deletions

View File

@ -101,8 +101,11 @@ void Plane::navigate()
void Plane::calc_airspeed_errors()
{
float airspeed_measured_cm = airspeed.get_airspeed_cm();
float airspeed_measured = 0;
// we use the airspeed estimate function not direct sensor as TECS
// may be using synthetic airspeed
ahrs.airspeed_estimate(&airspeed_measured);
// FBW_B airspeed target
if (control_mode == FLY_BY_WIRE_B ||
@ -125,10 +128,11 @@ void Plane::calc_airspeed_errors()
// but only when this is faster than the target airspeed commanded
// above.
if (control_mode >= FLY_BY_WIRE_B && (aparm.min_gndspeed_cm > 0)) {
int32_t min_gnd_target_airspeed = airspeed_measured_cm + groundspeed_undershoot;
if (min_gnd_target_airspeed > target_airspeed_cm)
int32_t min_gnd_target_airspeed = airspeed_measured*100 + groundspeed_undershoot;
if (min_gnd_target_airspeed > target_airspeed_cm) {
target_airspeed_cm = min_gnd_target_airspeed;
}
}
// Bump up the target airspeed based on throttle nudging
if (control_mode >= AUTO && airspeed_nudge_cm > 0) {
@ -141,7 +145,7 @@ void Plane::calc_airspeed_errors()
// use the TECS view of the target airspeed for reporting, to take
// account of the landing speed
airspeed_error = SpdHgt_Controller->get_target_airspeed() - airspeed_measured_cm * 0.01f;
airspeed_error = SpdHgt_Controller->get_target_airspeed() - airspeed_measured;
}
void Plane::calc_gndspeed_undershoot()

View File

@ -85,7 +85,6 @@ void Plane::read_airspeed(void)
if (should_log(MASK_LOG_IMU)) {
Log_Write_Airspeed();
}
calc_airspeed_errors();
// supply a new temperature to the barometer from the digital
// airspeed sensor if we can
@ -95,6 +94,11 @@ void Plane::read_airspeed(void)
}
}
// we calculate airspeed errors (and thus target_airspeed_cm) even
// when airspeed is disabled as TECS may be using synthetic
// airspeed for a quadplane transition
calc_airspeed_errors();
// update smoothed airspeed estimate
float aspeed;
if (ahrs.airspeed_estimate(&aspeed)) {