Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_DAL: added data access layer library 

This provides a wrapper around sensor and system calls to allow for
logging for replay in EKF2 and EKF3

Co-authored-by: Peter Barker <pbarker@barker.dropbear.id.au>
This commit is contained in:
Andrew Tridgell 2020-11-06 10:22:38 +11:00
parent 1587d88f58
commit e116b1ff0a
19 changed files with 1981 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

261
libraries/AP_DAL/AP_DAL.cpp Normal file
View File

@ -0,0 +1,261 @@
#include "AP_DAL.h"
#include <AP_HAL/AP_HAL.h>
#include <AP_Logger/AP_Logger.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Vehicle/AP_Vehicle.h>
extern const AP_HAL::HAL& hal;
enum class FrameItem : uint8_t {
AVAILABLE_MEMORY = 0,
};
AP_DAL *AP_DAL::_singleton = nullptr;
bool AP_DAL::force_write;
bool AP_DAL::logging_started;
void AP_DAL::start_frame(AP_DAL::FrameType frametype)
{
#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
const AP_AHRS &ahrs = AP::ahrs();
const uint32_t imu_us = AP::ins().get_last_update_usec();
if (_last_imu_time_us == imu_us) {
_RFRF.frame_types |= uint8_t(frametype);
return;
}
_last_imu_time_us = imu_us;
// we force write all msgs when logging starts
bool logging = AP::logger().logging_started() && AP::logger().allow_start_ekf();
if (logging && !logging_started) {
force_write = true;
}
logging_started = logging;
end_frame();
_RFRF.frame_types = uint8_t(frametype);
_RFRH.time_flying_ms = AP::vehicle()->get_time_flying_ms();
_RFRH.time_us = AP_HAL::micros64();
WRITE_REPLAY_BLOCK(RFRH, _RFRH);
// update RFRN data
const log_RFRN old = _RFRN;
_RFRN.state_bitmask = 0;
if (hal.util->get_soft_armed()) {
_RFRN.state_bitmask |= uint8_t(StateMask::ARMED);
}
_home = ahrs.get_home();
_RFRN.lat = _home.lat;
_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();
_RFRN.ahrs_airspeed_sensor_enabled = AP::ahrs().airspeed_sensor_enabled();
_RFRN.available_memory = hal.util->available_memory();
WRITE_REPLAY_BLOCK_IFCHANGD(RFRN, _RFRN, old);
// update body conversion
_rotation_vehicle_body_to_autopilot_body = ahrs.get_rotation_vehicle_body_to_autopilot_body();
_ins.start_frame();
_baro.start_frame();
_gps.start_frame();
_compass.start_frame();
_airspeed.start_frame();
_rangefinder.start_frame();
_beacon.start_frame();
_visualodom.start_frame();
// populate some derivative values:
_micros = _RFRH.time_us;
_millis = _RFRH.time_us / 1000UL;
_trim = ahrs.get_trim();
force_write = false;
#endif
}
void AP_DAL::end_frame(void)
{
if (_RFRF.frame_types != 0) {
WRITE_REPLAY_BLOCK(RFRF, _RFRF);
_RFRF.frame_types = 0;
}
}
void AP_DAL::log_event2(AP_DAL::Event2 event)
{
#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
end_frame();
struct log_REV2 pkt{
event : uint8_t(event),
};
WRITE_REPLAY_BLOCK(REV2, pkt);
#endif
}
void AP_DAL::log_SetOriginLLH2(const Location &loc)
{
#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
struct log_RSO2 pkt{
lat : loc.lat,
lng : loc.lng,
alt : loc.alt,
};
WRITE_REPLAY_BLOCK(RSO2, pkt);
#endif
}
void AP_DAL::log_writeDefaultAirSpeed2(const float airspeed)
{
#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
struct log_RWA2 pkt{
airspeed: airspeed,
};
WRITE_REPLAY_BLOCK(RWA2, pkt);
#endif
}
void AP_DAL::log_event3(AP_DAL::Event3 event)
{
#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
end_frame();
struct log_REV3 pkt{
event : uint8_t(event),
};
WRITE_REPLAY_BLOCK(REV3, pkt);
#endif
}
void AP_DAL::log_SetOriginLLH3(const Location &loc)
{
#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
struct log_RSO3 pkt{
lat : loc.lat,
lng : loc.lng,
alt : loc.alt,
};
WRITE_REPLAY_BLOCK(RSO3, pkt);
#endif
}
void AP_DAL::log_writeDefaultAirSpeed3(const float airspeed)
{
#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
struct log_RWA3 pkt{
airspeed: airspeed,
};
WRITE_REPLAY_BLOCK(RWA3, pkt);
#endif
}
void AP_DAL::log_writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t timeStamp_ms, uint8_t type)
{
#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
struct log_REY3 pkt{
yawangle : yawAngle,
yawangleerr : yawAngleErr,
timestamp_ms : timeStamp_ms,
type : type,
};
WRITE_REPLAY_BLOCK(REY3, pkt);
#endif
}
int AP_DAL::snprintf(char* str, size_t size, const char *format, ...)
{
va_list ap;
va_start(ap, format);
int res = hal.util->vsnprintf(str, size, format, ap);
va_end(ap);
return res;
}
void *AP_DAL::malloc_type(size_t size, Memory_Type mem_type)
{
return hal.util->malloc_type(size, AP_HAL::Util::Memory_Type(mem_type));
}
const AP_DAL_Compass *AP_DAL::get_compass() const
{
if (_RFRN.get_compass_is_null) {
return nullptr;
}
return &_compass;
}
// map core number for replay
uint8_t AP_DAL::logging_core(uint8_t c) const
{
#if APM_BUILD_TYPE(APM_BUILD_Replay)
return c+100U;
#else
return c;
#endif
}
// write out a DAL log message. If old_msg is non-null, then
// only write if the content has changed
void AP_DAL::WriteLogMessage(enum LogMessages msg_type, void *msg, const void *old_msg, uint8_t msg_size)
{
if (!logging_started) {
// we're not logging
return;
}
// we use the _end byte to hold a flag for forcing output
uint8_t &_end = ((uint8_t *)msg)[msg_size];
if (old_msg && !force_write && _end == 0 && memcmp(msg, old_msg, msg_size) == 0) {
// no change, skip this block write
return;
}
if (!AP::logger().WriteReplayBlock(msg_type, msg, msg_size)) {
// mark for forced write next time
_end = 1;
} else {
_end = 0;
}
}
#include <stdio.h>
namespace AP {
AP_DAL &dal()
{
return *AP_DAL::get_singleton();
}
};
void xxprintf(const char *format, ...)
{
#if APM_BUILD_TYPE(APM_BUILD_Replay) || CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if APM_BUILD_TYPE(APM_BUILD_Replay)
const char *fname = "/tmp/replay.log";
#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
const char *fname = "/tmp/real.log";
#endif
static FILE *f;
if (!f) {
f = ::fopen(fname, "w");
}
va_list ap;
va_start(ap, format);
vfprintf(f, format, ap);
fflush(f);
va_end(ap);
#endif
}

312
libraries/AP_DAL/AP_DAL.h Normal file
View File

