From 893e9ea7fd01dca644b78f65b02ccac7a6780439 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 26 Nov 2020 20:57:31 +1100 Subject: [PATCH] AP_DAL: don't allocate sensor backends unless needed this saves memory on boards without those sensors --- libraries/AP_DAL/AP_DAL.cpp | 62 +++++++++++++++++++++++--- libraries/AP_DAL/AP_DAL.h | 61 ++++++++++++++++--------- libraries/AP_DAL/AP_DAL_Beacon.cpp | 1 - libraries/AP_DAL/AP_DAL_Beacon.h | 3 -- libraries/AP_DAL/AP_DAL_VisualOdom.cpp | 1 - libraries/AP_DAL/AP_DAL_VisualOdom.h | 7 --- libraries/AP_DAL/LogStructure.h | 6 +-- 7 files changed, 97 insertions(+), 44 deletions(-) diff --git a/libraries/AP_DAL/AP_DAL.cpp b/libraries/AP_DAL/AP_DAL.cpp index 9980e76a46..2175178545 100644 --- a/libraries/AP_DAL/AP_DAL.cpp +++ b/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 diff --git a/libraries/AP_DAL/AP_DAL.h b/libraries/AP_DAL/AP_DAL.h index a8940287fb..e3bd452f5d 100644 --- a/libraries/AP_DAL/AP_DAL.h +++ b/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)) diff --git a/libraries/AP_DAL/AP_DAL_Beacon.cpp b/libraries/AP_DAL/AP_DAL_Beacon.cpp index 666fe511bc..3b0f88c6e2 100644 --- a/libraries/AP_DAL/AP_DAL_Beacon.cpp +++ b/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); diff --git a/libraries/AP_DAL/AP_DAL_Beacon.h b/libraries/AP_DAL/AP_DAL_Beacon.h index bfe1edff86..26f724b295 100644 --- a/libraries/AP_DAL/AP_DAL_Beacon.h +++ b/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; } diff --git a/libraries/AP_DAL/AP_DAL_VisualOdom.cpp b/libraries/AP_DAL/AP_DAL_VisualOdom.cpp index 770213dff1..81d4feede0 100644 --- a/libraries/AP_DAL/AP_DAL_VisualOdom.cpp +++ b/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(); diff --git a/libraries/AP_DAL/AP_DAL_VisualOdom.h b/libraries/AP_DAL/AP_DAL_VisualOdom.h index f601019b76..b6a2436595 100644 --- a/libraries/AP_DAL/AP_DAL_VisualOdom.h +++ b/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) { diff --git a/libraries/AP_DAL/LogStructure.h b/libraries/AP_DAL/LogStructure.h index 411a2d93a8..5a0b4fcde4 100644 --- a/libraries/AP_DAL/LogStructure.h +++ b/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), \