Line data    Source code

       1             : #include <AP_Compass/AP_Compass_config.h>
       2             : #include <AP_Logger/AP_Logger_config.h>
       3             : 
       4             : #if AP_COMPASS_ENABLED && HAL_LOGGING_ENABLED
       5             : 
       6             : #include "AP_Compass.h"
       7             : 
       8             : #include <AP_Logger/AP_Logger.h>
       9             : 
      10     4247415 : void Compass::Write_Compass_instance(const uint64_t time_us, const uint8_t mag_instance)
      11             : {
      12     4247415 :     const Vector3f &mag_field = get_field(mag_instance);
      13     4247415 :     const Vector3f &mag_offsets = get_offsets(mag_instance);
      14     4247415 :     const Vector3f &mag_motor_offsets = get_motor_offsets(mag_instance);
      15     4247415 :     const struct log_MAG pkt{
      16             :         LOG_PACKET_HEADER_INIT(LOG_MAG_MSG),
      17             :         time_us         : time_us,
      18             :         instance        : mag_instance,
      19     4247415 :         mag_x           : (int16_t)mag_field.x,
      20     4247415 :         mag_y           : (int16_t)mag_field.y,
      21     4247415 :         mag_z           : (int16_t)mag_field.z,
      22     4247415 :         offset_x        : (int16_t)mag_offsets.x,
      23     4247415 :         offset_y        : (int16_t)mag_offsets.y,
      24     4247415 :         offset_z        : (int16_t)mag_offsets.z,
      25     4247415 :         motor_offset_x  : (int16_t)mag_motor_offsets.x,
      26     4247415 :         motor_offset_y  : (int16_t)mag_motor_offsets.y,
      27     4247415 :         motor_offset_z  : (int16_t)mag_motor_offsets.z,
      28     8494830 :         health          : (uint8_t)healthy(mag_instance),
      29     4247415 :         SUS             : last_update_usec(mag_instance)
      30     4247415 :     };
      31     4247415 :     AP::logger().WriteBlock(&pkt, sizeof(pkt));
      32     4247415 : }
      33             : 
      34             : // Write a Compass packet
      35     1415805 : void Compass::Write_Compass()
      36             : {
      37     1415805 :     const uint64_t time_us = AP_HAL::micros64();
      38     5663220 :     for (uint8_t i=0; i<get_count(); i++) {
      39     4247415 :         Write_Compass_instance(time_us, i);
      40             :     }
      41     1415805 : }
      42             : 
      43             : #endif  // AP_COMPASS_ENABLED && HAL_LOGGING_ENABLED