@ -0,0 +1,312 @@
#pragma once
#include <stdint.h>
#include <cstddef>
#include "AP_DAL_InertialSensor.h"
#include "AP_DAL_Baro.h"
#include "AP_DAL_GPS.h"
#include "AP_DAL_RangeFinder.h"
#include "AP_DAL_Compass.h"
#include "AP_DAL_Airspeed.h"
#include "AP_DAL_Beacon.h"
#include "AP_DAL_VisualOdom.h"
#include "LogStructure.h"
#define DAL_CORE(c) AP::dal().logging_core(c)
class AP_DAL {
public:
enum class FrameType {
InitialiseFilterEKF2 = 1<<0,
UpdateFilterEKF2 = 1<<1,
InitialiseFilterEKF3 = 1<<2,
UpdateFilterEKF3 = 1<<3,
LogWriteEKF2 = 1<<4,
LogWriteEKF3 = 1<<5,
};
enum class Event2 {
resetGyroBias = 0,
resetHeightDatum = 1,
setInhibitGPS = 2,
setTakeoffExpected = 3,
unsetTakeoffExpected = 4,
setTouchdownExpected = 5,
unsetTouchdownExpected = 6,
setInhibitGpsVertVelUse = 7,
unsetInhibitGpsVertVelUse = 8,
setTerrainHgtStable = 9,
unsetTerrainHgtStable = 10,
requestYawReset = 11,
checkLaneSwitch = 12,
};
enum class Event3 {
resetGyroBias = 0,
resetHeightDatum = 1,
setInhibitGPS = 2,
setTakeoffExpected = 3,
unsetTakeoffExpected = 4,
setTouchdownExpected = 5,
unsetTouchdownExpected = 6,
setInhibitGpsVertVelUse = 7,
unsetInhibitGpsVertVelUse = 8,
setTerrainHgtStable = 9,
unsetTerrainHgtStable = 10,
requestYawReset = 11,
checkLaneSwitch = 12,
};
// must remain the same as AP_AHRS_VehicleClass numbers-wise
enum class VehicleClass : uint8_t {
UNKNOWN,
GROUND,
COPTER,
FIXED_WING,
SUBMARINE,
};
AP_DAL() {}
static AP_DAL *get_singleton() {
if (!_singleton) {
_singleton = new AP_DAL();
}
return _singleton;
}
void start_frame(FrameType frametype);
void end_frame(void);
uint64_t micros64() { return _RFRH.time_us; }
uint32_t micros() { return _micros; }
uint32_t millis() { return _millis; }
void log_event2(Event2 event);
void log_SetOriginLLH2(const Location &loc);
void log_writeDefaultAirSpeed2(float airspeed);
void log_event3(Event3 event);
void log_SetOriginLLH3(const Location &loc);
void log_writeDefaultAirSpeed3(float airspeed);
void log_writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t timeStamp_ms, uint8_t type);
enum class StateMask {
ARMED = (1U<<0),
};
// returns armed state for the current frame
bool get_armed() { return _RFRN.state_bitmask & uint8_t(StateMask::ARMED); }
// memory available at start of current frame. While this could
// potentially change as we go through the frame, the
// ramifications of being out of memory are that you don't start
// the EKF, so the simplicity of having one value for the entire
// frame is worthwhile.
uint32_t available_memory() { return _RFRN.available_memory; }
int8_t get_ekf_type(void) const {
return _RFRN.ekf_type;
}
int snprintf(char* str, size_t size, const char *format, ...);
// copied in AP_HAL/Util.h
enum Memory_Type {
MEM_DMA_SAFE,
MEM_FAST
};
void *malloc_type(size_t size, enum Memory_Type mem_type);
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;
}
AP_DAL_Airspeed *airspeed() {
if (_RFRN.airspeed_ptr_is_null) {
return nullptr;
}
return &_airspeed;
}
AP_DAL_Beacon *beacon() {
return _beacon.beacon();
}
AP_DAL_VisualOdom *visualodom() {
return _visualodom.visualodom();
}
// this method *always* returns you the compass. This is in
// constrast to get_compass, which only returns the compass once
// the vehicle deigns to permit its use by the EKF.
AP_DAL_Compass &compass() { return _compass; }
// this call replaces AP::ahrs()->get_compass(), whose return
// result can be varied by the vehicle (typically by setting when
// first reading is received). This is explicitly not
// "AP_DAL_Compass &compass() { return _compass; } - but it should
// change to be that.
const AP_DAL_Compass *get_compass() const;
// random methods that AP_NavEKF3 wants to call on AHRS:
bool airspeed_sensor_enabled(void) const {
return _RFRN.ahrs_airspeed_sensor_enabled;
}
// this replaces AP::ahrs()->EAS2TAS(), which should probably go
// away in favour of just using the Baro method.
// get apparent to true airspeed ratio
float get_EAS2TAS(void) const {
return _RFRN.EAS2TAS;
}
VehicleClass get_vehicle_class(void) const {
return (VehicleClass)_RFRN.vehicle_class;
}
bool get_fly_forward(void) const {
return _RFRN.fly_forward;
}
// get trim
const Vector3f &get_trim() const {
return _trim;
}
const Matrix3f &get_rotation_vehicle_body_to_autopilot_body(void) const {
return _rotation_vehicle_body_to_autopilot_body;
}
// get the home location. This is const to prevent any changes to
// home without telling AHRS about the change
const struct Location &get_home(void) const {
return _home;
}
uint32_t get_time_flying_ms(void) const {
return _RFRH.time_flying_ms;
}
// Replay support:
#if APM_BUILD_TYPE(APM_BUILD_Replay)
void handle_message(const log_RFRH &msg) {
_RFRH = msg;
_micros = _RFRH.time_us;
_millis = _RFRH.time_us / 1000UL;
}
void handle_message(const log_RFRN &msg) {
_RFRN = msg;
_home.lat = msg.lat;
_home.lng = msg.lng;
_home.alt = msg.alt;
}
void handle_message(const log_RISH &msg) {
_ins.handle_message(msg);
}
void handle_message(const log_RISI &msg) {
_ins.handle_message(msg);
}
void handle_message(const log_RASH &msg) {
_airspeed.handle_message(msg);
}
void handle_message(const log_RASI &msg) {
_airspeed.handle_message(msg);
}
void handle_message(const log_RBRH &msg) {
_baro.handle_message(msg);
}
void handle_message(const log_RBRI &msg) {
_baro.handle_message(msg);
}
void handle_message(const log_RRNH &msg) {
_rangefinder.handle_message(msg);
}
void handle_message(const log_RRNI &msg) {
_rangefinder.handle_message(msg);
}
void handle_message(const log_RGPH &msg) {
_gps.handle_message(msg);
}
void handle_message(const log_RGPI &msg) {
_gps.handle_message(msg);
}
void handle_message(const log_RGPJ &msg) {
_gps.handle_message(msg);
}
void handle_message(const log_RMGH &msg) {
_compass.handle_message(msg);
}
void handle_message(const log_RMGI &msg) {
_compass.handle_message(msg);
}
void handle_message(const log_RBCH &msg) {
_beacon.handle_message(msg);
}
void handle_message(const log_RBCI &msg) {
_beacon.handle_message(msg);
}
void handle_message(const log_RVOH &msg) {
_visualodom.handle_message(msg);
}
#endif
// map core number for replay
uint8_t logging_core(uint8_t c) const;
// write out a DAL log message. If old_msg is non-null, then
// only write if the content has changed
static void WriteLogMessage(enum LogMessages msg_type, void *msg, const void *old_msg, uint8_t msg_size);
private:
static AP_DAL *_singleton;
struct log_RFRH _RFRH;
struct log_RFRF _RFRF;
struct log_RFRN _RFRN;
// cached variables for speed:
uint32_t _micros;
uint32_t _millis;
Vector3f _trim;
Matrix3f _rotation_vehicle_body_to_autopilot_body;
Location _home;
uint32_t _last_imu_time_us;
AP_DAL_InertialSensor _ins;
AP_DAL_Baro _baro;
AP_DAL_GPS _gps;
AP_DAL_RangeFinder _rangefinder;
AP_DAL_Compass _compass;
AP_DAL_Airspeed _airspeed;
AP_DAL_Beacon _beacon;
AP_DAL_VisualOdom _visualodom;
static bool logging_started;
static bool force_write;
};
#define WRITE_REPLAY_BLOCK(sname,v) AP_DAL::WriteLogMessage(LOG_## sname ##_MSG, &v, nullptr, offsetof(log_ ##sname, _end))
#define WRITE_REPLAY_BLOCK_IFCHANGD(sname,v,old) AP_DAL::WriteLogMessage(LOG_## sname ##_MSG, &v, &old, offsetof(log_ ##sname, _end))
namespace AP {
AP_DAL &dal();
};
void xxprintf(const char *format, ...);

