mirror of https://github.com/ArduPilot/ardupilot
Plane: move to new `using_airspeed_sensor` ahrs method
This commit is contained in:
parent
19b263a220
commit
a6206bde3c
|
@ -49,7 +49,7 @@ float Plane::calc_speed_scaler(void)
|
|||
// no speed estimate and not armed, use a unit scaling
|
||||
speed_scaler = 1;
|
||||
}
|
||||
if (!plane.ahrs.airspeed_sensor_enabled() &&
|
||||
if (!plane.ahrs.using_airspeed_sensor() &&
|
||||
(plane.flight_option_enabled(FlightOptions::SURPRESS_TKOFF_SCALING)) &&
|
||||
(plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF)) { //scaling is suppressed during climb phase of automatic takeoffs with no airspeed sensor being used due to problems with inaccurate airspeed estimates
|
||||
return MIN(speed_scaler, 1.0f) ;
|
||||
|
|
|
@ -1475,7 +1475,7 @@ bool QuadPlane::should_assist(float aspeed, bool have_airspeed)
|
|||
// assistance due to Q_ASSIST_SPEED
|
||||
// if option bit is enabled only allow assist with real airspeed sensor
|
||||
if ((have_airspeed && aspeed < assist_speed) &&
|
||||
(!option_is_set(QuadPlane::OPTION::DISABLE_SYNTHETIC_AIRSPEED_ASSIST) || ahrs.airspeed_sensor_enabled())) {
|
||||
(!option_is_set(QuadPlane::OPTION::DISABLE_SYNTHETIC_AIRSPEED_ASSIST) || ahrs.using_airspeed_sensor())) {
|
||||
in_angle_assist = false;
|
||||
angle_error_start_ms = 0;
|
||||
return true;
|
||||
|
|
|
@ -196,7 +196,7 @@ void Plane::read_radio()
|
|||
&& channel_throttle->get_control_in() > 50
|
||||
&& stickmixing) {
|
||||
float nudge = (channel_throttle->get_control_in() - 50) * 0.02f;
|
||||
if (ahrs.airspeed_sensor_enabled()) {
|
||||
if (ahrs.using_airspeed_sensor()) {
|
||||
airspeed_nudge_cm = (aparm.airspeed_max * 100 - aparm.airspeed_cruise_cm) * nudge;
|
||||
} else {
|
||||
throttle_nudge = (aparm.throttle_max - aparm.throttle_cruise) * nudge;
|
||||
|
|
|
@ -134,7 +134,7 @@ bool Plane::suppress_throttle(void)
|
|||
// require 5m/s. This prevents throttle up due to spiky GPS
|
||||
// groundspeed with bad GPS reception
|
||||
#if AP_AIRSPEED_ENABLED
|
||||
if ((!ahrs.airspeed_sensor_enabled()) || airspeed.get_airspeed() >= 5) {
|
||||
if ((!ahrs.using_airspeed_sensor()) || airspeed.get_airspeed() >= 5) {
|
||||
// we're moving at more than 5 m/s
|
||||
throttle_suppressed = false;
|
||||
return false;
|
||||
|
@ -605,7 +605,7 @@ void Plane::set_servos_flaps(void)
|
|||
|
||||
if (control_mode->does_auto_throttle()) {
|
||||
int16_t flapSpeedSource = 0;
|
||||
if (ahrs.airspeed_sensor_enabled()) {
|
||||
if (ahrs.using_airspeed_sensor()) {
|
||||
flapSpeedSource = target_airspeed_cm * 0.01f;
|
||||
} else {
|
||||
flapSpeedSource = aparm.throttle_cruise;
|
||||
|
|
|
@ -185,7 +185,7 @@ void Plane::takeoff_calc_pitch(void)
|
|||
return;
|
||||
}
|
||||
|
||||
if (ahrs.airspeed_sensor_enabled()) {
|
||||
if (ahrs.using_airspeed_sensor()) {
|
||||
int16_t takeoff_pitch_min_cd = get_takeoff_pitch_min_cd();
|
||||
calc_nav_pitch();
|
||||
if (nav_pitch_cd < takeoff_pitch_min_cd) {
|
||||
|
|
Loading…
Reference in New Issue