Browse Source

AP_DAL: add define AP_AIRSPEED_ENABLED

gps-1.3.1
Josh Henderson 3 years ago committed by Andrew Tridgell
parent
commit
af348e6ba1
  1. 2
      libraries/AP_DAL/AP_DAL.cpp
  2. 4
      libraries/AP_DAL/AP_DAL_Airspeed.cpp

2
libraries/AP_DAL/AP_DAL.cpp

@ -129,10 +129,12 @@ void AP_DAL::init_sensors(void) @@ -129,10 +129,12 @@ void AP_DAL::init_sensors(void)
alloc_failed |= (_rangefinder = new AP_DAL_RangeFinder) == nullptr;
}
#if AP_AIRSPEED_ENABLED
auto *aspeed = AP::airspeed();
if (aspeed != nullptr && aspeed->get_num_sensors() > 0) {
alloc_failed |= (_airspeed = new AP_DAL_Airspeed) == nullptr;
}
#endif
auto *bcn = AP::beacon();
if (bcn != nullptr && bcn->enabled()) {

4
libraries/AP_DAL/AP_DAL_Airspeed.cpp

@ -5,13 +5,16 @@ @@ -5,13 +5,16 @@
AP_DAL_Airspeed::AP_DAL_Airspeed()
{
#if AP_AIRSPEED_ENABLED
for (uint8_t i=0; i<ARRAY_SIZE(_RASI); i++) {
_RASI[i].instance = i;
}
#endif
}
void AP_DAL_Airspeed::start_frame()
{
#if AP_AIRSPEED_ENABLED
const auto *airspeed = AP::airspeed();
if (airspeed == nullptr) {
return;
@ -31,4 +34,5 @@ void AP_DAL_Airspeed::start_frame() @@ -31,4 +34,5 @@ void AP_DAL_Airspeed::start_frame()
RASI.airspeed = airspeed->get_airspeed(i);
WRITE_REPLAY_BLOCK_IFCHANGED(RASI, RASI, old_RASI);
}
#endif
}

Loading…
Cancel
Save