36
libraries/AP_DAL/AP_DAL_Airspeed.cpp Normal file
View File

@ -0,0 +1,36 @@
#include "AP_DAL_Airspeed.h"
#include "AP_DAL.h"
#include <AP_Logger/AP_Logger.h>
AP_DAL_Airspeed::AP_DAL_Airspeed()
{
for (uint8_t i=0; i<ARRAY_SIZE(_RASI); i++) {
_RASI[i].instance = i;
}
}
void AP_DAL_Airspeed::start_frame()
{
const auto *airspeed = AP::airspeed();
if (airspeed == nullptr) {
return;
}
const log_RASH old = _RASH;
_RASH.num_sensors = airspeed->get_num_sensors();
_RASH.primary = airspeed->get_primary();
WRITE_REPLAY_BLOCK_IFCHANGD(RASH, _RASH, old);
for (uint8_t i=0; i<ARRAY_SIZE(_RASI); i++) {
log_RASI &RASI = _RASI[i];
log_RASI old_RASI = RASI;
const uint32_t last_update_ms = airspeed->last_update_ms(i);
_last_logged_update_ms[i] = last_update_ms;
RASI.last_update_ms = last_update_ms;
RASI.healthy = airspeed->healthy(i);
RASI.use = airspeed->use(i);
RASI.airspeed = airspeed->get_airspeed(i);
WRITE_REPLAY_BLOCK_IFCHANGD(RASI, RASI, old_RASI);
}
}

65
libraries/AP_DAL/AP_DAL_Airspeed.h Normal file
View File

@ -0,0 +1,65 @@
#pragma once
#include <AP_Logger/LogStructure.h>
#include <AP_Airspeed/AP_Airspeed.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
class AP_DAL_Airspeed {
public:
// Airspeed-like methods:
// return health status of sensor
bool healthy(uint8_t i) const {
return _RASI[i].healthy;
}
// return true if airspeed is enabled, and airspeed use is set
bool use(uint8_t i) const {
return _RASI[i].use;
}
bool use() const {
return use(get_primary());
}
// return the current airspeed in m/s
float get_airspeed(uint8_t i) const {
return _RASI[i].airspeed;
}
float get_airspeed() const {
return get_airspeed(get_primary());
}
// return time in ms of last update
uint32_t last_update_ms(uint8_t i) const { return _RASI[i].last_update_ms; }
uint32_t last_update_ms() const { return last_update_ms(get_primary()); }
// get number of sensors
uint8_t get_num_sensors(void) const { return _RASH.num_sensors; }
// get current primary sensor
uint8_t get_primary(void) const { return _RASH.primary; }
// AP_DAL methods:
AP_DAL_Airspeed();
void start_frame();
#if APM_BUILD_TYPE(APM_BUILD_Replay)
void handle_message(const log_RASH &msg) {
_RASH = msg;
}
void handle_message(const log_RASI &msg) {
_RASI[msg.instance] = msg;
}
#endif
private:
struct log_RASH _RASH;
struct log_RASI _RASI[AIRSPEED_MAX_SENSORS];
uint32_t _last_logged_update_ms[AIRSPEED_MAX_SENSORS];
};

37
libraries/AP_DAL/AP_DAL_Baro.cpp Normal file
View File

@ -0,0 +1,37 @@
#include "AP_DAL_Baro.h"
#include <AP_Logger/AP_Logger.h>
#include "AP_DAL.h"
AP_DAL_Baro::AP_DAL_Baro()
{
for (uint8_t i=0; i<BARO_MAX_INSTANCES; i++) {
_RBRI[i].instance = i;
}
}
void AP_DAL_Baro::start_frame()
{
const auto &baro = AP::baro();
const log_RBRH old_RBRH = _RBRH;
_RBRH.primary = baro.get_primary();
_RBRH.num_instances = baro.num_instances();
WRITE_REPLAY_BLOCK_IFCHANGD(RBRH, _RBRH, old_RBRH);
for (uint8_t i=0; i<BARO_MAX_INSTANCES; i++) {
log_RBRI &RBRI = _RBRI[i];
log_RBRI old = RBRI;
const uint32_t last_update_ms = baro.get_last_update(i);
_last_logged_update_ms[i] = last_update_ms;
RBRI.last_update_ms = last_update_ms;
RBRI.healthy = baro.healthy(i);
RBRI.altitude = baro.get_altitude(i);
WRITE_REPLAY_BLOCK_IFCHANGD(RBRI, _RBRI[i], old);
}
}
void AP_DAL_Baro::update_calibration()
{
AP::baro().update_calibration();
}

59
libraries/AP_DAL/AP_DAL_Baro.h Normal file
View File

@ -0,0 +1,59 @@
#pragma once
#include <AP_Baro/AP_Baro.h>
#include <AP_Logger/LogStructure.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
class AP_DAL_Baro {
public:
// methods so we look like AP_Baro:
uint8_t get_primary() const {
return _RBRH.primary;
}
uint8_t num_instances() const {
return _RBRH.num_instances;
}
bool healthy(uint8_t sensor_id) const {
return _RBRI[sensor_id].healthy;
}
uint32_t get_last_update(uint8_t sensor_id) const {
return _RBRI[sensor_id].last_update_ms;
}
uint32_t get_last_update() const {
return get_last_update(get_primary());
}
float get_altitude(uint8_t sensor_id) const {
return _RBRI[sensor_id].altitude;
}
float get_altitude() const {
return get_altitude(get_primary());
}
// update_calibration is a no-op in Replay as it simply modifies the data
// which we'll be logging for input to the EKF.
void update_calibration();
// methods for being part of AP_DAL:
AP_DAL_Baro();
void start_frame();
#if APM_BUILD_TYPE(APM_BUILD_Replay)
void handle_message(const log_RBRH &msg) {
_RBRH = msg;
}
void handle_message(const log_RBRI &msg) {
_RBRI[msg.instance] = msg;
}
#endif
private:
struct log_RBRH _RBRH;
struct log_RBRI _RBRI[BARO_MAX_INSTANCES];
uint32_t _last_logged_update_ms[BARO_MAX_INSTANCES];
};

47
libraries/AP_DAL/AP_DAL_Beacon.cpp Normal file
View File

