AP_DAL: don't allocate sensor backends unless needed

this saves memory on boards without those sensors

libraries/AP_DAL/AP_DAL.cpp

@@ -22,6 +22,10 @@ void AP_DAL::start_frame(AP_DAL::FrameType frametype)
 {
 #if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
 
+    if (!init_done) {
+        init_sensors();
+    }
+
     const AP_AHRS &ahrs = AP::ahrs();
 
     const uint32_t imu_us = AP::ins().get_last_update_usec();
@@ -54,8 +58,6 @@ void AP_DAL::start_frame(AP_DAL::FrameType frametype)
     _RFRN.lng = _home.lng;
     _RFRN.alt = _home.alt;
     _RFRN.get_compass_is_null = AP::ahrs().get_compass() == nullptr;
-    _RFRN.rangefinder_ptr_is_null = AP::rangefinder() == nullptr;
-    _RFRN.airspeed_ptr_is_null = AP::airspeed() == nullptr;
     _RFRN.EAS2TAS = AP::baro().get_EAS2TAS();
     _RFRN.vehicle_class = ahrs.get_vehicle_class();
     _RFRN.fly_forward = ahrs.get_fly_forward();
@@ -72,11 +74,19 @@ void AP_DAL::start_frame(AP_DAL::FrameType frametype)
     _baro.start_frame();
     _gps.start_frame();
     _compass.start_frame();
-    _airspeed.start_frame();
-    _rangefinder.start_frame();
-    _beacon.start_frame();
+    if (_airspeed) {
+        _airspeed->start_frame();
+    }
+    if (_rangefinder) {
+        _rangefinder->start_frame();
+    }
+    if (_beacon) {
+        _beacon->start_frame();
+    }
 #if HAL_VISUALODOM_ENABLED
-    _visualodom.start_frame();
+    if (_visualodom) {
+        _visualodom->start_frame();
+    }
 #endif
 
     // populate some derivative values:
@@ -87,6 +97,46 @@ void AP_DAL::start_frame(AP_DAL::FrameType frametype)
 #endif
 }
 
