Browse Source

AP_RCTelemetry: add define AP_AIRSPEED_ENABLED

gps-1.3.1
Josh Henderson 3 years ago committed by Andrew Tridgell
parent
commit
56a30617e2
  1. 7
      libraries/AP_RCTelemetry/AP_Spektrum_Telem.cpp

7
libraries/AP_RCTelemetry/AP_Spektrum_Telem.cpp

@ -433,13 +433,18 @@ void AP_Spektrum_Telem::calc_airspeed() @@ -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

Loading…
Cancel
Save