@ -0,0 +1,47 @@
#include "AP_DAL_Beacon.h"
#include <AP_Beacon/AP_Beacon.h>
#include <AP_Logger/AP_Logger.h>
#include "AP_DAL.h"
AP_DAL_Beacon::AP_DAL_Beacon()
{
for (uint8_t i=0; i<ARRAY_SIZE(_RBCI); i++) {
_RBCI[i].instance = i;
}
}
void AP_DAL_Beacon::start_frame()
{
const auto *beacon = AP::beacon();
_RBCH.ptr_is_nullptr = (beacon == nullptr);
const log_RBCH old = _RBCH;
if (beacon != nullptr) {
_RBCH.count = beacon->count();
_RBCH.get_vehicle_position_ned_returncode = beacon->get_vehicle_position_ned(_RBCH.vehicle_position_ned, _RBCH.accuracy_estimate);
_RBCH.get_origin_returncode = beacon->get_origin(_origin);
_RBCH.origin_lat = _origin.lat;
_RBCH.origin_lng = _origin.lng;
_RBCH.origin_alt = _origin.alt;
}
WRITE_REPLAY_BLOCK_IFCHANGD(RBCH, _RBCH, old);
if (beacon == nullptr) {
return;
}
for (uint8_t i=0; i<ARRAY_SIZE(_RBCI); i++) {
log_RBCI &RBCI = _RBCI[i];
const log_RBCI old_RBCI = RBCI;
const uint32_t last_update_ms = beacon->beacon_last_update_ms(i);
_last_logged_update_ms[i] = last_update_ms;
RBCI.last_update_ms = last_update_ms;
RBCI.position = beacon->beacon_position(i);
RBCI.distance = beacon->beacon_distance(i);
RBCI.healthy = beacon->beacon_healthy(i);
WRITE_REPLAY_BLOCK_IFCHANGD(RBCI, RBCI, old_RBCI);
}
}

81
libraries/AP_DAL/AP_DAL_Beacon.h Normal file
View File

@ -0,0 +1,81 @@
#pragma once
#include <AP_Logger/LogStructure.h>
#include <AP_Beacon/AP_Beacon.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
class AP_DAL_Beacon {
public:
// Beacon-like methods:
bool count() const {
return _RBCH.count;
}
bool get_origin(Location &loc) const {
loc = _origin;
return _RBCH.get_origin_returncode;
}
// return beacon health
bool beacon_healthy(uint8_t i) const {
return _RBCI[i].healthy;
}
// return last update time from beacon in milliseconds
uint32_t beacon_last_update_ms(uint8_t i) const {
return _RBCI[i].last_update_ms;
}
// return distance to beacon in meters
float beacon_distance(uint8_t i) const {
return _RBCI[i].distance;
}
// return NED position of beacon in meters relative to the beacon systems origin
const Vector3f &beacon_position(uint8_t i) const {
return _RBCI[i].position;
}
// return vehicle position in NED from position estimate system's origin in meters
bool get_vehicle_position_ned(Vector3f& pos, float& accuracy_estimate) const {
pos = _RBCH.vehicle_position_ned;
accuracy_estimate = _RBCH.accuracy_estimate;
return _RBCH.get_vehicle_position_ned_returncode;
}
// AP_DAL methods:
AP_DAL_Beacon();
AP_DAL_Beacon *beacon() {
if (_RBCH.ptr_is_nullptr) {
return nullptr;
}
return this;
}
void start_frame();
#if APM_BUILD_TYPE(APM_BUILD_Replay)
void handle_message(const log_RBCH &msg) {
_RBCH = msg;
_origin.lat = _RBCH.origin_lat;
_origin.lng = _RBCH.origin_lng;
_origin.alt = _RBCH.origin_alt;
}
void handle_message(const log_RBCI &msg) {
_RBCI[msg.instance] = msg;
}
#endif
private:
struct log_RBCH _RBCH;
struct log_RBCI _RBCI[AP_BEACON_MAX_BEACONS];
uint32_t _last_logged_update_ms[AP_BEACON_MAX_BEACONS];
Location _origin;
};

40
libraries/AP_DAL/AP_DAL_Compass.cpp Normal file
View File

@ -0,0 +1,40 @@
#include "AP_DAL_Compass.h"
#include <AP_Compass/AP_Compass.h>
#include <AP_Logger/AP_Logger.h>
#include "AP_DAL.h"
AP_DAL_Compass::AP_DAL_Compass()
{
for (uint8_t i=0; i<ARRAY_SIZE(_RMGI); i++) {
_RMGI[i].instance = i;
}
}
void AP_DAL_Compass::start_frame()
{
const auto &compass = AP::compass();
const log_RMGH old = _RMGH;
_RMGH.count = compass.get_count();
_RMGH.auto_declination_enabled = compass.auto_declination_enabled();
_RMGH.declination = compass.get_declination();
_RMGH.num_enabled = compass.get_num_enabled();
_RMGH.consistent = compass.consistent();
WRITE_REPLAY_BLOCK_IFCHANGD(RMGH, _RMGH, old);
for (uint8_t i=0; i<ARRAY_SIZE(_RMGI); i++) {
log_RMGI &RMGI = _RMGI[i];
const log_RMGI old_RMGI = RMGI;
RMGI.use_for_yaw = compass.use_for_yaw(i);
RMGI.healthy = compass.healthy(i);
RMGI.offsets = compass.get_offsets(i);
RMGI.have_scale_factor = compass.have_scale_factor(i);
RMGI.last_update_usec = compass.last_update_usec(i);
RMGI.field = compass.get_field(i);
WRITE_REPLAY_BLOCK_IFCHANGD(RMGI, RMGI, old_RMGI);
}
}

74
libraries/AP_DAL/AP_DAL_Compass.h Normal file
View File

@ -0,0 +1,74 @@
#pragma once
#include <AP_Logger/LogStructure.h>
#include <AP_Compass/AP_Compass.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
class AP_DAL_Compass {
public:
// Compass-like methods:
bool use_for_yaw(uint8_t i) const {
return _RMGI[i].use_for_yaw;
}
bool healthy(uint8_t i) const {
return _RMGI[i].healthy;
}
const Vector3f &get_offsets(uint8_t i) const {
return _RMGI[i].offsets;
}
bool have_scale_factor(uint8_t i) const {
return _RMGI[i].have_scale_factor;
}
bool auto_declination_enabled() const {
return _RMGH.auto_declination_enabled;
}
uint8_t get_count() const {
return _RMGH.count;
}
float get_declination() const {
return _RMGH.declination;
}
// return the number of enabled sensors
uint8_t get_num_enabled(void) const { return _RMGH.num_enabled; }
// learn offsets accessor
bool learn_offsets_enabled() const { return _RMGH.learn_offsets_enabled; }
// return last update time in microseconds
uint32_t last_update_usec(uint8_t i) const { return _RMGI[i].last_update_usec; }
/// Return the current field as a Vector3f in milligauss
const Vector3f &get_field(uint8_t i) const { return _RMGI[i].field; }
// check if the compasses are pointing in the same direction
bool consistent() const { return _RMGH.consistent; }
// AP_DAL methods:
AP_DAL_Compass();
void start_frame();
#if APM_BUILD_TYPE(APM_BUILD_Replay)
void handle_message(const log_RMGH &msg) {
_RMGH = msg;
}
void handle_message(const log_RMGI &msg) {
_RMGI[msg.instance] = msg;
}
#endif
private:
struct log_RMGH _RMGH;
struct log_RMGI _RMGI[COMPASS_MAX_INSTANCES];
};

60
libraries/AP_DAL/AP_DAL_GPS.cpp Normal file
View File