+/*
+  setup optional sensor backends
+ */
+void AP_DAL::init_sensors(void)
+{
+    init_done = true;
+    bool alloc_failed = false;
+
+    /*
+      we only allocate the DAL backends if we had at least one sensor
+      at the time we startup the EKF
+     */
+
+    auto *rangefinder = AP::rangefinder();
+    if (rangefinder && rangefinder->num_sensors() > 0) {
+        alloc_failed |= (_rangefinder = new AP_DAL_RangeFinder) == nullptr;
+    }
+
+    auto *airspeed = AP::airspeed();
+    if (airspeed != nullptr && airspeed->get_num_sensors() > 0) {
+        alloc_failed |= (_airspeed = new AP_DAL_Airspeed) == nullptr;
+    }
+
+    auto *beacon = AP::beacon();
+    if (beacon != nullptr && beacon->enabled()) {
+        alloc_failed |= (_beacon = new AP_DAL_Beacon) == nullptr;
+    }
+
+#if HAL_VISUALODOM_ENABLED
+    auto *visualodom = AP::visualodom();
+    if (visualodom != nullptr && visualodom->enabled()) {
+        alloc_failed |= (_visualodom = new AP_DAL_VisualOdom) == nullptr;
+    }
+#endif
+
+    if (alloc_failed) {
+        AP_BoardConfig::config_error("Unable to allocate DAL backends");
+    }
+}
+
 /*
   end a frame. Must be called on all events and injections of data (eg
   flow) and before starting a new frame

libraries/AP_DAL/AP_DAL.h

@@ -121,24 +121,19 @@ public:
     AP_DAL_InertialSensor &ins() { return _ins; }
     AP_DAL_Baro &baro() { return _baro; }
     AP_DAL_GPS &gps() { return _gps; }
+
     AP_DAL_RangeFinder *rangefinder() {
-        if (_RFRN.rangefinder_ptr_is_null) {
-            return nullptr;
-        }
-        return &_rangefinder;
+        return _rangefinder;
     }
     AP_DAL_Airspeed *airspeed() {
-        if (_RFRN.airspeed_ptr_is_null) {
-            return nullptr;
-        }
-        return &_airspeed;
+        return _airspeed;
     }
     AP_DAL_Beacon *beacon() {
-        return _beacon.beacon();
+        return _beacon;
     }
 #if HAL_VISUALODOM_ENABLED
     AP_DAL_VisualOdom *visualodom() {
-        return _visualodom.visualodom();
+        return _visualodom;
     }
 #endif
 
@@ -230,10 +225,16 @@ public:
     }
 
     void handle_message(const log_RASH &msg) {
-        _airspeed.handle_message(msg);
+        if (_airspeed == nullptr) {
+            _airspeed = new AP_DAL_Airspeed;
+        }
+        _airspeed->handle_message(msg);
     }
     void handle_message(const log_RASI &msg) {
-        _airspeed.handle_message(msg);
+        if (_airspeed == nullptr) {
+            _airspeed = new AP_DAL_Airspeed;
+        }
+        _airspeed->handle_message(msg);
     }
 
     void handle_message(const log_RBRH &msg) {
@@ -244,10 +245,16 @@ public:
     }
 
     void handle_message(const log_RRNH &msg) {
-        _rangefinder.handle_message(msg);
+        if (_rangefinder == nullptr) {
+            _rangefinder = new AP_DAL_RangeFinder;
+        }
+        _rangefinder->handle_message(msg);
     }
     void handle_message(const log_RRNI &msg) {
-        _rangefinder.handle_message(msg);
+        if (_rangefinder == nullptr) {
+            _rangefinder = new AP_DAL_RangeFinder;
+        }
+        _rangefinder->handle_message(msg);
     }
 
     void handle_message(const log_RGPH &msg) {
@@ -268,14 +275,23 @@ public:
     }
 
     void handle_message(const log_RBCH &msg) {
-        _beacon.handle_message(msg);
+        if (_beacon == nullptr) {
+            _beacon = new AP_DAL_Beacon;
+        }
+        _beacon->handle_message(msg);
     }
     void handle_message(const log_RBCI &msg) {
-        _beacon.handle_message(msg);
+        if (_beacon == nullptr) {
+            _beacon = new AP_DAL_Beacon;
+        }
+        _beacon->handle_message(msg);
     }
     void handle_message(const log_RVOH &msg) {
 #if HAL_VISUALODOM_ENABLED
-        _visualodom.handle_message(msg);
+        if (_visualodom == nullptr) {
+            _visualodom = new AP_DAL_VisualOdom;
+        }
+        _visualodom->handle_message(msg);
 #endif
     }
     void handle_message(const log_ROFH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
@@ -318,12 +334,12 @@ private:
     AP_DAL_InertialSensor _ins;
     AP_DAL_Baro _baro;
     AP_DAL_GPS _gps;
-    AP_DAL_RangeFinder _rangefinder;
+    AP_DAL_RangeFinder *_rangefinder;
     AP_DAL_Compass _compass;
-    AP_DAL_Airspeed _airspeed;
-    AP_DAL_Beacon _beacon;
+    AP_DAL_Airspeed *_airspeed;
+    AP_DAL_Beacon *_beacon;
 #if HAL_VISUALODOM_ENABLED
-    AP_DAL_VisualOdom _visualodom;
+    AP_DAL_VisualOdom *_visualodom;
 #endif
 
     static bool logging_started;
@@ -331,6 +347,9 @@ private:
 
     bool ekf2_init_done;
     bool ekf3_init_done;
+
+    void init_sensors(void);
+    bool init_done;
 };
 
 #define WRITE_REPLAY_BLOCK(sname,v) AP_DAL::WriteLogMessage(LOG_## sname ##_MSG, &v, nullptr, offsetof(log_ ##sname, _end))

libraries/AP_DAL/AP_DAL_Beacon.cpp

@@ -17,7 +17,6 @@ void AP_DAL_Beacon::start_frame()
     const auto *bcon = AP::beacon();
 
     const log_RBCH old = _RBCH;
-    _RBCH.ptr_is_nullptr = (bcon == nullptr);
     if (bcon != nullptr) {
         _RBCH.count = bcon->count();
         _RBCH.get_vehicle_position_ned_returncode = bcon->get_vehicle_position_ned(_RBCH.vehicle_position_ned, _RBCH.accuracy_estimate);

libraries/AP_DAL/AP_DAL_Beacon.h

@@ -58,9 +58,6 @@ public:
     AP_DAL_Beacon();
 
     AP_DAL_Beacon *beacon() {
-        if (_RBCH.ptr_is_nullptr) {
-            return nullptr;
-        }
         return this;
     }
 

libraries/AP_DAL/AP_DAL_VisualOdom.cpp

@@ -25,7 +25,6 @@ void AP_DAL_VisualOdom::start_frame()
     const auto *vo = AP::visualodom();
 
     const log_RVOH old = RVOH;
-    RVOH.ptr_is_nullptr = (vo == nullptr);
     if (vo != nullptr) {
         RVOH.pos_offset = vo->get_pos_offset();
         RVOH.delay_ms = vo->get_delay_ms();

libraries/AP_DAL/AP_DAL_VisualOdom.h

@@ -33,13 +33,6 @@ public:
     // should only be called when this library is not being used as the position source
     void align_position_to_ahrs(bool align_xy, bool align_z);
 
-    AP_DAL_VisualOdom *visualodom() {
-        if (RVOH.ptr_is_nullptr) {
-            return nullptr;
-        }
-        return this;
-    }
-
     void start_frame();
 
     void handle_message(const log_RVOH &msg) {

libraries/AP_DAL/LogStructure.h

@@ -62,9 +62,7 @@ struct log_RFRN {
     uint8_t vehicle_class;
     uint8_t ekf_type;
     uint8_t armed:1;
-    uint8_t rangefinder_ptr_is_null:1;
     uint8_t get_compass_is_null:1;
-    uint8_t airspeed_ptr_is_null:1;
     uint8_t fly_forward:1;
     uint8_t ahrs_airspeed_sensor_enabled:1;
     uint8_t opticalflow_enabled:1;
@@ -269,7 +267,6 @@ struct log_RBCH {
     uint8_t get_vehicle_position_ned_returncode:1;
     uint8_t get_origin_returncode:1;
     uint8_t enabled:1;
-    uint8_t ptr_is_nullptr:1;
     uint8_t count;
     uint8_t _end;
 };
@@ -292,7 +289,6 @@ struct log_RVOH {
     uint32_t delay_ms;
     uint8_t healthy;
     bool enabled;
-    uint8_t ptr_is_nullptr;
     uint8_t _end;
 };
 
@@ -408,7 +404,7 @@ struct log_RBOH {
     { LOG_RBCI_MSG, RLOG_SIZE(RBCI),                                   \
       "RBCI", "IffffBB", "LU,PX,PY,PZ,Dist,H,I", "smmmm-#", "?0000--" }, \
     { LOG_RVOH_MSG, RLOG_SIZE(RVOH),                                   \
-      "RVOH", "fffIBBB", "OX,OY,OZ,Del,H,Ena,NPtr", "-------", "-------" }, \
+      "RVOH", "fffIBB", "OX,OY,OZ,Del,H,Ena", "------", "------" }, \
     { LOG_ROFH_MSG, RLOG_SIZE(ROFH),                                   \
       "ROFH", "ffffIfffB", "FX,FY,GX,GY,Tms,PX,PY,PZ,Qual", "---------", "---------" }, \
     { LOG_REPH_MSG, RLOG_SIZE(REPH),                                   \