ardupilot/libraries/AP_DAL/AP_DAL.cpp

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
}