@ -0,0 +1,60 @@
#include "AP_DAL_GPS.h"
#include <AP_Logger/AP_Logger.h>
#include "AP_DAL.h"
AP_DAL_GPS::AP_DAL_GPS()
{
for (uint8_t i=0; i<ARRAY_SIZE(_RGPI); i++) {
_RGPI[i].instance = i;
_RGPJ[i].instance = i;
}
}
void AP_DAL_GPS::start_frame()
{
const auto &gps = AP::gps();
const log_RGPH old_RGPH = _RGPH;
_RGPH.primary_sensor = gps.primary_sensor();
_RGPH.num_sensors = gps.num_sensors();
WRITE_REPLAY_BLOCK_IFCHANGD(RGPH, _RGPH, old_RGPH);
for (uint8_t i=0; i<ARRAY_SIZE(_RGPI); i++) {
log_RGPI &RGPI = _RGPI[i];
RGPI.status = (GPS_Status)gps.status(i);
const uint32_t last_message_time_ms = gps.last_message_time_ms(i);
if (last_message_time_ms == _last_logged_message_time_ms[i]) {
// assume that no other state changes if we don't get a message.
return;
}
_last_logged_message_time_ms[i] = last_message_time_ms;
const Location &loc = gps.location(i);
RGPI.last_message_time_ms = last_message_time_ms;
RGPI.lat = loc.lat;
RGPI.lng = loc.lng;
RGPI.alt = loc.alt;
RGPI.have_vertical_velocity = gps.have_vertical_velocity(i);
RGPI.horizontal_accuracy_returncode = gps.horizontal_accuracy(i, RGPI.hacc);
RGPI.vertical_accuracy_returncode = gps.vertical_accuracy(i, RGPI.vacc);
RGPI.hdop = gps.get_hdop(i);
RGPI.num_sats = gps.num_sats(i);
RGPI.get_lag_returncode = gps.get_lag(i, RGPI.lag_sec);
WRITE_REPLAY_BLOCK(RGPI, RGPI);
log_RGPJ &RGPJ = _RGPJ[i];
RGPJ.velocity = gps.velocity(i);
RGPJ.speed_accuracy_returncode = gps.speed_accuracy(i, RGPJ.sacc);
RGPJ.gps_yaw_deg_returncode = gps.gps_yaw_deg(i, RGPJ.yaw_deg, RGPJ.yaw_accuracy_deg);
WRITE_REPLAY_BLOCK(RGPJ, RGPJ);
// also fetch antenna offset for this frame
antenna_offset[i] = gps.get_antenna_offset(i);
}
}

155
libraries/AP_DAL/AP_DAL_GPS.h Normal file
View File

@ -0,0 +1,155 @@
#pragma once
#include <AP_GPS/AP_GPS.h>
#include <AP_Logger/LogStructure.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
static Location tmp_location[GPS_MAX_INSTANCES];
class AP_DAL_GPS {
public:
/// GPS status codes
enum GPS_Status : uint8_t {
NO_GPS = GPS_FIX_TYPE_NO_GPS, ///< No GPS connected/detected
NO_FIX = GPS_FIX_TYPE_NO_FIX, ///< Receiving valid GPS messages but no lock
GPS_OK_FIX_2D = GPS_FIX_TYPE_2D_FIX, ///< Receiving valid messages and 2D lock
GPS_OK_FIX_3D = GPS_FIX_TYPE_3D_FIX, ///< Receiving valid messages and 3D lock
GPS_OK_FIX_3D_DGPS = GPS_FIX_TYPE_DGPS, ///< Receiving valid messages and 3D lock with differential improvements
GPS_OK_FIX_3D_RTK_FLOAT = GPS_FIX_TYPE_RTK_FLOAT, ///< Receiving valid messages and 3D RTK Float
GPS_OK_FIX_3D_RTK_FIXED = GPS_FIX_TYPE_RTK_FIXED, ///< Receiving valid messages and 3D RTK Fixed
};
AP_DAL_GPS();
GPS_Status status(uint8_t sensor_id) const {
return (GPS_Status)_RGPI[sensor_id].status;
}
GPS_Status status() const {
return status(primary_sensor());
}
const Location &location(uint8_t instance) const {
Location &loc = tmp_location[instance];
loc.lat = _RGPI[instance].lat;
loc.lng = _RGPI[instance].lng;
loc.alt = _RGPI[instance].alt;
return loc;
}
bool have_vertical_velocity(uint8_t instance) const {
return _RGPI[instance].have_vertical_velocity;
}
bool have_vertical_velocity() const {
return have_vertical_velocity(primary_sensor());
}
bool horizontal_accuracy(uint8_t instance, float &hacc) const {
hacc = _RGPI[instance].hacc;
return _RGPI[instance].horizontal_accuracy_returncode;
}
bool horizontal_accuracy(float &hacc) const {
return horizontal_accuracy(primary_sensor(),hacc);
}
bool vertical_accuracy(uint8_t instance, float &vacc) const {
vacc = _RGPI[instance].vacc;
return _RGPI[instance].vertical_accuracy_returncode;
}
bool vertical_accuracy(float &vacc) const {
return vertical_accuracy(primary_sensor(), vacc);
}
uint16_t get_hdop(uint8_t instance) const {
return _RGPI[instance].hdop;
}
uint16_t get_hdop() const {
return get_hdop(primary_sensor());
}
uint32_t last_message_time_ms(uint8_t instance) const {
return _RGPI[instance].last_message_time_ms;
}
uint8_t num_sats(uint8_t instance) const {
return _RGPI[instance].num_sats;
}
uint8_t num_sats() const {
return num_sats(primary_sensor());
}
bool get_lag(uint8_t instance, float &lag_sec) const {
lag_sec = _RGPI[instance].lag_sec;
return _RGPI[instance].get_lag_returncode;
}
bool get_lag(float &lag_sec) const {
return get_lag(primary_sensor(), lag_sec);
}
const Vector3f &velocity(uint8_t instance) const {
return _RGPJ[instance].velocity;
}
const Vector3f &velocity() const {
return velocity(primary_sensor());
}
bool speed_accuracy(uint8_t instance, float &sacc) const {
sacc = _RGPJ[instance].sacc;
return _RGPJ[instance].speed_accuracy_returncode;
}
bool speed_accuracy(float &sacc) const {
return speed_accuracy(primary_sensor(), sacc);
}
bool gps_yaw_deg(float &yaw_deg, float &accuracy_deg) const {
return gps_yaw_deg(_RGPH.primary_sensor, yaw_deg, accuracy_deg);
}
bool gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg) const {
yaw_deg = _RGPJ[instance].yaw_deg;
accuracy_deg = _RGPJ[instance].yaw_accuracy_deg;
return _RGPJ[instance].gps_yaw_deg_returncode;
}
uint8_t num_sensors(void) const {
return _RGPH.num_sensors;
}
uint8_t primary_sensor(void) const {
return _RGPH.primary_sensor;
}
// TODO: decide if this really, really should be here!
const Location &location() const {
return location(_RGPH.primary_sensor);
}
// return a 3D vector defining the offset of the GPS antenna in meters relative to the body frame origin
const Vector3f &get_antenna_offset(uint8_t instance) const {
return antenna_offset[instance];
}
void start_frame();
#if APM_BUILD_TYPE(APM_BUILD_Replay)
void handle_message(const log_RGPH &msg) {
_RGPH = msg;
}
void handle_message(const log_RGPI &msg) {
_RGPI[msg.instance] = msg;
antenna_offset[msg.instance] = AP::gps().get_antenna_offset(msg.instance);
}
void handle_message(const log_RGPJ &msg) {
_RGPJ[msg.instance] = msg;
}
#endif
private:
struct log_RGPH _RGPH;
struct log_RGPI _RGPI[GPS_MAX_INSTANCES];
struct log_RGPJ _RGPJ[GPS_MAX_INSTANCES];
Vector3f antenna_offset[GPS_MAX_INSTANCES];
uint32_t _last_logged_message_time_ms[GPS_MAX_INSTANCES];
};

56
libraries/AP_DAL/AP_DAL_InertialSensor.cpp Normal file
View File

