Browse Source

Plane: correct compilation when airspeed disabled

apm_2208
Peter Barker 3 years ago committed by Peter Barker
parent
commit
1f5165349e
  1. 2
      ArduPlane/GCS_Mavlink.cpp
  2. 2
      ArduPlane/Parameters.cpp
  3. 1
      ArduPlane/Plane.h
  4. 8
      ArduPlane/is_flying.cpp
  5. 2
      ArduPlane/quadplane.cpp
  6. 6
      ArduPlane/servos.cpp

2
ArduPlane/GCS_Mavlink.cpp

@ -247,9 +247,11 @@ float GCS_MAVLINK_Plane::vfr_hud_airspeed() const
// will use an airspeed sensor, that value is constrained by the // will use an airspeed sensor, that value is constrained by the
// ground speed. When reporting we should send the true airspeed // ground speed. When reporting we should send the true airspeed
// value if possible: // value if possible:
#if AP_AIRSPEED_ENABLED
if (plane.airspeed.enabled() && plane.airspeed.healthy()) { if (plane.airspeed.enabled() && plane.airspeed.healthy()) {
return plane.airspeed.get_airspeed(); return plane.airspeed.get_airspeed();
} }
#endif
// airspeed estimates are OK: // airspeed estimates are OK:
float aspeed; float aspeed;

2
ArduPlane/Parameters.cpp

@ -1492,6 +1492,7 @@ void Plane::load_parameters(void)
} }
#endif #endif
#if AP_AIRSPEED_ENABLED
// PARAMETER_CONVERSION - Added: Jan-2022 // PARAMETER_CONVERSION - Added: Jan-2022
{ {
const uint16_t old_key = g.k_param_airspeed; const uint16_t old_key = g.k_param_airspeed;
@ -1499,6 +1500,7 @@ void Plane::load_parameters(void)
const uint16_t old_top_element = 0; // Old group element in the tree for the first subgroup element (see AP_PARAM_KEY_DUMP) const uint16_t old_top_element = 0; // Old group element in the tree for the first subgroup element (see AP_PARAM_KEY_DUMP)
AP_Param::convert_class(old_key, &airspeed, airspeed.var_info, old_index, old_top_element, true); AP_Param::convert_class(old_key, &airspeed, airspeed.var_info, old_index, old_top_element, true);
} }
#endif
hal.console->printf("load_all took %uus\n", (unsigned)(micros() - before)); hal.console->printf("load_all took %uus\n", (unsigned)(micros() - before));
} }

1
ArduPlane/Plane.h

@ -31,6 +31,7 @@
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AP_Common/AP_Common.h> #include <AP_Common/AP_Common.h>
#include <AP_Airspeed/AP_Airspeed.h>
#include <AP_Param/AP_Param.h> #include <AP_Param/AP_Param.h>
#include <StorageManager/StorageManager.h> #include <StorageManager/StorageManager.h>
#include <AP_Math/AP_Math.h> // ArduPilot Mega Vector/Matrix math Library #include <AP_Math/AP_Math.h> // ArduPilot Mega Vector/Matrix math Library

8
ArduPlane/is_flying.cpp

@ -291,8 +291,14 @@ void Plane::crash_detection_update(void)
// if we have no GPS lock and we don't have a functional airspeed // if we have no GPS lock and we don't have a functional airspeed
// sensor then don't do crash detection // sensor then don't do crash detection
if (gps.status() < AP_GPS::GPS_OK_FIX_3D && (!airspeed.use() || !airspeed.healthy())) { if (gps.status() < AP_GPS::GPS_OK_FIX_3D) {
#if AP_AIRSPEED_ENABLED
if (!airspeed.use() || !airspeed.healthy()) {
crashed = false;
}
#else
crashed = false; crashed = false;
#endif
} }
if (!crashed) { if (!crashed) {

2
ArduPlane/quadplane.cpp

@ -3014,11 +3014,13 @@ bool QuadPlane::verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd)
return false; return false;
} }
#if AP_AIRSPEED_ENABLED
if (is_positive(maximum_takeoff_airspeed) && (plane.airspeed.get_airspeed() > maximum_takeoff_airspeed)) { if (is_positive(maximum_takeoff_airspeed) && (plane.airspeed.get_airspeed() > maximum_takeoff_airspeed)) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Failed to complete takeoff, excessive wind"); gcs().send_text(MAV_SEVERITY_CRITICAL, "Failed to complete takeoff, excessive wind");
plane.set_mode(plane.mode_qland, ModeReason::VTOL_FAILED_TAKEOFF); plane.set_mode(plane.mode_qland, ModeReason::VTOL_FAILED_TAKEOFF);
return false; return false;
} }
#endif
if (plane.current_loc.alt < plane.next_WP_loc.alt) { if (plane.current_loc.alt < plane.next_WP_loc.alt) {
return false; return false;

6
ArduPlane/servos.cpp

@ -133,11 +133,17 @@ bool Plane::suppress_throttle(void)
// if we have an airspeed sensor, then check it too, and // if we have an airspeed sensor, then check it too, and
// require 5m/s. This prevents throttle up due to spiky GPS // require 5m/s. This prevents throttle up due to spiky GPS
// groundspeed with bad GPS reception // groundspeed with bad GPS reception
#if AP_AIRSPEED_ENABLED
if ((!ahrs.airspeed_sensor_enabled()) || airspeed.get_airspeed() >= 5) { if ((!ahrs.airspeed_sensor_enabled()) || airspeed.get_airspeed() >= 5) {
// we're moving at more than 5 m/s // we're moving at more than 5 m/s
throttle_suppressed = false; throttle_suppressed = false;
return false; return false;
} }
#else
// no airspeed sensor, so we trust that the GPS's movement is truthful
throttle_suppressed = false;
return false;
#endif
} }
#if HAL_QUADPLANE_ENABLED #if HAL_QUADPLANE_ENABLED

Loading…
Cancel
Save