AP_DAL: only allocate rangefinder backends that we need

libraries/AP_DAL/AP_DAL_RangeFinder.cpp

@@ -4,17 +4,31 @@
 
 #include <AP_RangeFinder/AP_RangeFinder_Backend.h>
 #include "AP_DAL.h"
+#include <AP_BoardConfig/AP_BoardConfig.h>
 
 AP_DAL_RangeFinder::AP_DAL_RangeFinder()
 {
-    for (uint8_t i=0; i<ARRAY_SIZE(_RRNI); i++) {
+#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
+    _RRNH.num_sensors = AP::rangefinder()->num_sensors();
+    _RRNI = new log_RRNI[_RRNH.num_sensors];
+    _backend = new AP_DAL_RangeFinder_Backend *[_RRNH.num_sensors];
+    if (!_RRNI || !_backend) {
+        goto failed;
+    }
+    for (uint8_t i=0; i<_RRNH.num_sensors; i++) {
         _RRNI[i].instance = i;
     }
-
-    for (uint8_t i=0; i<RANGEFINDER_MAX_INSTANCES; i++) {
+    for (uint8_t i=0; i<_RRNH.num_sensors; i++) {
         // this avoids having to discard a const....
         _backend[i] = new AP_DAL_RangeFinder_Backend(_RRNI[i]);
+        if (!_backend[i]) {
+            goto failed;
+        }
     }
+    return;
+failed:
+    AP_BoardConfig::config_error("Unable to allocate DAL backends");
+#endif
 }
 
 int16_t AP_DAL_RangeFinder::ground_clearance_cm_orient(enum Rotation orientation) const
@@ -58,14 +72,12 @@ void AP_DAL_RangeFinder::start_frame()
     // EKF only asks for this *down*.
     _RRNH.ground_clearance_cm = rangefinder->ground_clearance_cm_orient(ROTATION_PITCH_270);
     _RRNH.max_distance_cm = rangefinder->max_distance_cm_orient(ROTATION_PITCH_270);
-    _RRNH.backend_mask = 0;
 
-    for (uint8_t i=0; i<RANGEFINDER_MAX_INSTANCES; i++) {
+    for (uint8_t i=0; i<_RRNH.num_sensors; i++) {
         auto *backend = rangefinder->get_backend(i);
         if (backend == nullptr) {
             break;
         }
-        _RRNH.backend_mask |= (1U<<i);
         _backend[i]->start_frame(backend);
     }
 
@@ -92,7 +104,7 @@ void AP_DAL_RangeFinder_Backend::start_frame(AP_RangeFinder_Backend *backend) {
 // return true if we have a range finder with the specified orientation
 bool AP_DAL_RangeFinder::has_orientation(enum Rotation orientation) const
 {
-    for (uint8_t i=0; i<RANGEFINDER_MAX_INSTANCES; i++) {
+    for (uint8_t i=0; i<_RRNH.num_sensors; i++) {
         if (_RRNI[i].orientation == orientation) {
             return true;
         }
@@ -103,14 +115,29 @@ bool AP_DAL_RangeFinder::has_orientation(enum Rotation orientation) const
 
 AP_DAL_RangeFinder_Backend *AP_DAL_RangeFinder::get_backend(uint8_t id) const
 {
-   if (id > RANGEFINDER_MAX_INSTANCES) {
+   if (id >= RANGEFINDER_MAX_INSTANCES) {
         INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
         return nullptr;
     }
 
-   if ((_RRNH.backend_mask & (1U<<id)) == 0) {
-       return nullptr;
-   }
-
    return _backend[id];
 }
+
+void AP_DAL_RangeFinder::handle_message(const log_RRNH &msg)
+{
+    _RRNH = msg;
+    if (_RRNH.num_sensors > 0 && _RRNI == nullptr) {
+        _RRNI = new log_RRNI[_RRNH.num_sensors];
+        _backend = new AP_DAL_RangeFinder_Backend *[_RRNH.num_sensors];
+    }
+}
+
+void AP_DAL_RangeFinder::handle_message(const log_RRNI &msg)
+{
+    if (_RRNI != nullptr && msg.instance < _RRNH.num_sensors) {
+        _RRNI[msg.instance] = msg;
+        if (_backend != nullptr && _backend[msg.instance] == nullptr) {
+            _backend[msg.instance] = new AP_DAL_RangeFinder_Backend(_RRNI[msg.instance]);
+        }
+    }
+}

libraries/AP_DAL/AP_DAL_RangeFinder.h

@@ -33,19 +33,14 @@ public:
 
     class AP_DAL_RangeFinder_Backend *get_backend(uint8_t id) const;
 
-    void handle_message(const log_RRNH &msg) {
-        _RRNH = msg;
-    }
-    void handle_message(const log_RRNI &msg) {
-        _RRNI[msg.instance] = msg;
-    }
+    void handle_message(const log_RRNH &msg);
+    void handle_message(const log_RRNI &msg);
 
 private:
 
     struct log_RRNH _RRNH;
-    struct log_RRNI _RRNI[RANGEFINDER_MAX_INSTANCES];
-
-    AP_DAL_RangeFinder_Backend *_backend[RANGEFINDER_MAX_INSTANCES];
+    struct log_RRNI *_RRNI;
+    AP_DAL_RangeFinder_Backend **_backend;
 };
 
 

libraries/AP_DAL/LogStructure.h

@@ -156,7 +156,7 @@ struct log_RRNH {
     // this is rotation-pitch-270!
     int16_t ground_clearance_cm;
     int16_t max_distance_cm;
-    uint16_t backend_mask;
+    uint8_t num_sensors;
     uint8_t _end;
 };
 
@@ -386,7 +386,7 @@ struct log_RBOH {
     { LOG_RBRI_MSG, RLOG_SIZE(RBRI),                                   \
       "RBRI", "IfBB", "LastUpdate,Alt,H,I", "---#", "----" }, \
     { LOG_RRNH_MSG, RLOG_SIZE(RRNH),                                   \
-      "RRNH", "hhH", "GCl,MaxD,BMask", "???", "???" },  \
+      "RRNH", "hhB", "GCl,MaxD,NumSensors", "???", "???" },  \
     { LOG_RRNI_MSG, RLOG_SIZE(RRNI),                                   \
       "RRNI", "fffHBBB", "PX,PY,PZ,Dist,Orient,Status,I", "------#", "-------" }, \
     { LOG_RGPH_MSG, RLOG_SIZE(RGPH),                                   \