mirror of https://github.com/ArduPilot/ardupilot
566 lines
16 KiB
C++
566 lines
16 KiB
C++
#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>
|
|
#include <AP_OpticalFlow/AP_OpticalFlow.h>
|
|
#include <AP_WheelEncoder/AP_WheelEncoder.h>
|
|
#include <AP_Vehicle/AP_Vehicle_Type.h>
|
|
#include <AP_NavEKF3/AP_NavEKF3_feature.h>
|
|
|
|
#if APM_BUILD_TYPE(APM_BUILD_Replay)
|
|
#include <AP_NavEKF2/AP_NavEKF2.h>
|
|
#include <AP_NavEKF3/AP_NavEKF3.h>
|
|
#endif
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
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)
|
|
|
|
if (!init_done) {
|
|
init_sensors();
|
|
}
|
|
|
|
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
|
|
#if HAL_LOGGING_ENABLED
|
|
bool logging = AP::logger().logging_started() && AP::logger().allow_start_ekf();
|
|
if (logging && !logging_started) {
|
|
force_write = true;
|
|
}
|
|
logging_started = logging;
|
|
#endif
|
|
|
|
end_frame();
|
|
|
|
_RFRF.frame_types = uint8_t(frametype);
|
|
|
|
#if AP_VEHICLE_ENABLED
|
|
_RFRH.time_flying_ms = AP::vehicle()->get_time_flying_ms();
|
|
#else
|
|
_RFRH.time_flying_ms = 0;
|
|
#endif
|
|
_RFRH.time_us = AP_HAL::micros64();
|
|
WRITE_REPLAY_BLOCK(RFRH, _RFRH);
|
|
|
|
// update RFRN data
|
|
const log_RFRN old = _RFRN;
|
|
_RFRN.armed = hal.util->get_soft_armed();
|
|
_home = ahrs.get_home();
|
|
_RFRN.lat = _home.lat;
|
|
_RFRN.lng = _home.lng;
|
|
_RFRN.alt = _home.alt;
|
|
_RFRN.EAS2TAS = ahrs.get_EAS2TAS();
|
|
_RFRN.vehicle_class = (uint8_t)ahrs.get_vehicle_class();
|
|
_RFRN.fly_forward = ahrs.get_fly_forward();
|
|
_RFRN.takeoff_expected = ahrs.get_takeoff_expected();
|
|
_RFRN.touchdown_expected = ahrs.get_touchdown_expected();
|
|
_RFRN.ahrs_airspeed_sensor_enabled = ahrs.airspeed_sensor_enabled(ahrs.get_active_airspeed_index());
|
|
_RFRN.available_memory = hal.util->available_memory();
|
|
_RFRN.ahrs_trim = ahrs.get_trim();
|
|
#if AP_OPTICALFLOW_ENABLED
|
|
_RFRN.opticalflow_enabled = AP::opticalflow() && AP::opticalflow()->enabled();
|
|
#endif
|
|
_RFRN.wheelencoder_enabled = AP::wheelencoder() && (AP::wheelencoder()->num_sensors() > 0);
|
|
_RFRN.ekf_type = ahrs.get_ekf_type();
|
|
WRITE_REPLAY_BLOCK_IFCHANGED(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();
|
|
if (_airspeed) {
|
|
_airspeed->start_frame();
|
|
}
|
|
#if AP_RANGEFINDER_ENABLED
|
|
if (_rangefinder) {
|
|
_rangefinder->start_frame();
|
|
}
|
|
#endif
|
|
#if AP_BEACON_ENABLED
|
|
if (_beacon) {
|
|
_beacon->start_frame();
|
|
}
|
|
#endif
|
|
#if HAL_VISUALODOM_ENABLED
|
|
if (_visualodom) {
|
|
_visualodom->start_frame();
|
|
}
|
|
#endif
|
|
|
|
// populate some derivative values:
|
|
_micros = _RFRH.time_us;
|
|
_millis = _RFRH.time_us / 1000UL;
|
|
|
|
force_write = false;
|
|
#endif
|
|
}
|
|
|
|
// for EKF usage to enable takeoff expected to true
|
|
void AP_DAL::set_takeoff_expected()
|
|
{
|
|
#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
|
|
AP_AHRS &ahrs = AP::ahrs();
|
|
ahrs.set_takeoff_expected(true);
|
|
#endif
|
|
}
|
|
|
|
/*
|
|
setup optional sensor backends
|
|
*/
|
|
void AP_DAL::init_sensors(void)
|
|
{
|
|
init_done = true;
|
|
bool alloc_failed = false;
|
|
|
|
/*
|
|
we only allocate the DAL backends if we had at least one sensor
|
|
at the time we startup the EKF
|
|
*/
|
|
|
|
#if AP_RANGEFINDER_ENABLED
|
|
auto *rng = AP::rangefinder();
|
|
if (rng && rng->num_sensors() > 0) {
|
|
alloc_failed |= (_rangefinder = NEW_NOTHROW AP_DAL_RangeFinder) == nullptr;
|
|
}
|
|
#endif
|
|
|
|
#if AP_AIRSPEED_ENABLED
|
|
auto *aspeed = AP::airspeed();
|
|
if (aspeed != nullptr && aspeed->get_num_sensors() > 0) {
|
|
alloc_failed |= (_airspeed = NEW_NOTHROW AP_DAL_Airspeed) == nullptr;
|
|
}
|
|
#endif
|
|
|
|
#if AP_BEACON_ENABLED
|
|
auto *bcn = AP::beacon();
|
|
if (bcn != nullptr && bcn->enabled()) {
|
|
alloc_failed |= (_beacon = NEW_NOTHROW AP_DAL_Beacon) == nullptr;
|
|
}
|
|
#endif
|
|
|
|
#if HAL_VISUALODOM_ENABLED
|
|
auto *vodom = AP::visualodom();
|
|
if (vodom != nullptr && vodom->enabled()) {
|
|
alloc_failed |= (_visualodom = NEW_NOTHROW AP_DAL_VisualOdom) == nullptr;
|
|
}
|
|
#endif
|
|
|
|
if (alloc_failed) {
|
|
AP_BoardConfig::allocation_error("DAL backends");
|
|
}
|
|
}
|
|
|
|
/*
|
|
end a frame. Must be called on all events and injections of data (eg
|
|
flow) and before starting a new frame
|
|
*/
|
|
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::Event 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 aspeed, const float uncertainty)
|
|
{
|
|
#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
|
|
struct log_RWA2 pkt{
|
|
airspeed: aspeed,
|
|
uncertainty: uncertainty,
|
|
};
|
|
WRITE_REPLAY_BLOCK(RWA2, pkt);
|
|
#endif
|
|
}
|
|
|
|
void AP_DAL::log_event3(AP_DAL::Event 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 aspeed, const float uncertainty)
|
|
{
|
|
#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
|
|
struct log_RWA3 pkt{
|
|
airspeed: aspeed,
|
|
uncertainty: uncertainty
|
|
};
|
|
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, ...) const
|
|
{
|
|
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, MemoryType mem_type) const
|
|
{
|
|
return hal.util->malloc_type(size, AP_HAL::Util::Memory_Type(mem_type));
|
|
}
|
|
void AP_DAL::free_type(void *ptr, size_t size, MemoryType mem_type) const
|
|
{
|
|
return hal.util->free_type(ptr, size, AP_HAL::Util::Memory_Type(mem_type));
|
|
}
|
|
|
|
// 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
|
|
}
|
|
|
|
#if HAL_LOGGING_ENABLED
|
|
// 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;
|
|
}
|
|
}
|
|
#endif
|
|
|
|
/*
|
|
check if we are low on CPU for this core. This needs to capture the
|
|
timing of running the cores
|
|
*/
|
|
bool AP_DAL::ekf_low_time_remaining(EKFType etype, uint8_t core)
|
|
{
|
|
static_assert(MAX_EKF_CORES <= 4, "max 4 EKF cores supported");
|
|
const uint8_t mask = (1U<<(core+(uint8_t(etype)*4)));
|
|
#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
|
|
/*
|
|
if we have used more than 1/3 of the time for a loop then we
|
|
return true, indicating that we are low on CPU. This changes the
|
|
scheduling of fusion between lanes
|
|
*/
|
|
const auto &imu = AP::ins();
|
|
if ((AP_HAL::micros() - imu.get_last_update_usec())*1.0e-6 > imu.get_loop_delta_t()*0.33) {
|
|
_RFRF.core_slow |= mask;
|
|
} else {
|
|
_RFRF.core_slow &= ~mask;
|
|
}
|
|
#endif
|
|
return (_RFRF.core_slow & mask) != 0;
|
|
}
|
|
|
|
// log optical flow data
|
|
void AP_DAL::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
|
|
{
|
|
end_frame();
|
|
|
|
const log_ROFH old = _ROFH;
|
|
_ROFH.rawFlowQuality = rawFlowQuality;
|
|
_ROFH.rawFlowRates = rawFlowRates;
|
|
_ROFH.rawGyroRates = rawGyroRates;
|
|
_ROFH.msecFlowMeas = msecFlowMeas;
|
|
_ROFH.posOffset = posOffset;
|
|
_ROFH.heightOverride = heightOverride;
|
|
WRITE_REPLAY_BLOCK_IFCHANGED(ROFH, _ROFH, old);
|
|
}
|
|
|
|
// log external navigation data
|
|
void AP_DAL::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms)
|
|
{
|
|
end_frame();
|
|
|
|
const log_REPH old = _REPH;
|
|
_REPH.pos = pos;
|
|
_REPH.quat = quat;
|
|
_REPH.posErr = posErr;
|
|
_REPH.angErr = angErr;
|
|
_REPH.timeStamp_ms = timeStamp_ms;
|
|
_REPH.delay_ms = delay_ms;
|
|
_REPH.resetTime_ms = resetTime_ms;
|
|
WRITE_REPLAY_BLOCK_IFCHANGED(REPH, _REPH, old);
|
|
}
|
|
|
|
void AP_DAL::log_SetLatLng(const Location &loc, float posAccuracy, uint32_t timestamp_ms)
|
|
{
|
|
end_frame();
|
|
const log_RSLL old = _RSLL;
|
|
_RSLL.lat = loc.lat;
|
|
_RSLL.lng = loc.lng;
|
|
_RSLL.posAccSD = posAccuracy;
|
|
_RSLL.timestamp_ms = timestamp_ms;
|
|
WRITE_REPLAY_BLOCK_IFCHANGED(RSLL, _RSLL, old);
|
|
}
|
|
|
|
// log external velocity data
|
|
void AP_DAL::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms)
|
|
{
|
|
end_frame();
|
|
|
|
const log_REVH old = _REVH;
|
|
_REVH.vel = vel;
|
|
_REVH.err = err;
|
|
_REVH.timeStamp_ms = timeStamp_ms;
|
|
_REVH.delay_ms = delay_ms;
|
|
WRITE_REPLAY_BLOCK_IFCHANGED(REVH, _REVH, old);
|
|
|
|
}
|
|
|
|
// log wheel odometry data
|
|
void AP_DAL::writeWheelOdom(float delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset, float radius)
|
|
{
|
|
end_frame();
|
|
|
|
const log_RWOH old = _RWOH;
|
|
_RWOH.delAng = delAng;
|
|
_RWOH.delTime = delTime;
|
|
_RWOH.timeStamp_ms = timeStamp_ms;
|
|
_RWOH.posOffset = posOffset;
|
|
_RWOH.radius = radius;
|
|
WRITE_REPLAY_BLOCK_IFCHANGED(RWOH, _RWOH, old);
|
|
}
|
|
|
|
void AP_DAL::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset)
|
|
{
|
|
end_frame();
|
|
|
|
const log_RBOH old = _RBOH;
|
|
_RBOH.quality = quality;
|
|
_RBOH.delPos = delPos;
|
|
_RBOH.delAng = delAng;
|
|
_RBOH.delTime = delTime;
|
|
_RBOH.timeStamp_ms = timeStamp_ms;
|
|
WRITE_REPLAY_BLOCK_IFCHANGED(RBOH, _RBOH, old);
|
|
}
|
|
|
|
#if APM_BUILD_TYPE(APM_BUILD_Replay)
|
|
/*
|
|
handle frame message. This message triggers the EKF2/EKF3 updates and logging
|
|
*/
|
|
void AP_DAL::handle_message(const log_RFRF &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
|
|
{
|
|
_RFRF.core_slow = msg.core_slow;
|
|
|
|
/*
|
|
note that we need to handle the case of LOG_REPLAY=1 with
|
|
LOG_DISARMED=0. To handle this we need to record the start of the filter
|
|
*/
|
|
const uint8_t frame_types = msg.frame_types;
|
|
if (frame_types & uint8_t(AP_DAL::FrameType::InitialiseFilterEKF2)) {
|
|
ekf2_init_done = ekf2.InitialiseFilter();
|
|
}
|
|
if (frame_types & uint8_t(AP_DAL::FrameType::UpdateFilterEKF2)) {
|
|
if (!ekf2_init_done) {
|
|
ekf2_init_done = ekf2.InitialiseFilter();
|
|
}
|
|
if (ekf2_init_done) {
|
|
ekf2.UpdateFilter();
|
|
}
|
|
}
|
|
if (frame_types & uint8_t(AP_DAL::FrameType::InitialiseFilterEKF3)) {
|
|
ekf3_init_done = ekf3.InitialiseFilter();
|
|
}
|
|
if (frame_types & uint8_t(AP_DAL::FrameType::UpdateFilterEKF3)) {
|
|
if (!ekf3_init_done) {
|
|
ekf3_init_done = ekf3.InitialiseFilter();
|
|
}
|
|
if (ekf3_init_done) {
|
|
ekf3.UpdateFilter();
|
|
}
|
|
}
|
|
if (frame_types & uint8_t(AP_DAL::FrameType::LogWriteEKF2)) {
|
|
ekf2.Log_Write();
|
|
}
|
|
if (frame_types & uint8_t(AP_DAL::FrameType::LogWriteEKF3)) {
|
|
ekf3.Log_Write();
|
|
}
|
|
}
|
|
|
|
/*
|
|
handle optical flow message
|
|
*/
|
|
void AP_DAL::handle_message(const log_ROFH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
|
|
{
|
|
_ROFH = msg;
|
|
ekf2.writeOptFlowMeas(msg.rawFlowQuality, msg.rawFlowRates, msg.rawGyroRates, msg.msecFlowMeas, msg.posOffset, msg.heightOverride);
|
|
#if EK3_FEATURE_OPTFLOW_FUSION
|
|
ekf3.writeOptFlowMeas(msg.rawFlowQuality, msg.rawFlowRates, msg.rawGyroRates, msg.msecFlowMeas, msg.posOffset, msg.heightOverride);
|
|
#endif
|
|
}
|
|
|
|
/*
|
|
handle external position data
|
|
*/
|
|
void AP_DAL::handle_message(const log_REPH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
|
|
{
|
|
_REPH = msg;
|
|
ekf2.writeExtNavData(msg.pos, msg.quat, msg.posErr, msg.angErr, msg.timeStamp_ms, msg.delay_ms, msg.resetTime_ms);
|
|
ekf3.writeExtNavData(msg.pos, msg.quat, msg.posErr, msg.angErr, msg.timeStamp_ms, msg.delay_ms, msg.resetTime_ms);
|
|
}
|
|
|
|
/*
|
|
handle external velocity data
|
|
*/
|
|
void AP_DAL::handle_message(const log_REVH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
|
|
{
|
|
_REVH = msg;
|
|
ekf2.writeExtNavVelData(msg.vel, msg.err, msg.timeStamp_ms, msg.delay_ms);
|
|
ekf3.writeExtNavVelData(msg.vel, msg.err, msg.timeStamp_ms, msg.delay_ms);
|
|
}
|
|
|
|
/*
|
|
handle wheel odometry data
|
|
*/
|
|
void AP_DAL::handle_message(const log_RWOH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
|
|
{
|
|
_RWOH = msg;
|
|
// note that EKF2 does not support wheel odometry
|
|
ekf3.writeWheelOdom(msg.delAng, msg.delTime, msg.timeStamp_ms, msg.posOffset, msg.radius);
|
|
}
|
|
|
|
/*
|
|
handle body frame odometry
|
|
*/
|
|
void AP_DAL::handle_message(const log_RBOH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
|
|
{
|
|
_RBOH = msg;
|
|
// note that EKF2 does not support body frame odometry
|
|
ekf3.writeBodyFrameOdom(msg.quality, msg.delPos, msg.delAng, msg.delTime, msg.timeStamp_ms, msg.delay_ms, msg.posOffset);
|
|
}
|
|
|
|
/*
|
|
handle position reset
|
|
*/
|
|
void AP_DAL::handle_message(const log_RSLL &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
|
|
{
|
|
_RSLL = msg;
|
|
// note that EKF2 does not support body frame odometry
|
|
const Location loc {msg.lat, msg.lng, 0, Location::AltFrame::ABSOLUTE };
|
|
ekf3.setLatLng(loc, msg.posAccSD, msg.timestamp_ms);
|
|
}
|
|
#endif // APM_BUILD_Replay
|
|
|
|
namespace AP {
|
|
|
|
AP_DAL &dal()
|
|
{
|
|
return *AP_DAL::get_singleton();
|
|
}
|
|
|
|
};
|
|
|
|
/*
|
|
replay printf. To debug replay failures add rprintf() calls into
|
|
EKF2/EKF3 and compare /tmp/replay.log to /tmp/real.log
|
|
*/
|
|
void rprintf(const char *format, ...)
|
|
{
|
|
|
|
#if (APM_BUILD_TYPE(APM_BUILD_Replay) || CONFIG_HAL_BOARD == HAL_BOARD_SITL) && CONFIG_HAL_BOARD != HAL_BOARD_CHIBIOS
|
|
#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
|
|
}
|
|
|