From 56a30617e24f2ab4b88bab2d551d6a6e9983a791 Mon Sep 17 00:00:00 2001 From: Josh Henderson Date: Mon, 1 Nov 2021 04:13:20 -0400 Subject: [PATCH] AP_RCTelemetry: add define AP_AIRSPEED_ENABLED --- libraries/AP_RCTelemetry/AP_Spektrum_Telem.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/libraries/AP_RCTelemetry/AP_Spektrum_Telem.cpp b/libraries/AP_RCTelemetry/AP_Spektrum_Telem.cpp index d6f22489a7..44ab799ce1 100644 --- a/libraries/AP_RCTelemetry/AP_Spektrum_Telem.cpp +++ b/libraries/AP_RCTelemetry/AP_Spektrum_Telem.cpp @@ -433,13 +433,18 @@ void AP_Spektrum_Telem::calc_airspeed() AP_AHRS &ahrs = AP::ahrs(); WITH_SEMAPHORE(ahrs.get_semaphore()); - const AP_Airspeed *airspeed = AP::airspeed(); float speed = 0.0f; +#if AP_AIRSPEED_ENABLED + const AP_Airspeed *airspeed = AP::airspeed(); if (airspeed && airspeed->healthy()) { speed = roundf(airspeed->get_airspeed() * 3.6); } else { speed = roundf(AP::ahrs().groundspeed() * 3.6); } +#else + speed = roundf(AP::ahrs().groundspeed() * 3.6); +#endif + _telem.speed.airspeed = htobe16(uint16_t(speed)); // 1 km/h increments _max_speed = MAX(speed, _max_speed); _telem.speed.maxAirspeed = htobe16(uint16_t(_max_speed)); // 1 km/h increments