mirror of https://github.com/ArduPilot/ardupilot
408 lines
12 KiB
C++
408 lines
12 KiB
C++
#pragma once
|
|
|
|
#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"
|
|
|
|
#include <stdio.h>
|
|
#include <stdint.h>
|
|
#include <cstddef>
|
|
|
|
|
|
#define DAL_CORE(c) AP::dal().logging_core(c)
|
|
|
|
class NavEKF2;
|
|
class NavEKF3;
|
|
|
|
class AP_DAL {
|
|
public:
|
|
|
|
enum class FrameType : uint8_t {
|
|
InitialiseFilterEKF2 = 1U<<0,
|
|
UpdateFilterEKF2 = 1U<<1,
|
|
InitialiseFilterEKF3 = 1<<2,
|
|
UpdateFilterEKF3 = 1<<3,
|
|
LogWriteEKF2 = 1<<4,
|
|
LogWriteEKF3 = 1<<5,
|
|
};
|
|
|
|
enum class Event : uint8_t {
|
|
resetGyroBias = 0,
|
|
resetHeightDatum = 1,
|
|
//setInhibitGPS = 2, // removed
|
|
//setTakeoffExpected = 3, // removed
|
|
//unsetTakeoffExpected = 4, // removed
|
|
//setTouchdownExpected = 5, // removed
|
|
//unsetTouchdownExpected = 6, // removed
|
|
//setInhibitGpsVertVelUse = 7, // removed
|
|
//unsetInhibitGpsVertVelUse = 8, // removed
|
|
setTerrainHgtStable = 9,
|
|
unsetTerrainHgtStable = 10,
|
|
requestYawReset = 11,
|
|
checkLaneSwitch = 12,
|
|
setSourceSet0 = 13,
|
|
setSourceSet1 = 14,
|
|
setSourceSet2 = 15,
|
|
};
|
|
|
|
// 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_NOTHROW AP_DAL();
|
|
}
|
|
return _singleton;
|
|
}
|
|
|
|
void start_frame(FrameType frametype);
|
|
void end_frame(void);
|
|
uint64_t micros64() const { return _RFRH.time_us; }
|
|
uint32_t micros() const { return _micros; }
|
|
uint32_t millis() const { return _millis; }
|
|
|
|
void log_event2(Event event);
|
|
void log_SetOriginLLH2(const Location &loc);
|
|
void log_writeDefaultAirSpeed2(const float aspeed, const float uncertainty);
|
|
|
|
void log_event3(Event event);
|
|
void log_SetOriginLLH3(const Location &loc);
|
|
void log_SetLatLng(const Location &loc, float posAccuracy, uint32_t timestamp_ms);
|
|
|
|
void log_writeDefaultAirSpeed3(const float aspeed, const float uncertainty);
|
|
void log_writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t timeStamp_ms, uint8_t type);
|
|
|
|
enum class StateMask {
|
|
ARMED = (1U<<0),
|
|
};
|
|
|
|
// EKF ID for timing checks
|
|
enum class EKFType : uint8_t {
|
|
EKF2 = 0,
|
|
EKF3 = 1,
|
|
};
|
|
|
|
// check if we are low on CPU for this core
|
|
bool ekf_low_time_remaining(EKFType etype, uint8_t core);
|
|
|
|
// returns armed state for the current frame
|
|
bool get_armed() const { return _RFRN.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.
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
|
uint32_t available_memory() const { return _RFRN.available_memory + 10240; }
|
|
#else
|
|
uint32_t available_memory() const { return _RFRN.available_memory; }
|
|
#endif
|
|
|
|
int8_t get_ekf_type(void) const {
|
|
return _RFRN.ekf_type;
|
|
}
|
|
|
|
int snprintf(char* str, size_t size, const char *format, ...) const;
|
|
|
|
// copied in AP_HAL/Util.h
|
|
enum class MemoryType : uint8_t {
|
|
DMA_SAFE = 0,
|
|
FAST = 1,
|
|
};
|
|
void *malloc_type(size_t size, MemoryType mem_type) const;
|
|
void free_type(void *ptr, size_t size, MemoryType memtype) const;
|
|
|
|
AP_DAL_InertialSensor &ins() { return _ins; }
|
|
AP_DAL_Baro &baro() { return _baro; }
|
|
AP_DAL_GPS &gps() { return _gps; }
|
|
|
|
#if AP_RANGEFINDER_ENABLED
|
|
AP_DAL_RangeFinder *rangefinder() {
|
|
return _rangefinder;
|
|
}
|
|
#endif
|
|
|
|
AP_DAL_Airspeed *airspeed() {
|
|
return _airspeed;
|
|
}
|
|
#if AP_BEACON_ENABLED
|
|
AP_DAL_Beacon *beacon() {
|
|
return _beacon;
|
|
}
|
|
#endif
|
|
#if HAL_VISUALODOM_ENABLED
|
|
AP_DAL_VisualOdom *visualodom() {
|
|
return _visualodom;
|
|
}
|
|
#endif
|
|
|
|
AP_DAL_Compass &compass() { return _compass; }
|
|
|
|
// 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;
|
|
}
|
|
|
|
bool get_takeoff_expected(void) const {
|
|
return _RFRN.takeoff_expected;
|
|
}
|
|
|
|
bool get_touchdown_expected(void) const {
|
|
return _RFRN.touchdown_expected;
|
|
}
|
|
|
|
// for EKF usage to enable takeoff expected to true
|
|
void set_takeoff_expected();
|
|
|
|
// get ahrs trim
|
|
const Vector3f &get_trim() const {
|
|
return _RFRN.ahrs_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 class Location &get_home(void) const {
|
|
return _home;
|
|
}
|
|
|
|
uint32_t get_time_flying_ms(void) const {
|
|
return _RFRH.time_flying_ms;
|
|
}
|
|
|
|
bool opticalflow_enabled(void) const {
|
|
return _RFRN.opticalflow_enabled;
|
|
}
|
|
|
|
bool wheelencoder_enabled(void) const {
|
|
return _RFRN.wheelencoder_enabled;
|
|
}
|
|
|
|
// log optical flow data
|
|
void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride);
|
|
|
|
// log external nav data
|
|
void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms);
|
|
void writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms);
|
|
|
|
// log wheel odometry data
|
|
void writeWheelOdom(float delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset, float radius);
|
|
void writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset);
|
|
|
|
// Replay support:
|
|
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_RFRF &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
|
|
|
|
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) {
|
|
if (_airspeed == nullptr) {
|
|
_airspeed = NEW_NOTHROW AP_DAL_Airspeed;
|
|
}
|
|
_airspeed->handle_message(msg);
|
|
}
|
|
void handle_message(const log_RASI &msg) {
|
|
if (_airspeed == nullptr) {
|
|
_airspeed = NEW_NOTHROW AP_DAL_Airspeed;
|
|
}
|
|
_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) {
|
|
#if AP_RANGEFINDER_ENABLED
|
|
if (_rangefinder == nullptr) {
|
|
_rangefinder = NEW_NOTHROW AP_DAL_RangeFinder;
|
|
}
|
|
_rangefinder->handle_message(msg);
|
|
#endif
|
|
}
|
|
void handle_message(const log_RRNI &msg) {
|
|
#if AP_RANGEFINDER_ENABLED
|
|
if (_rangefinder == nullptr) {
|
|
_rangefinder = NEW_NOTHROW AP_DAL_RangeFinder;
|
|
}
|
|
_rangefinder->handle_message(msg);
|
|
#endif
|
|
}
|
|
|
|
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) {
|
|
#if AP_BEACON_ENABLED
|
|
if (_beacon == nullptr) {
|
|
_beacon = NEW_NOTHROW AP_DAL_Beacon;
|
|
}
|
|
_beacon->handle_message(msg);
|
|
#endif
|
|
}
|
|
void handle_message(const log_RBCI &msg) {
|
|
#if AP_BEACON_ENABLED
|
|
if (_beacon == nullptr) {
|
|
_beacon = NEW_NOTHROW AP_DAL_Beacon;
|
|
}
|
|
_beacon->handle_message(msg);
|
|
#endif
|
|
}
|
|
void handle_message(const log_RVOH &msg) {
|
|
#if HAL_VISUALODOM_ENABLED
|
|
if (_visualodom == nullptr) {
|
|
_visualodom = NEW_NOTHROW AP_DAL_VisualOdom;
|
|
}
|
|
_visualodom->handle_message(msg);
|
|
#endif
|
|
}
|
|
void handle_message(const log_ROFH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
|
|
void handle_message(const log_REPH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
|
|
void handle_message(const log_REVH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
|
|
void handle_message(const log_RWOH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
|
|
void handle_message(const log_RBOH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
|
|
void handle_message(const log_RSLL &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
|
|
|
|
// map core number for replay
|
|
uint8_t logging_core(uint8_t c) const;
|
|
|
|
#if HAL_LOGGING_ENABLED
|
|
// 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);
|
|
#endif
|
|
|
|
private:
|
|
|
|
static AP_DAL *_singleton;
|
|
|
|
// framing structures
|
|
struct log_RFRH _RFRH;
|
|
struct log_RFRF _RFRF;
|
|
struct log_RFRN _RFRN;
|
|
|
|
// push-based sensor structures
|
|
struct log_ROFH _ROFH;
|
|
struct log_REPH _REPH;
|
|
struct log_REVH _REVH;
|
|
struct log_RWOH _RWOH;
|
|
struct log_RBOH _RBOH;
|
|
struct log_RSLL _RSLL;
|
|
|
|
// cached variables for speed:
|
|
uint32_t _micros;
|
|
uint32_t _millis;
|
|
|
|
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;
|
|
#if AP_RANGEFINDER_ENABLED
|
|
AP_DAL_RangeFinder *_rangefinder;
|
|
#endif
|
|
AP_DAL_Compass _compass;
|
|
AP_DAL_Airspeed *_airspeed;
|
|
#if AP_BEACON_ENABLED
|
|
AP_DAL_Beacon *_beacon;
|
|
#endif
|
|
#if HAL_VISUALODOM_ENABLED
|
|
AP_DAL_VisualOdom *_visualodom;
|
|
#endif
|
|
|
|
static bool logging_started;
|
|
static bool force_write;
|
|
|
|
bool ekf2_init_done;
|
|
bool ekf3_init_done;
|
|
|
|
void init_sensors(void);
|
|
bool init_done;
|
|
};
|
|
|
|
#if HAL_LOGGING_ENABLED
|
|
#define WRITE_REPLAY_BLOCK(sname,v) AP_DAL::WriteLogMessage(LOG_## sname ##_MSG, &v, nullptr, offsetof(log_ ##sname, _end))
|
|
#define WRITE_REPLAY_BLOCK_IFCHANGED(sname,v,old) do { static_assert(sizeof(v) == sizeof(old), "types must match"); \
|
|
AP_DAL::WriteLogMessage(LOG_## sname ##_MSG, &v, &old, offsetof(log_ ##sname, _end)); } \
|
|
while (0)
|
|
#else
|
|
#define WRITE_REPLAY_BLOCK(sname,v) do { (void)v; } while (false)
|
|
#define WRITE_REPLAY_BLOCK_IFCHANGED(sname,v,old) do { (void)old; } while (false)
|
|
#endif
|
|
|
|
namespace AP {
|
|
AP_DAL &dal();
|
|
};
|
|
|
|
// replay printf for debugging
|
|
void rprintf(const char *format, ...);
|
|
|