You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
42 lines
1.2 KiB
42 lines
1.2 KiB
#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.available = compass.available(); |
|
_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); |
|
} |
|
}
|
|
|