@ -0,0 +1,56 @@
#include "AP_DAL_InertialSensor.h"
#include <AP_Logger/AP_Logger.h>
#include "AP_DAL.h"
AP_DAL_InertialSensor::AP_DAL_InertialSensor()
{
for (uint8_t i=0; i<ARRAY_SIZE(_RISI); i++) {
_RISI[i].instance = i;
}
}
void AP_DAL_InertialSensor::start_frame()
{
const auto &ins = AP::ins();
const log_RISH old_RISH = _RISH;
_RISH.loop_rate_hz = ins.get_loop_rate_hz();
_RISH.primary_gyro = ins.get_primary_gyro();
_RISH.loop_delta_t = ins.get_loop_delta_t();
_RISH.primary_accel = ins.get_primary_accel();
_RISH.accel_count = ins.get_accel_count();
_RISH.gyro_count = ins.get_gyro_count();
WRITE_REPLAY_BLOCK_IFCHANGD(RISH, _RISH, old_RISH);
for (uint8_t i=0; i<ARRAY_SIZE(_RISI); i++) {
log_RISI &RISI = _RISI[i];
const log_RISI old_RISI = RISI;
// accel stuff
RISI.use_accel = ins.use_accel(i);
RISI.get_delta_velocity_ret = ins.get_delta_velocity(i, RISI.delta_velocity);
RISI.delta_velocity_dt = ins.get_delta_velocity_dt(i);
// gryo stuff
RISI.use_gyro = ins.use_gyro(i);
RISI.delta_angle_dt = ins.get_delta_angle_dt(i);
RISI.get_delta_angle_ret = ins.get_delta_angle(i, RISI.delta_angle);
update_filtered(i);
WRITE_REPLAY_BLOCK_IFCHANGD(RISI, RISI, old_RISI);
// update sensor position
pos[i] = ins.get_imu_pos_offset(i);
}
}
// update filtered gyro and accel
void AP_DAL_InertialSensor::update_filtered(uint8_t i)
{
gyro_filtered[i] = gyro_filtered[i] * alpha + (_RISI[i].delta_angle/_RISI[i].delta_angle_dt) * (1-alpha);
accel_filtered[i] = accel_filtered[i] * alpha + (_RISI[i].delta_velocity/_RISI[i].delta_velocity_dt) * (1-alpha);
}

86
libraries/AP_DAL/AP_DAL_InertialSensor.h Normal file
View File

@ -0,0 +1,86 @@
#pragma once
#include <AP_InertialSensor/AP_InertialSensor.h>
#include <AP_Logger/LogStructure.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
class AP_DAL_InertialSensor {
public:
// InertialSensor-like methods:
// return the selected loop rate at which samples are made available
uint16_t get_loop_rate_hz(void) const { return _RISH.loop_rate_hz; }
const Vector3f &get_imu_pos_offset(uint8_t instance) const {
return pos[instance];
}
// accel stuff
uint8_t get_accel_count(void) const { return _RISH.accel_count; }
uint8_t get_primary_accel(void) const { return _RISH.primary_accel; };
bool use_accel(uint8_t instance) const { return _RISI[instance].use_accel; }
const Vector3f &get_accel(uint8_t i) const { return accel_filtered[i]; }
bool get_delta_velocity(uint8_t i, Vector3f &delta_velocity) const {
delta_velocity = _RISI[i].delta_velocity;
return _RISI[i].get_delta_velocity_ret;
}
float get_delta_velocity_dt(uint8_t i) const {
return _RISI[i].delta_velocity_dt;
}
// gyro stuff
uint8_t get_gyro_count(void) const { return _RISH.gyro_count; }
uint8_t get_primary_gyro(void) const { return _RISH.primary_gyro; };
bool use_gyro(uint8_t instance) const { return _RISI[instance].use_gyro; }
const Vector3f &get_gyro(uint8_t i) const { return gyro_filtered[i]; }
const Vector3f &get_gyro() const { return get_gyro(_primary_gyro); }
bool get_delta_angle(uint8_t i, Vector3f &delta_angle) const {
delta_angle = _RISI[i].delta_angle;
return _RISI[i].get_delta_angle_ret;
}
float get_delta_angle_dt(uint8_t i) const { return _RISI[i].delta_angle_dt; }
// return the main loop delta_t in seconds
float get_loop_delta_t(void) const { return _RISH.loop_delta_t; }
// AP_DAL methods:
AP_DAL_InertialSensor();
void start_frame();
#if APM_BUILD_TYPE(APM_BUILD_Replay)
void handle_message(const log_RISH &msg) {
_RISH = msg;
}
void handle_message(const log_RISI &msg) {
_RISI[msg.instance] = msg;
pos[msg.instance] = AP::ins().get_imu_pos_offset(msg.instance);
update_filtered(msg.instance);
}
#endif
private:
// filter constant for deltas to gyro/accel
const float alpha = 0.9;
struct log_RISH _RISH;
struct log_RISI _RISI[INS_MAX_INSTANCES];
// sensor positions
Vector3f pos[INS_MAX_INSTANCES];
Vector3f gyro_filtered[INS_MAX_INSTANCES];
Vector3f accel_filtered[INS_MAX_INSTANCES];
uint8_t _primary_gyro;
void log_header(uint64_t time_us);
void log_instance(uint64_t time_us, uint8_t i);
void update_filtered(uint8_t i);
};

104
libraries/AP_DAL/AP_DAL_RangeFinder.cpp Normal file
View File

@ -0,0 +1,104 @@
#include "AP_DAL_RangeFinder.h"
#include <AP_Logger/AP_Logger.h>
#include <AP_RangeFinder/AP_RangeFinder_Backend.h>
#include "AP_DAL.h"
AP_DAL_RangeFinder::AP_DAL_RangeFinder()
{
for (uint8_t i=0; i<ARRAY_SIZE(_RRNI); i++) {
_RRNI[i].instance = i;
}
for (uint8_t i=0; i<RANGEFINDER_MAX_INSTANCES; i++) {
// this avoids having to discard a const....
_backend[i] = new AP_DAL_RangeFinder_Backend(_RRNI[i]);
}
}
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)
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.max_distance_cm;
}
void AP_DAL_RangeFinder::start_frame()
{
const auto *rangefinder = AP::rangefinder();
if (rangefinder == nullptr) {
return;
}
// EKF only asks for this *down*.
const log_RRNH old = _RRNH;
_RRNH.ground_clearance_cm = rangefinder->ground_clearance_cm_orient(ROTATION_PITCH_270);
_RRNH.max_distance_cm = rangefinder->ground_clearance_cm_orient(ROTATION_PITCH_270);
_RRNH.backend_mask = 0;
for (uint8_t i=0; i<RANGEFINDER_MAX_INSTANCES; i++) {
auto *backend = rangefinder->get_backend(i);
if (backend == nullptr) {
break;
}
_RRNH.backend_mask |= (1U<<i);
_backend[i]->start_frame(backend);
}
WRITE_REPLAY_BLOCK_IFCHANGD(RRNH, _RRNH, old);
}
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();
WRITE_REPLAY_BLOCK_IFCHANGD(RRNI, _RRNI, old);
}
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 ((_RRNH.backend_mask & (1U<<id)) == 0) {
return nullptr;
}
return _backend[id];
}

76
libraries/AP_DAL/AP_DAL_RangeFinder.h Normal file
View File

