AP_DAL: cleanups from review

This commit is contained in:
Andrew Tridgell 2020-11-08 08:29:45 +11:00
parent 541d11ee05
commit 905ffed7fd
11 changed files with 35 additions and 46 deletions

View File

@ -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)

View File

@ -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)

View File

@ -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);
} }
} }

View File

@ -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);
} }
} }

View File

@ -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);
} }
} }

View File

@ -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);
} }
} }

View File

@ -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);

View File

@ -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);

View File

@ -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);
} }

View File

@ -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

View File

@ -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;