From e116b1ff0aafec86a0b182f2a1915b05937d59e9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 6 Nov 2020 10:22:38 +1100 Subject: [PATCH] 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 --- libraries/AP_DAL/AP_DAL.cpp | 261 +++++++++++++++ libraries/AP_DAL/AP_DAL.h | 312 ++++++++++++++++++ libraries/AP_DAL/AP_DAL_Airspeed.cpp | 36 +++ libraries/AP_DAL/AP_DAL_Airspeed.h | 65 ++++ libraries/AP_DAL/AP_DAL_Baro.cpp | 37 +++ libraries/AP_DAL/AP_DAL_Baro.h | 59 ++++ libraries/AP_DAL/AP_DAL_Beacon.cpp | 47 +++ libraries/AP_DAL/AP_DAL_Beacon.h | 81 +++++ libraries/AP_DAL/AP_DAL_Compass.cpp | 40 +++ libraries/AP_DAL/AP_DAL_Compass.h | 74 +++++ libraries/AP_DAL/AP_DAL_GPS.cpp | 60 ++++ libraries/AP_DAL/AP_DAL_GPS.h | 155 +++++++++ libraries/AP_DAL/AP_DAL_InertialSensor.cpp | 56 ++++ libraries/AP_DAL/AP_DAL_InertialSensor.h | 86 +++++ libraries/AP_DAL/AP_DAL_RangeFinder.cpp | 104 ++++++ libraries/AP_DAL/AP_DAL_RangeFinder.h | 76 +++++ libraries/AP_DAL/AP_DAL_VisualOdom.cpp | 23 ++ libraries/AP_DAL/AP_DAL_VisualOdom.h | 51 +++ libraries/AP_DAL/LogStructure.h | 358 +++++++++++++++++++++ 19 files changed, 1981 insertions(+) create mode 100644 libraries/AP_DAL/AP_DAL.cpp create mode 100644 libraries/AP_DAL/AP_DAL.h create mode 100644 libraries/AP_DAL/AP_DAL_Airspeed.cpp create mode 100644 libraries/AP_DAL/AP_DAL_Airspeed.h create mode 100644 libraries/AP_DAL/AP_DAL_Baro.cpp create mode 100644 libraries/AP_DAL/AP_DAL_Baro.h create mode 100644 libraries/AP_DAL/AP_DAL_Beacon.cpp create mode 100644 libraries/AP_DAL/AP_DAL_Beacon.h create mode 100644 libraries/AP_DAL/AP_DAL_Compass.cpp create mode 100644 libraries/AP_DAL/AP_DAL_Compass.h create mode 100644 libraries/AP_DAL/AP_DAL_GPS.cpp create mode 100644 libraries/AP_DAL/AP_DAL_GPS.h create mode 100644 libraries/AP_DAL/AP_DAL_InertialSensor.cpp create mode 100644 libraries/AP_DAL/AP_DAL_InertialSensor.h create mode 100644 libraries/AP_DAL/AP_DAL_RangeFinder.cpp create mode 100644 libraries/AP_DAL/AP_DAL_RangeFinder.h create mode 100644 libraries/AP_DAL/AP_DAL_VisualOdom.cpp create mode 100644 libraries/AP_DAL/AP_DAL_VisualOdom.h create mode 100644 libraries/AP_DAL/LogStructure.h diff --git a/libraries/AP_DAL/AP_DAL.cpp b/libraries/AP_DAL/AP_DAL.cpp new file mode 100644 index 0000000000..60ea78d447 --- /dev/null +++ b/libraries/AP_DAL/AP_DAL.cpp @@ -0,0 +1,261 @@ +#include "AP_DAL.h" + +#include +#include +#include +#include + +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 + +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 +} + diff --git a/libraries/AP_DAL/AP_DAL.h b/libraries/AP_DAL/AP_DAL.h new file mode 100644 index 0000000000..a0d7cede21 --- /dev/null +++ b/libraries/AP_DAL/AP_DAL.h @@ -0,0 +1,312 @@ +#pragma once + +#include + +#include + +#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, ...); + diff --git a/libraries/AP_DAL/AP_DAL_Airspeed.cpp b/libraries/AP_DAL/AP_DAL_Airspeed.cpp new file mode 100644 index 0000000000..15444ffbd9 --- /dev/null +++ b/libraries/AP_DAL/AP_DAL_Airspeed.cpp @@ -0,0 +1,36 @@ +#include "AP_DAL_Airspeed.h" +#include "AP_DAL.h" + +#include + +AP_DAL_Airspeed::AP_DAL_Airspeed() +{ + for (uint8_t i=0; iget_num_sensors(); + _RASH.primary = airspeed->get_primary(); + WRITE_REPLAY_BLOCK_IFCHANGD(RASH, _RASH, old); + + for (uint8_t i=0; ilast_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); + } +} diff --git a/libraries/AP_DAL/AP_DAL_Airspeed.h b/libraries/AP_DAL/AP_DAL_Airspeed.h new file mode 100644 index 0000000000..8436255543 --- /dev/null +++ b/libraries/AP_DAL/AP_DAL_Airspeed.h @@ -0,0 +1,65 @@ +#pragma once + +#include + +#include + +#include + +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]; +}; diff --git a/libraries/AP_DAL/AP_DAL_Baro.cpp b/libraries/AP_DAL/AP_DAL_Baro.cpp new file mode 100644 index 0000000000..6211edb6ac --- /dev/null +++ b/libraries/AP_DAL/AP_DAL_Baro.cpp @@ -0,0 +1,37 @@ +#include "AP_DAL_Baro.h" + +#include +#include "AP_DAL.h" + +AP_DAL_Baro::AP_DAL_Baro() +{ + for (uint8_t i=0; i + +#include + +#include + +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]; +}; + diff --git a/libraries/AP_DAL/AP_DAL_Beacon.cpp b/libraries/AP_DAL/AP_DAL_Beacon.cpp new file mode 100644 index 0000000000..55bb07594d --- /dev/null +++ b/libraries/AP_DAL/AP_DAL_Beacon.cpp @@ -0,0 +1,47 @@ +#include "AP_DAL_Beacon.h" + +#include + +#include +#include "AP_DAL.h" + +AP_DAL_Beacon::AP_DAL_Beacon() +{ + for (uint8_t i=0; icount(); + _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; ibeacon_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); + } +} diff --git a/libraries/AP_DAL/AP_DAL_Beacon.h b/libraries/AP_DAL/AP_DAL_Beacon.h new file mode 100644 index 0000000000..95de2741b2 --- /dev/null +++ b/libraries/AP_DAL/AP_DAL_Beacon.h @@ -0,0 +1,81 @@ +#pragma once + +#include + +#include + +#include + +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; +}; diff --git a/libraries/AP_DAL/AP_DAL_Compass.cpp b/libraries/AP_DAL/AP_DAL_Compass.cpp new file mode 100644 index 0000000000..21a427e22a --- /dev/null +++ b/libraries/AP_DAL/AP_DAL_Compass.cpp @@ -0,0 +1,40 @@ +#include "AP_DAL_Compass.h" + +#include + +#include +#include "AP_DAL.h" + +AP_DAL_Compass::AP_DAL_Compass() +{ + for (uint8_t i=0; i + +#include + +#include + +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]; +}; diff --git a/libraries/AP_DAL/AP_DAL_GPS.cpp b/libraries/AP_DAL/AP_DAL_GPS.cpp new file mode 100644 index 0000000000..6a696c7020 --- /dev/null +++ b/libraries/AP_DAL/AP_DAL_GPS.cpp @@ -0,0 +1,60 @@ +#include "AP_DAL_GPS.h" + +#include +#include "AP_DAL.h" + +AP_DAL_GPS::AP_DAL_GPS() +{ + for (uint8_t i=0; i + +#include + +#include + +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]; +}; diff --git a/libraries/AP_DAL/AP_DAL_InertialSensor.cpp b/libraries/AP_DAL/AP_DAL_InertialSensor.cpp new file mode 100644 index 0000000000..0b1abbdbba --- /dev/null +++ b/libraries/AP_DAL/AP_DAL_InertialSensor.cpp @@ -0,0 +1,56 @@ +#include "AP_DAL_InertialSensor.h" + +#include +#include "AP_DAL.h" + +AP_DAL_InertialSensor::AP_DAL_InertialSensor() +{ + for (uint8_t i=0; i + +#include + +#include + +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); +}; diff --git a/libraries/AP_DAL/AP_DAL_RangeFinder.cpp b/libraries/AP_DAL/AP_DAL_RangeFinder.cpp new file mode 100644 index 0000000000..584c3ea388 --- /dev/null +++ b/libraries/AP_DAL/AP_DAL_RangeFinder.cpp @@ -0,0 +1,104 @@ +#include "AP_DAL_RangeFinder.h" + +#include + +#include +#include "AP_DAL.h" + +AP_DAL_RangeFinder::AP_DAL_RangeFinder() +{ + for (uint8_t i=0; iground_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; iget_backend(i); + if (backend == nullptr) { + break; + } + _RRNH.backend_mask |= (1U<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< + +#include + +#include + +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; +}; diff --git a/libraries/AP_DAL/AP_DAL_VisualOdom.cpp b/libraries/AP_DAL/AP_DAL_VisualOdom.cpp new file mode 100644 index 0000000000..d86e2e195f --- /dev/null +++ b/libraries/AP_DAL/AP_DAL_VisualOdom.cpp @@ -0,0 +1,23 @@ +#include "AP_DAL_VisualOdom.h" + +#include + +#include +#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); +} diff --git a/libraries/AP_DAL/AP_DAL_VisualOdom.h b/libraries/AP_DAL/AP_DAL_VisualOdom.h new file mode 100644 index 0000000000..de537b6d41 --- /dev/null +++ b/libraries/AP_DAL/AP_DAL_VisualOdom.h @@ -0,0 +1,51 @@ +#pragma once + +#include + +#include + +#include + +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; +}; diff --git a/libraries/AP_DAL/LogStructure.h b/libraries/AP_DAL/LogStructure.h new file mode 100644 index 0000000000..6e86a7d1b3 --- /dev/null +++ b/libraries/AP_DAL/LogStructure.h @@ -0,0 +1,358 @@ +#pragma once + +#include +#include + +#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", "-------", "-------" },