@ -0,0 +1,76 @@
#pragma once
#include <AP_RangeFinder/AP_RangeFinder.h>
#include <AP_Logger/LogStructure.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
class AP_RangeFinder_Backend;
class AP_DAL_RangeFinder {
public:
// RangeFinder-like methods:
enum class Status {
NotConnected = 0,
NoData,
OutOfRangeLow,
OutOfRangeHigh,
Good
};
int16_t ground_clearance_cm_orient(enum Rotation orientation) const;
int16_t max_distance_cm_orient(enum Rotation orientation) const;
// DAL methods:
AP_DAL_RangeFinder();
void start_frame();
class AP_DAL_RangeFinder_Backend *get_backend(uint8_t id) const;
#if APM_BUILD_TYPE(APM_BUILD_Replay)
void handle_message(const log_RRNH &msg) {
_RRNH = msg;
}
void handle_message(const log_RRNI &msg) {
_RRNI[msg.instance] = msg;
}
#endif
private:
struct log_RRNH _RRNH;
struct log_RRNI _RRNI[RANGEFINDER_MAX_INSTANCES];
AP_DAL_RangeFinder_Backend *_backend[RANGEFINDER_MAX_INSTANCES];
};
class AP_DAL_RangeFinder_Backend {
public:
AP_DAL_RangeFinder_Backend(struct log_RRNI &RRNI);
// RangeFinder-backend-like methods
enum Rotation orientation() const {
return (Rotation)_RRNI.orientation;
}
AP_DAL_RangeFinder::Status status() const {
return (AP_DAL_RangeFinder::Status)_RRNI.status;
}
uint16_t distance_cm() const { return _RRNI.distance_cm; }
const Vector3f &get_pos_offset() const { return _RRNI.pos_offset; }
// DAL methods:
void start_frame(AP_RangeFinder_Backend *backend);
private:
struct log_RRNI &_RRNI;
};

23
libraries/AP_DAL/AP_DAL_VisualOdom.cpp Normal file
View File

@ -0,0 +1,23 @@
#include "AP_DAL_VisualOdom.h"
#include <AP_VisualOdom/AP_VisualOdom.h>
#include <AP_Logger/AP_Logger.h>
#include "AP_DAL.h"
AP_DAL_VisualOdom::AP_DAL_VisualOdom()
{
}
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.healthy = vo->healthy();
}
WRITE_REPLAY_BLOCK_IFCHANGD(RVOH, RVOH, old);
}

51
libraries/AP_DAL/AP_DAL_VisualOdom.h Normal file
View File

@ -0,0 +1,51 @@
#pragma once
#include <AP_Logger/LogStructure.h>
#include <AP_VisualOdom/AP_VisualOdom.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
class AP_DAL_VisualOdom {
public:
// return VisualOdom health
bool healthy() const {
return RVOH.healthy;
}
bool enabled() const {
return RVOH.enabled;
}
bool get_delay_ms() const {
return RVOH.delay_ms;
}
// return a 3D vector defining the position offset of the camera in meters relative to the body frame origin
const Vector3f &get_pos_offset() const {
return RVOH.pos_offset;
}
// AP_DAL methods:
AP_DAL_VisualOdom();
AP_DAL_VisualOdom *visualodom() {
if (RVOH.ptr_is_nullptr) {
return nullptr;
}
return this;
}
void start_frame();
#if APM_BUILD_TYPE(APM_BUILD_Replay)
void handle_message(const log_RVOH &msg) {
RVOH = msg;
}
#endif
private:
struct log_RVOH RVOH;
};

358
libraries/AP_DAL/LogStructure.h Normal file
View File

