mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 18:38:28 -04:00
APM: use ahrs.airspeed_estimate() in two more places
this applies the wind constraint
This commit is contained in:
parent
8f70a24fd4
commit
4219cb55ce
@ -1091,12 +1091,15 @@ static void update_current_flight_mode(void)
|
||||
nav_pitch_cd = constrain(nav_pitch_cd, 500, takeoff_pitch_cd);
|
||||
}
|
||||
|
||||
// don't use a pitch/roll integrators during takeoff if we are
|
||||
// below minimum speed
|
||||
#if APM_CONTROL == DISABLED
|
||||
if (airspeed.use() && airspeed.get_airspeed() < g.flybywire_airspeed_min) {
|
||||
g.pidServoPitch.reset_I();
|
||||
g.pidServoRoll.reset_I();
|
||||
float aspeed;
|
||||
if (ahrs.airspeed_estimate(&aspeed)) {
|
||||
// don't use a pitch/roll integrators during takeoff if we are
|
||||
// below minimum speed
|
||||
if (aspeed < g.flybywire_airspeed_min) {
|
||||
g.pidServoPitch.reset_I();
|
||||
g.pidServoRoll.reset_I();
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
|
@ -220,9 +220,7 @@ static void calc_nav_roll()
|
||||
|
||||
// Use airspeed_cruise as an analogue for airspeed if we don't have airspeed.
|
||||
float speed;
|
||||
if(airspeed.use()) {
|
||||
speed = airspeed.get_airspeed();
|
||||
} else {
|
||||
if (!ahrs.airspeed_estimate(&speed)) {
|
||||
speed = g.airspeed_cruise_cm*0.01;
|
||||
|
||||
// Floor the speed so that the user can't enter a bad value
|
||||
|
Loading…
Reference in New Issue
Block a user