mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AP_DAL: cleanups from review
This commit is contained in:
parent
541d11ee05
commit
905ffed7fd
@ -12,10 +12,6 @@
|
|||||||
|
|
||||||
extern const AP_HAL::HAL& hal;
|
extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
enum class FrameItem : uint8_t {
|
|
||||||
AVAILABLE_MEMORY = 0,
|
|
||||||
};
|
|
||||||
|
|
||||||
AP_DAL *AP_DAL::_singleton = nullptr;
|
AP_DAL *AP_DAL::_singleton = nullptr;
|
||||||
|
|
||||||
bool AP_DAL::force_write;
|
bool AP_DAL::force_write;
|
||||||
@ -67,7 +63,7 @@ void AP_DAL::start_frame(AP_DAL::FrameType frametype)
|
|||||||
_RFRN.fly_forward = ahrs.get_fly_forward();
|
_RFRN.fly_forward = ahrs.get_fly_forward();
|
||||||
_RFRN.ahrs_airspeed_sensor_enabled = AP::ahrs().airspeed_sensor_enabled();
|
_RFRN.ahrs_airspeed_sensor_enabled = AP::ahrs().airspeed_sensor_enabled();
|
||||||
_RFRN.available_memory = hal.util->available_memory();
|
_RFRN.available_memory = hal.util->available_memory();
|
||||||
WRITE_REPLAY_BLOCK_IFCHANGD(RFRN, _RFRN, old);
|
WRITE_REPLAY_BLOCK_IFCHANGED(RFRN, _RFRN, old);
|
||||||
|
|
||||||
// update body conversion
|
// update body conversion
|
||||||
_rotation_vehicle_body_to_autopilot_body = ahrs.get_rotation_vehicle_body_to_autopilot_body();
|
_rotation_vehicle_body_to_autopilot_body = ahrs.get_rotation_vehicle_body_to_autopilot_body();
|
||||||
@ -274,7 +270,7 @@ void AP_DAL::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawF
|
|||||||
_ROFH.rawGyroRates = rawGyroRates;
|
_ROFH.rawGyroRates = rawGyroRates;
|
||||||
_ROFH.msecFlowMeas = msecFlowMeas;
|
_ROFH.msecFlowMeas = msecFlowMeas;
|
||||||
_ROFH.posOffset = posOffset;
|
_ROFH.posOffset = posOffset;
|
||||||
WRITE_REPLAY_BLOCK_IFCHANGD(ROFH, _ROFH, old);
|
WRITE_REPLAY_BLOCK_IFCHANGED(ROFH, _ROFH, old);
|
||||||
}
|
}
|
||||||
|
|
||||||
// log external navigation data
|
// log external navigation data
|
||||||
@ -290,7 +286,7 @@ void AP_DAL::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float
|
|||||||
_REPH.timeStamp_ms = timeStamp_ms;
|
_REPH.timeStamp_ms = timeStamp_ms;
|
||||||
_REPH.delay_ms = delay_ms;
|
_REPH.delay_ms = delay_ms;
|
||||||
_REPH.resetTime_ms = resetTime_ms;
|
_REPH.resetTime_ms = resetTime_ms;
|
||||||
WRITE_REPLAY_BLOCK_IFCHANGD(REPH, _REPH, old);
|
WRITE_REPLAY_BLOCK_IFCHANGED(REPH, _REPH, old);
|
||||||
}
|
}
|
||||||
|
|
||||||
// log external velocity data
|
// log external velocity data
|
||||||
@ -303,7 +299,7 @@ void AP_DAL::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeSta
|
|||||||
_REVH.err = err;
|
_REVH.err = err;
|
||||||
_REVH.timeStamp_ms = timeStamp_ms;
|
_REVH.timeStamp_ms = timeStamp_ms;
|
||||||
_REVH.delay_ms = delay_ms;
|
_REVH.delay_ms = delay_ms;
|
||||||
WRITE_REPLAY_BLOCK_IFCHANGD(REVH, _REVH, old);
|
WRITE_REPLAY_BLOCK_IFCHANGED(REVH, _REVH, old);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -318,7 +314,7 @@ void AP_DAL::writeWheelOdom(float delAng, float delTime, uint32_t timeStamp_ms,
|
|||||||
_RWOH.timeStamp_ms = timeStamp_ms;
|
_RWOH.timeStamp_ms = timeStamp_ms;
|
||||||
_RWOH.posOffset = posOffset;
|
_RWOH.posOffset = posOffset;
|
||||||
_RWOH.radius = radius;
|
_RWOH.radius = radius;
|
||||||
WRITE_REPLAY_BLOCK_IFCHANGD(RWOH, _RWOH, old);
|
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)
|
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)
|
||||||
@ -331,7 +327,7 @@ void AP_DAL::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vec
|
|||||||
_RBOH.delAng = delAng;
|
_RBOH.delAng = delAng;
|
||||||
_RBOH.delTime = delTime;
|
_RBOH.delTime = delTime;
|
||||||
_RBOH.timeStamp_ms = timeStamp_ms;
|
_RBOH.timeStamp_ms = timeStamp_ms;
|
||||||
WRITE_REPLAY_BLOCK_IFCHANGD(RBOH, _RBOH, old);
|
WRITE_REPLAY_BLOCK_IFCHANGED(RBOH, _RBOH, old);
|
||||||
}
|
}
|
||||||
|
|
||||||
#if APM_BUILD_TYPE(APM_BUILD_Replay)
|
#if APM_BUILD_TYPE(APM_BUILD_Replay)
|
||||||
|
@ -1,9 +1,5 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <stdint.h>
|
|
||||||
|
|
||||||
#include <cstddef>
|
|
||||||
|
|
||||||
#include "AP_DAL_InertialSensor.h"
|
#include "AP_DAL_InertialSensor.h"
|
||||||
#include "AP_DAL_Baro.h"
|
#include "AP_DAL_Baro.h"
|
||||||
#include "AP_DAL_GPS.h"
|
#include "AP_DAL_GPS.h"
|
||||||
@ -15,6 +11,10 @@
|
|||||||
|
|
||||||
#include "LogStructure.h"
|
#include "LogStructure.h"
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <cstddef>
|
||||||
|
|
||||||
|
|
||||||
#define DAL_CORE(c) AP::dal().logging_core(c)
|
#define DAL_CORE(c) AP::dal().logging_core(c)
|
||||||
|
|
||||||
#if APM_BUILD_TYPE(APM_BUILD_Replay)
|
#if APM_BUILD_TYPE(APM_BUILD_Replay)
|
||||||
@ -25,16 +25,16 @@ class NavEKF3;
|
|||||||
class AP_DAL {
|
class AP_DAL {
|
||||||
public:
|
public:
|
||||||
|
|
||||||
enum class FrameType {
|
enum class FrameType : uint8_t {
|
||||||
InitialiseFilterEKF2 = 1<<0,
|
InitialiseFilterEKF2 = 1U<<0,
|
||||||
UpdateFilterEKF2 = 1<<1,
|
UpdateFilterEKF2 = 1U<<1,
|
||||||
InitialiseFilterEKF3 = 1<<2,
|
InitialiseFilterEKF3 = 1<<2,
|
||||||
UpdateFilterEKF3 = 1<<3,
|
UpdateFilterEKF3 = 1<<3,
|
||||||
LogWriteEKF2 = 1<<4,
|
LogWriteEKF2 = 1<<4,
|
||||||
LogWriteEKF3 = 1<<5,
|
LogWriteEKF3 = 1<<5,
|
||||||
};
|
};
|
||||||
|
|
||||||
enum class Event2 {
|
enum class Event2 : uint8_t {
|
||||||
resetGyroBias = 0,
|
resetGyroBias = 0,
|
||||||
resetHeightDatum = 1,
|
resetHeightDatum = 1,
|
||||||
setInhibitGPS = 2,
|
setInhibitGPS = 2,
|
||||||
@ -50,7 +50,7 @@ public:
|
|||||||
checkLaneSwitch = 12,
|
checkLaneSwitch = 12,
|
||||||
};
|
};
|
||||||
|
|
||||||
enum class Event3 {
|
enum class Event3 : uint8_t {
|
||||||
resetGyroBias = 0,
|
resetGyroBias = 0,
|
||||||
resetHeightDatum = 1,
|
resetHeightDatum = 1,
|
||||||
setInhibitGPS = 2,
|
setInhibitGPS = 2,
|
||||||
@ -350,7 +350,7 @@ private:
|
|||||||
};
|
};
|
||||||
|
|
||||||
#define WRITE_REPLAY_BLOCK(sname,v) AP_DAL::WriteLogMessage(LOG_## sname ##_MSG, &v, nullptr, offsetof(log_ ##sname, _end))
|
#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) do { static_assert(sizeof(v) == sizeof(old), "types must match"); \
|
#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)); } \
|
AP_DAL::WriteLogMessage(LOG_## sname ##_MSG, &v, &old, offsetof(log_ ##sname, _end)); } \
|
||||||
while (0)
|
while (0)
|
||||||
|
|
||||||
|
@ -20,7 +20,7 @@ void AP_DAL_Airspeed::start_frame()
|
|||||||
const log_RASH old = _RASH;
|
const log_RASH old = _RASH;
|
||||||
_RASH.num_sensors = airspeed->get_num_sensors();
|
_RASH.num_sensors = airspeed->get_num_sensors();
|
||||||
_RASH.primary = airspeed->get_primary();
|
_RASH.primary = airspeed->get_primary();
|
||||||
WRITE_REPLAY_BLOCK_IFCHANGD(RASH, _RASH, old);
|
WRITE_REPLAY_BLOCK_IFCHANGED(RASH, _RASH, old);
|
||||||
|
|
||||||
for (uint8_t i=0; i<ARRAY_SIZE(_RASI); i++) {
|
for (uint8_t i=0; i<ARRAY_SIZE(_RASI); i++) {
|
||||||
log_RASI &RASI = _RASI[i];
|
log_RASI &RASI = _RASI[i];
|
||||||
@ -29,6 +29,6 @@ void AP_DAL_Airspeed::start_frame()
|
|||||||
RASI.healthy = airspeed->healthy(i);
|
RASI.healthy = airspeed->healthy(i);
|
||||||
RASI.use = airspeed->use(i);
|
RASI.use = airspeed->use(i);
|
||||||
RASI.airspeed = airspeed->get_airspeed(i);
|
RASI.airspeed = airspeed->get_airspeed(i);
|
||||||
WRITE_REPLAY_BLOCK_IFCHANGD(RASI, RASI, old_RASI);
|
WRITE_REPLAY_BLOCK_IFCHANGED(RASI, RASI, old_RASI);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -17,16 +17,16 @@ void AP_DAL_Baro::start_frame()
|
|||||||
const log_RBRH old_RBRH = _RBRH;
|
const log_RBRH old_RBRH = _RBRH;
|
||||||
_RBRH.primary = baro.get_primary();
|
_RBRH.primary = baro.get_primary();
|
||||||
_RBRH.num_instances = baro.num_instances();
|
_RBRH.num_instances = baro.num_instances();
|
||||||
WRITE_REPLAY_BLOCK_IFCHANGD(RBRH, _RBRH, old_RBRH);
|
WRITE_REPLAY_BLOCK_IFCHANGED(RBRH, _RBRH, old_RBRH);
|
||||||
|
|
||||||
for (uint8_t i=0; i<BARO_MAX_INSTANCES; i++) {
|
for (uint8_t i=0; i<_RBRH.num_instances; i++) {
|
||||||
log_RBRI &RBRI = _RBRI[i];
|
log_RBRI &RBRI = _RBRI[i];
|
||||||
log_RBRI old = RBRI;
|
log_RBRI old = RBRI;
|
||||||
const uint32_t last_update_ms = baro.get_last_update(i);
|
const uint32_t last_update_ms = baro.get_last_update(i);
|
||||||
RBRI.last_update_ms = last_update_ms;
|
RBRI.last_update_ms = last_update_ms;
|
||||||
RBRI.healthy = baro.healthy(i);
|
RBRI.healthy = baro.healthy(i);
|
||||||
RBRI.altitude = baro.get_altitude(i);
|
RBRI.altitude = baro.get_altitude(i);
|
||||||
WRITE_REPLAY_BLOCK_IFCHANGD(RBRI, _RBRI[i], old);
|
WRITE_REPLAY_BLOCK_IFCHANGED(RBRI, _RBRI[i], old);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -28,7 +28,7 @@ void AP_DAL_Beacon::start_frame()
|
|||||||
_RBCH.origin_lng = loc.lng;
|
_RBCH.origin_lng = loc.lng;
|
||||||
_RBCH.origin_alt = loc.alt;
|
_RBCH.origin_alt = loc.alt;
|
||||||
}
|
}
|
||||||
WRITE_REPLAY_BLOCK_IFCHANGD(RBCH, _RBCH, old);
|
WRITE_REPLAY_BLOCK_IFCHANGED(RBCH, _RBCH, old);
|
||||||
if (bcon == nullptr) {
|
if (bcon == nullptr) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
@ -41,6 +41,6 @@ void AP_DAL_Beacon::start_frame()
|
|||||||
RBCI.distance = bcon->beacon_distance(i);
|
RBCI.distance = bcon->beacon_distance(i);
|
||||||
RBCI.healthy = bcon->beacon_healthy(i);
|
RBCI.healthy = bcon->beacon_healthy(i);
|
||||||
|
|
||||||
WRITE_REPLAY_BLOCK_IFCHANGD(RBCI, RBCI, old_RBCI);
|
WRITE_REPLAY_BLOCK_IFCHANGED(RBCI, RBCI, old_RBCI);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -23,9 +23,9 @@ void AP_DAL_Compass::start_frame()
|
|||||||
_RMGH.num_enabled = compass.get_num_enabled();
|
_RMGH.num_enabled = compass.get_num_enabled();
|
||||||
_RMGH.consistent = compass.consistent();
|
_RMGH.consistent = compass.consistent();
|
||||||
|
|
||||||
WRITE_REPLAY_BLOCK_IFCHANGD(RMGH, _RMGH, old);
|
WRITE_REPLAY_BLOCK_IFCHANGED(RMGH, _RMGH, old);
|
||||||
|
|
||||||
for (uint8_t i=0; i<ARRAY_SIZE(_RMGI); i++) {
|
for (uint8_t i=0; i<_RMGH.count; i++) {
|
||||||
log_RMGI &RMGI = _RMGI[i];
|
log_RMGI &RMGI = _RMGI[i];
|
||||||
const log_RMGI old_RMGI = RMGI;
|
const log_RMGI old_RMGI = RMGI;
|
||||||
RMGI.use_for_yaw = compass.use_for_yaw(i);
|
RMGI.use_for_yaw = compass.use_for_yaw(i);
|
||||||
@ -35,6 +35,6 @@ void AP_DAL_Compass::start_frame()
|
|||||||
RMGI.last_update_usec = compass.last_update_usec(i);
|
RMGI.last_update_usec = compass.last_update_usec(i);
|
||||||
RMGI.field = compass.get_field(i);
|
RMGI.field = compass.get_field(i);
|
||||||
|
|
||||||
WRITE_REPLAY_BLOCK_IFCHANGD(RMGI, RMGI, old_RMGI);
|
WRITE_REPLAY_BLOCK_IFCHANGED(RMGI, RMGI, old_RMGI);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -19,7 +19,7 @@ void AP_DAL_GPS::start_frame()
|
|||||||
_RGPH.primary_sensor = gps.primary_sensor();
|
_RGPH.primary_sensor = gps.primary_sensor();
|
||||||
_RGPH.num_sensors = gps.num_sensors();
|
_RGPH.num_sensors = gps.num_sensors();
|
||||||
|
|
||||||
WRITE_REPLAY_BLOCK_IFCHANGD(RGPH, _RGPH, old_RGPH);
|
WRITE_REPLAY_BLOCK_IFCHANGED(RGPH, _RGPH, old_RGPH);
|
||||||
|
|
||||||
for (uint8_t i=0; i<ARRAY_SIZE(_RGPI); i++) {
|
for (uint8_t i=0; i<ARRAY_SIZE(_RGPI); i++) {
|
||||||
log_RGPI &RGPI = _RGPI[i];
|
log_RGPI &RGPI = _RGPI[i];
|
||||||
@ -39,7 +39,7 @@ void AP_DAL_GPS::start_frame()
|
|||||||
RGPI.hdop = gps.get_hdop(i);
|
RGPI.hdop = gps.get_hdop(i);
|
||||||
RGPI.num_sats = gps.num_sats(i);
|
RGPI.num_sats = gps.num_sats(i);
|
||||||
RGPI.get_lag_returncode = gps.get_lag(i, RGPI.lag_sec);
|
RGPI.get_lag_returncode = gps.get_lag(i, RGPI.lag_sec);
|
||||||
WRITE_REPLAY_BLOCK_IFCHANGD(RGPI, RGPI, old_RGPI);
|
WRITE_REPLAY_BLOCK_IFCHANGED(RGPI, RGPI, old_RGPI);
|
||||||
|
|
||||||
log_RGPJ &RGPJ = _RGPJ[i];
|
log_RGPJ &RGPJ = _RGPJ[i];
|
||||||
const log_RGPJ old_RGPJ = RGPJ;
|
const log_RGPJ old_RGPJ = RGPJ;
|
||||||
@ -47,7 +47,7 @@ void AP_DAL_GPS::start_frame()
|
|||||||
RGPJ.velocity = gps.velocity(i);
|
RGPJ.velocity = gps.velocity(i);
|
||||||
RGPJ.speed_accuracy_returncode = gps.speed_accuracy(i, RGPJ.sacc);
|
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);
|
RGPJ.gps_yaw_deg_returncode = gps.gps_yaw_deg(i, RGPJ.yaw_deg, RGPJ.yaw_accuracy_deg);
|
||||||
WRITE_REPLAY_BLOCK_IFCHANGD(RGPJ, RGPJ, old_RGPJ);
|
WRITE_REPLAY_BLOCK_IFCHANGED(RGPJ, RGPJ, old_RGPJ);
|
||||||
|
|
||||||
// also fetch antenna offset for this frame
|
// also fetch antenna offset for this frame
|
||||||
antenna_offset[i] = gps.get_antenna_offset(i);
|
antenna_offset[i] = gps.get_antenna_offset(i);
|
||||||
|
@ -23,7 +23,7 @@ void AP_DAL_InertialSensor::start_frame()
|
|||||||
_RISH.primary_accel = ins.get_primary_accel();
|
_RISH.primary_accel = ins.get_primary_accel();
|
||||||
_RISH.accel_count = ins.get_accel_count();
|
_RISH.accel_count = ins.get_accel_count();
|
||||||
_RISH.gyro_count = ins.get_gyro_count();
|
_RISH.gyro_count = ins.get_gyro_count();
|
||||||
WRITE_REPLAY_BLOCK_IFCHANGD(RISH, _RISH, old_RISH);
|
WRITE_REPLAY_BLOCK_IFCHANGED(RISH, _RISH, old_RISH);
|
||||||
|
|
||||||
for (uint8_t i=0; i<ARRAY_SIZE(_RISI); i++) {
|
for (uint8_t i=0; i<ARRAY_SIZE(_RISI); i++) {
|
||||||
log_RISI &RISI = _RISI[i];
|
log_RISI &RISI = _RISI[i];
|
||||||
@ -41,7 +41,7 @@ void AP_DAL_InertialSensor::start_frame()
|
|||||||
|
|
||||||
update_filtered(i);
|
update_filtered(i);
|
||||||
|
|
||||||
WRITE_REPLAY_BLOCK_IFCHANGD(RISI, RISI, old_RISI);
|
WRITE_REPLAY_BLOCK_IFCHANGED(RISI, RISI, old_RISI);
|
||||||
|
|
||||||
// update sensor position
|
// update sensor position
|
||||||
pos[i] = ins.get_imu_pos_offset(i);
|
pos[i] = ins.get_imu_pos_offset(i);
|
||||||
|
@ -57,7 +57,7 @@ void AP_DAL_RangeFinder::start_frame()
|
|||||||
// EKF only asks for this *down*.
|
// EKF only asks for this *down*.
|
||||||
const log_RRNH old = _RRNH;
|
const log_RRNH old = _RRNH;
|
||||||
_RRNH.ground_clearance_cm = rangefinder->ground_clearance_cm_orient(ROTATION_PITCH_270);
|
_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.max_distance_cm = rangefinder->max_distance_cm_orient(ROTATION_PITCH_270);
|
||||||
_RRNH.backend_mask = 0;
|
_RRNH.backend_mask = 0;
|
||||||
|
|
||||||
for (uint8_t i=0; i<RANGEFINDER_MAX_INSTANCES; i++) {
|
for (uint8_t i=0; i<RANGEFINDER_MAX_INSTANCES; i++) {
|
||||||
@ -69,7 +69,7 @@ void AP_DAL_RangeFinder::start_frame()
|
|||||||
_backend[i]->start_frame(backend);
|
_backend[i]->start_frame(backend);
|
||||||
}
|
}
|
||||||
|
|
||||||
WRITE_REPLAY_BLOCK_IFCHANGD(RRNH, _RRNH, old);
|
WRITE_REPLAY_BLOCK_IFCHANGED(RRNH, _RRNH, old);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@ -85,7 +85,7 @@ void AP_DAL_RangeFinder_Backend::start_frame(AP_RangeFinder_Backend *backend) {
|
|||||||
_RRNI.orientation = backend->orientation();
|
_RRNI.orientation = backend->orientation();
|
||||||
_RRNI.status = (uint8_t)backend->status();
|
_RRNI.status = (uint8_t)backend->status();
|
||||||
_RRNI.pos_offset = backend->get_pos_offset();
|
_RRNI.pos_offset = backend->get_pos_offset();
|
||||||
WRITE_REPLAY_BLOCK_IFCHANGD(RRNI, _RRNI, old);
|
WRITE_REPLAY_BLOCK_IFCHANGED(RRNI, _RRNI, old);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@ -7,10 +7,6 @@
|
|||||||
#include <AP_Logger/AP_Logger.h>
|
#include <AP_Logger/AP_Logger.h>
|
||||||
#include "AP_DAL.h"
|
#include "AP_DAL.h"
|
||||||
|
|
||||||
AP_DAL_VisualOdom::AP_DAL_VisualOdom()
|
|
||||||
{
|
|
||||||
}
|
|
||||||
|
|
||||||
void AP_DAL_VisualOdom::start_frame()
|
void AP_DAL_VisualOdom::start_frame()
|
||||||
{
|
{
|
||||||
const auto *vo = AP::visualodom();
|
const auto *vo = AP::visualodom();
|
||||||
@ -21,7 +17,7 @@ void AP_DAL_VisualOdom::start_frame()
|
|||||||
RVOH.healthy = vo->healthy();
|
RVOH.healthy = vo->healthy();
|
||||||
}
|
}
|
||||||
|
|
||||||
WRITE_REPLAY_BLOCK_IFCHANGD(RVOH, RVOH, old);
|
WRITE_REPLAY_BLOCK_IFCHANGED(RVOH, RVOH, old);
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // HAL_VISUALODOM_ENABLED
|
#endif // HAL_VISUALODOM_ENABLED
|
||||||
|
@ -29,9 +29,6 @@ public:
|
|||||||
return RVOH.pos_offset;
|
return RVOH.pos_offset;
|
||||||
}
|
}
|
||||||
|
|
||||||
// AP_DAL methods:
|
|
||||||
AP_DAL_VisualOdom();
|
|
||||||
|
|
||||||
AP_DAL_VisualOdom *visualodom() {
|
AP_DAL_VisualOdom *visualodom() {
|
||||||
if (RVOH.ptr_is_nullptr) {
|
if (RVOH.ptr_is_nullptr) {
|
||||||
return nullptr;
|
return nullptr;
|
||||||
|
Loading…
Reference in New Issue
Block a user