diff --git a/libraries/AP_DAL/AP_DAL_RangeFinder.cpp b/libraries/AP_DAL/AP_DAL_RangeFinder.cpp index 884170c757..d7d0704f26 100644 --- a/libraries/AP_DAL/AP_DAL_RangeFinder.cpp +++ b/libraries/AP_DAL/AP_DAL_RangeFinder.cpp @@ -4,17 +4,31 @@ #include #include "AP_DAL.h" +#include AP_DAL_RangeFinder::AP_DAL_RangeFinder() { - for (uint8_t i=0; inum_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; iground_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; iget_backend(i); if (backend == nullptr) { break; } - _RRNH.backend_mask |= (1U<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) { + if (id >= RANGEFINDER_MAX_INSTANCES) { INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); return nullptr; } - if ((_RRNH.backend_mask & (1U< 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]); + } + } +} diff --git a/libraries/AP_DAL/AP_DAL_RangeFinder.h b/libraries/AP_DAL/AP_DAL_RangeFinder.h index c6216d2e91..9b9ac027b7 100644 --- a/libraries/AP_DAL/AP_DAL_RangeFinder.h +++ b/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; }; diff --git a/libraries/AP_DAL/LogStructure.h b/libraries/AP_DAL/LogStructure.h index 5a0b4fcde4..01d8821030 100644 --- a/libraries/AP_DAL/LogStructure.h +++ b/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), \