ardupilot/libraries/AP_DAL/AP_DAL_RangeFinder.cpp

147 lines
4.2 KiB
C++
Raw Normal View History

#include "AP_DAL_RangeFinder.h"
#include <AP_Logger/AP_Logger.h>
#include <AP_RangeFinder/AP_RangeFinder_Backend.h>
#include "AP_DAL.h"
#include <AP_BoardConfig/AP_BoardConfig.h>
AP_DAL_RangeFinder::AP_DAL_RangeFinder()
{
#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<_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::allocation_error("DAL backends");
#endif
}
int16_t AP_DAL_RangeFinder::ground_clearance_cm_orient(enum Rotation orientation) const
{
#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone)
const auto *rangefinder = AP::rangefinder();
if (orientation != ROTATION_PITCH_270) {
// the EKF only asks for this from a specific orientation. Thankfully.
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
return rangefinder->ground_clearance_cm_orient(orientation);
}
#endif
return _RRNH.ground_clearance_cm;
}
int16_t AP_DAL_RangeFinder::max_distance_cm_orient(enum Rotation orientation) const
{
#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone)
if (orientation != ROTATION_PITCH_270) {
2020-11-09 04:49:27 -04:00
const auto *rangefinder = AP::rangefinder();
// the EKF only asks for this from a specific orientation. Thankfully.
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
return rangefinder->ground_clearance_cm_orient(orientation);
}
#endif
return _RRNH.max_distance_cm;
}
void AP_DAL_RangeFinder::start_frame()
{
const auto *rangefinder = AP::rangefinder();
if (rangefinder == nullptr) {
return;
}
const log_RRNH old = _RRNH;
2020-11-09 04:49:27 -04:00
// EKF only asks for this *down*.
_RRNH.ground_clearance_cm = rangefinder->ground_clearance_cm_orient(ROTATION_PITCH_270);
2020-11-07 17:29:45 -04:00
_RRNH.max_distance_cm = rangefinder->max_distance_cm_orient(ROTATION_PITCH_270);
WRITE_REPLAY_BLOCK_IFCHANGED(RRNH, _RRNH, old);
for (uint8_t i=0; i<_RRNH.num_sensors; i++) {
auto *backend = rangefinder->get_backend(i);
if (backend == nullptr) {
break;
}
_backend[i]->start_frame(backend);
}
}
AP_DAL_RangeFinder_Backend::AP_DAL_RangeFinder_Backend(struct log_RRNI &RRNI) :
_RRNI(RRNI)
{
}
void AP_DAL_RangeFinder_Backend::start_frame(AP_RangeFinder_Backend *backend) {
const log_RRNI old = _RRNI;
_RRNI.orientation = backend->orientation();
_RRNI.status = (uint8_t)backend->status();
_RRNI.pos_offset = backend->get_pos_offset();
_RRNI.distance_cm = backend->distance_cm();
2020-11-07 17:29:45 -04:00
WRITE_REPLAY_BLOCK_IFCHANGED(RRNI, _RRNI, old);
}
// 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<_RRNH.num_sensors; i++) {
if (_RRNI[i].orientation == orientation) {
return true;
}
}
return false;
}
AP_DAL_RangeFinder_Backend *AP_DAL_RangeFinder::get_backend(uint8_t id) const
{
if (id >= RANGEFINDER_MAX_INSTANCES) {
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
return nullptr;
}
if (id >= _RRNH.num_sensors) {
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]);
}
}
}