@ -0,0 +1,358 @@
#pragma once
#include <AP_Logger/LogStructure.h>
#include <AP_Math/vector3.h>
#define LOG_IDS_FROM_DAL \
LOG_RFRH_MSG, \
LOG_RFRF_MSG, \
LOG_REV2_MSG, \
LOG_RSO2_MSG, \
LOG_RWA2_MSG, \
LOG_REV3_MSG, \
LOG_RSO3_MSG, \
LOG_RWA3_MSG, \
LOG_REY3_MSG, \
LOG_RFRN_MSG, \
LOG_RISH_MSG, \
LOG_RISI_MSG, \
LOG_RBRH_MSG, \
LOG_RBRI_MSG, \
LOG_RRNH_MSG, \
LOG_RRNI_MSG, \
LOG_RGPH_MSG, \
LOG_RGPI_MSG, \
LOG_RGPJ_MSG, \
LOG_RASH_MSG, \
LOG_RASI_MSG, \
LOG_RBCH_MSG, \
LOG_RBCI_MSG, \
LOG_RVOH_MSG, \
LOG_RMGH_MSG, \
LOG_RMGI_MSG
// Replay Data Structures
struct log_RFRH {
uint64_t time_us;
uint32_t time_flying_ms;
uint8_t _end;
};
struct log_RFRF {
uint8_t frame_types;
uint8_t _end;
};
struct log_RFRN {
int32_t lat;
int32_t lng;
int32_t alt;
float EAS2TAS;
uint32_t available_memory;
uint8_t state_bitmask;
uint8_t rangefinder_ptr_is_null;
uint8_t get_compass_is_null;
uint8_t airspeed_ptr_is_null;
uint8_t fly_forward;
uint8_t vehicle_class;
uint8_t ahrs_airspeed_sensor_enabled;
uint8_t ekf_type;
uint8_t _end;
};
// Replay Data Structure - Inertial Sensor header
struct log_RISH {
uint16_t loop_rate_hz;
uint8_t primary_gyro;
uint8_t primary_accel;
float loop_delta_t;
uint8_t accel_count;
uint8_t gyro_count;
uint8_t _end;
};
// Replay Data Structure - Inertial Sensor instance data
struct log_RISI {
Vector3f delta_velocity;
Vector3f delta_angle;
float delta_velocity_dt;
float delta_angle_dt;
uint8_t use_accel:1;
uint8_t use_gyro:1;
uint8_t get_delta_velocity_ret:1;
uint8_t get_delta_angle_ret:1;
uint8_t instance;
uint8_t _end;
};
// @LoggerMessage: REV2
// @Description: Replay Event
struct log_REV2 {
uint8_t event;
uint8_t _end;
};
// @LoggerMessage: RSO2
// @Description: Replay Set Origin event
struct log_RSO2 {
int32_t lat;
int32_t lng;
int32_t alt;
uint8_t _end;
};
// @LoggerMessage: RWA2
// @Description: Replay set-default-airspeed event
struct log_RWA2 {
float airspeed;
uint8_t _end;
};
// @LoggerMessage: REV3
// @Description: Replay Event
struct log_REV3 {
uint8_t event;
uint8_t _end;
};
// @LoggerMessage: RSO3
// @Description: Replay Set Origin event
struct log_RSO3 {
int32_t lat;
int32_t lng;
int32_t alt;
uint8_t _end;
};
// @LoggerMessage: RWA3
// @Description: Replay set-default-airspeed event
struct log_RWA3 {
float airspeed;
uint8_t _end;
};
// @LoggerMessage: REY3
// @Description: Replay Euler Yaw event
struct log_REY3 {
float yawangle;
float yawangleerr;
uint32_t timestamp_ms;
uint8_t type;
uint8_t _end;
};
// @LoggerMessage: RBRH
// @Description: Replay Data Barometer Header
struct log_RBRH {
uint8_t primary;
uint8_t num_instances;
uint8_t _end;
};
// @LoggerMessage: RBRI
// @Description: Replay Data Barometer Instance
struct log_RBRI {
uint32_t last_update_ms;
float altitude; // from get_altitude
bool healthy;
uint8_t instance;
uint8_t _end;
};
// @LoggerMessage: RRNH
// @Description: Replay Data Rangefinder Header
struct log_RRNH {
// this is rotation-pitch-270!
int16_t ground_clearance_cm;
int16_t max_distance_cm;
uint16_t backend_mask;
uint8_t _end;
};
// @LoggerMessage: RRNI
// @Description: Replay Data Rangefinder Instance
struct log_RRNI {
Vector3f pos_offset;
uint16_t distance_cm;
uint8_t orientation;
uint8_t status;
uint8_t instance;
uint8_t _end;
};
// @LoggerMessage: RGPH
// @Description: Replay Data GPS Header
struct log_RGPH {
uint8_t num_sensors;
uint8_t primary_sensor;
uint8_t _end;
};
// @LoggerMessage: RGPI
// @Description: Replay Data GPS Instance
struct log_RGPI {
uint32_t last_message_time_ms;
int32_t lat;
int32_t lng;
int32_t alt;
float hacc;
float vacc;
float lag_sec;
uint16_t hdop;
uint8_t status;
uint8_t have_vertical_velocity;
uint8_t horizontal_accuracy_returncode;
uint8_t vertical_accuracy_returncode;
uint8_t num_sats;
uint8_t get_lag_returncode;
uint8_t instance;
uint8_t _end;
};
// @LoggerMessage: RGPJ
// @Description: Replay Data GPS Instance - more data
struct log_RGPJ {
Vector3f velocity;
uint32_t speed_accuracy_returncode;
float sacc;
float yaw_deg;
float yaw_accuracy_deg;
uint8_t gps_yaw_deg_returncode;
uint8_t instance;
uint8_t _end;
};
// Replay Data Structure - Airspeed Sensor header
struct log_RASH {
uint8_t num_sensors;
uint8_t primary;
uint8_t _end;
};
// Replay Data Structure - Airspeed Sensor instance
struct log_RASI {
float airspeed;
uint32_t last_update_ms;
bool healthy;
bool use;
uint8_t instance;
uint8_t _end;
};
// @LoggerMessage: RMGH
// @Description: Replay Data Magnetometer Header
struct log_RMGH {
float declination;
uint8_t count;
bool auto_declination_enabled;
uint8_t num_enabled;
bool learn_offsets_enabled;
bool consistent;
uint8_t _end;
};
// @LoggerMessage: RMGI
// @Description: Replay Data Magnetometer Instance
struct log_RMGI {
uint32_t last_update_usec;
Vector3f offsets;
Vector3f field;
bool use_for_yaw;
bool healthy;
bool have_scale_factor;
uint8_t instance;
uint8_t _end;
};
// @LoggerMessage: RBCH
// @Description: Replay Data Beacon Header
struct log_RBCH {
Vector3f vehicle_position_ned;
float accuracy_estimate;
int32_t origin_lat;
int32_t origin_lng;
int32_t origin_alt;
bool get_vehicle_position_ned_returncode;
uint8_t count;
bool get_origin_returncode;
uint8_t ptr_is_nullptr;
uint8_t _end;
};
// @LoggerMessage: RBCI
// @Description: Replay Data Beacon Instance
struct log_RBCI {
uint32_t last_update_ms;
Vector3f position;
float distance;
uint8_t healthy;
uint8_t instance;
uint8_t _end;
};
// @LoggerMessage: RVOH
// @Description: Replay Data Visual Odometry data
struct log_RVOH {
Vector3f pos_offset;
uint32_t delay_ms;
uint8_t healthy;
bool enabled;
uint8_t ptr_is_nullptr;
uint8_t _end;
};
#define RLOG_SIZE(sname) 3+offsetof(struct log_ ##sname,_end)
#define LOG_STRUCTURE_FROM_DAL \
{ LOG_RFRH_MSG, RLOG_SIZE(RFRH), \
"RFRH", "QI", "TimeUS,TF", "s-", "F-" }, \
{ LOG_RFRF_MSG, RLOG_SIZE(RFRF), \
"RFRF", "B", "FTypes", "-", "-" }, \
{ LOG_RFRN_MSG, RLOG_SIZE(RFRN), \
"RFRN", "IIIfIBBBBBBBB", "HLat,HLon,HAlt,E2T,AM,State,NlRF,NlCRP,NlAS,FF,VC,ASE,EKT", "DUm??????????", "GGB----------" }, \
{ LOG_REV2_MSG, RLOG_SIZE(REV2), \
"REV2", "B", "Event", "-", "-" }, \
{ LOG_RSO2_MSG, RLOG_SIZE(RSO2), \
"RSO2", "III", "Lat,Lon,Alt", "DUm", "GGB" }, \
{ LOG_RWA2_MSG, RLOG_SIZE(RWA2), \
"RWA2", "f", "Airspeed", "n", "0" }, \
{ LOG_REV3_MSG, RLOG_SIZE(REV3), \
"REV3", "B", "Event", "-", "-" }, \
{ LOG_RSO3_MSG, RLOG_SIZE(RSO3), \
"RSO3", "III", "Lat,Lon,Alt", "DUm", "GGB" }, \
{ LOG_RWA3_MSG, RLOG_SIZE(RWA3), \
"RWA3", "f", "Airspeed", "n", "0" }, \
{ LOG_REY3_MSG, RLOG_SIZE(REY3), \
"REY3", "ffIB", "yawangle,yawangleerr,timestamp_ms,type", "???-", "???-" }, \
{ LOG_RISH_MSG, RLOG_SIZE(RISH), \
"RISH", "HBBfBB", "LR,PG,PA,LD,AC,GC", "------", "------" }, \
{ LOG_RISI_MSG, RLOG_SIZE(RISI), \
"RISI", "ffffffffBB", "DVX,DVY,DVZ,DAX,DAY,DAZ,DVDT,DADT,Flags,I", "---------#", "----------" }, \
{ LOG_RASH_MSG, RLOG_SIZE(RASH), \
"RASH", "BB", "Primary,NumInst", "--", "--" }, \
{ LOG_RASI_MSG, RLOG_SIZE(RASI), \
"RASI", "fIBBB", "pd,UpdateMS,H,Use,I", "----#", "-----" }, \
{ LOG_RBRH_MSG, RLOG_SIZE(RBRH), \
"RBRH", "BB", "Primary,NumInst", "--", "--" }, \
{ LOG_RBRI_MSG, RLOG_SIZE(RBRI), \
"RBRI", "IfBB", "LastUpdate,Alt,H,I", "---#", "----" }, \
{ LOG_RRNH_MSG, RLOG_SIZE(RRNH), \
"RRNH", "hhH", "GCl,MaxD,BMask", "???", "???" }, \
{ LOG_RRNI_MSG, RLOG_SIZE(RRNI), \
"RRNI", "fffHBBB", "PX,PY,PZ,Dist,Orient,Status,I", "------#", "-------" }, \
{ LOG_RGPH_MSG, RLOG_SIZE(RGPH), \
"RGPH", "BB", "NumInst,Primary", "--", "--" }, \
{ LOG_RGPI_MSG, RLOG_SIZE(RGPI), \
"RGPI", "IiiifffHBBBBBBB", "LMT,lat,lon,alt,ha,va,lg,hdp,st,hvv,harc,varc,ns,lgrc,I", "--------------#", "---------------" }, \
{ LOG_RGPJ_MSG, RLOG_SIZE(RGPJ), \
"RGPJ", "fffIfffBB", "vx,vy,vz,sarc,sa,yd,yda,ydrc,I", "--------#", "---------" }, \
{ LOG_RMGH_MSG, RLOG_SIZE(RMGH), \
"RMGH", "BBfBBB", "Dec,NumInst,AutoDec,NumEna,LOE,C", "------", "------" }, \
{ LOG_RMGI_MSG, RLOG_SIZE(RMGI), \
"RMGI", "IffffffBBBB", "LU,OX,OY,OZ,FX,FY,FZ,UFY,H,HSF,I", "----------#", "-----------" }, \
{ LOG_RBCH_MSG, RLOG_SIZE(RBCH), \
"RBCH", "ffffiiiBBBB", "PX,PY,PZ,AE,OLat,OLng,OAlt,GVPR,NumInst,ORet,NPtr", "-----------", "-----------" }, \
{ 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", "-------", "-------" },