ardupilot/libraries/AP_DAL/AP_DAL_Compass.cpp

42 lines
1.2 KiB
C++

#include "AP_DAL_Compass.h"
#include <AP_Compass/AP_Compass.h>
#include <AP_Logger/AP_Logger.h>
#include "AP_DAL.h"
AP_DAL_Compass::AP_DAL_Compass()
{
for (uint8_t i=0; i<ARRAY_SIZE(_RMGI); i++) {
_RMGI[i].instance = i;
}
}
void AP_DAL_Compass::start_frame()
{
const auto &compass = AP::compass();
const log_RMGH old = _RMGH;
_RMGH.count = compass.get_count();
_RMGH.auto_declination_enabled = compass.auto_declination_enabled();
_RMGH.declination = compass.get_declination();
_RMGH.num_enabled = compass.get_num_enabled();
_RMGH.consistent = compass.consistent();
_RMGH.first_usable = compass.get_first_usable();
WRITE_REPLAY_BLOCK_IFCHANGED(RMGH, _RMGH, old);
for (uint8_t i=0; i<_RMGH.count; i++) {
log_RMGI &RMGI = _RMGI[i];
const log_RMGI old_RMGI = RMGI;
RMGI.use_for_yaw = compass.use_for_yaw(i);
RMGI.healthy = compass.healthy(i);
RMGI.offsets = compass.get_offsets(i);
RMGI.have_scale_factor = compass.have_scale_factor(i);
RMGI.last_update_usec = compass.last_update_usec(i);
RMGI.field = compass.get_field(i);
WRITE_REPLAY_BLOCK_IFCHANGED(RMGI, RMGI, old_RMGI);
}
}