aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_DAL: only allocate rangefinder backends that we need

Browse Source
This commit is contained in:
Andrew Tridgell 2020-11-26 21:39:57 +11:00
parent 893e9ea7fd
commit 73755e2d1f
3 changed files with 45 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
libraries/AP_DAL/AP_DAL_RangeFinder.cpp
View File

@ -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]);
}
}
}

13
libraries/AP_DAL/AP_DAL_RangeFinder.h
View File

@ -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;
};

4
libraries/AP_DAL/LogStructure.h
View File

@ -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), \