Line data    Source code

       1             : #include "AP_DAL_Compass.h"
       2             : 
       3             : #include <AP_Compass/AP_Compass.h>
       4             : 
       5             : #include <AP_Logger/AP_Logger.h>
       6             : #include "AP_DAL.h"
       7             : 
       8        8772 : AP_DAL_Compass::AP_DAL_Compass()
       9             : {
      10        8772 :     for (uint8_t i=0; i<ARRAY_SIZE(_RMGI); i++) {
      11        6579 :         _RMGI[i].instance = i;
      12             :     }
      13        2193 : }
      14             : 
      15    41156285 : void AP_DAL_Compass::start_frame()
      16             : {
      17    41156285 :     const auto &compass = AP::compass();
      18             : 
      19    41156285 :     const log_RMGH old = _RMGH;
      20    41156285 :     _RMGH.available = compass.available();
      21    41156285 :     _RMGH.count = compass.get_count();
      22    41156285 :     _RMGH.auto_declination_enabled = compass.auto_declination_enabled();
      23    41156285 :     _RMGH.declination = compass.get_declination();
      24    41156285 :     _RMGH.num_enabled = compass.get_num_enabled();
      25    41156285 :     _RMGH.consistent = compass.consistent();
      26    41156285 :     _RMGH.first_usable = compass.get_first_usable();
      27    41156285 :     _RMGH.learn_offsets_enabled = compass.learn_offsets_enabled();
      28             : 
      29    41156285 :     WRITE_REPLAY_BLOCK_IFCHANGED(RMGH, _RMGH, old);
      30             : 
      31   164593904 :     for (uint8_t i=0; i<_RMGH.count; i++) {
      32   123437619 :         log_RMGI &RMGI = _RMGI[i];
      33   123437619 :         const log_RMGI old_RMGI = RMGI;
      34   123437619 :         RMGI.use_for_yaw = compass.use_for_yaw(i);
      35   123437619 :         RMGI.healthy = compass.healthy(i);
      36   123437619 :         RMGI.offsets = compass.get_offsets(i);
      37   123437619 :         RMGI.have_scale_factor = compass.have_scale_factor(i);
      38   123437619 :         RMGI.last_update_usec = compass.last_update_usec(i);
      39   123437619 :         RMGI.field = compass.get_field(i);
      40             : 
      41   123437619 :         WRITE_REPLAY_BLOCK_IFCHANGED(RMGI, RMGI, old_RMGI);
      42             :     }
      43    41156285 : }