Line data    Source code

       1             : #include "EKFGSF_yaw.h"
       2             : 
       3             : #include <AP_Logger/AP_Logger.h>
       4             : 
       5             : #if HAL_LOGGING_ENABLED
       6             : 
       7             : #pragma GCC diagnostic ignored "-Wnarrowing"
       8             : 
       9     6313594 : void EKFGSF_yaw::Log_Write(uint64_t time_us, LogMessages id0, LogMessages id1, uint8_t core_index)
      10             : {
      11     6313594 :     if (!vel_fuse_running) {
      12     2193463 :         return;
      13             :     }
      14             : 
      15             :     static_assert(N_MODELS_EKFGSF >= 5, "Logging will break on <5 EKFGSF models");
      16             : 
      17     4120131 :     const struct log_KY0 ky0{
      18             :         LOG_PACKET_HEADER_INIT(id0),
      19             :         time_us                 : time_us,
      20             :         core                    : core_index,
      21     4120131 :         yaw_composite           : wrap_360(degrees(GSF.yaw)),
      22     8240262 :         yaw_composite_variance  : sqrtF(MAX(degrees(GSF.yaw_variance), 0.0f)),
      23     4120131 :         yaw0                    : wrap_360(degrees(EKF[0].X[2])),
      24     4120131 :         yaw1                    : wrap_360(degrees(EKF[1].X[2])),
      25     4120131 :         yaw2                    : wrap_360(degrees(EKF[2].X[2])),
      26     4120131 :         yaw3                    : wrap_360(degrees(EKF[3].X[2])),
      27     4120131 :         yaw4                    : wrap_360(degrees(EKF[4].X[2])),
      28     4120131 :         wgt0                    : GSF.weights[0],
      29     4120131 :         wgt1                    : GSF.weights[1],
      30     4120131 :         wgt2                    : GSF.weights[2],
      31     4120131 :         wgt3                    : GSF.weights[3],
      32     4120131 :         wgt4                    : GSF.weights[4],
      33     4120131 :     };
      34     4120131 :     AP::logger().WriteBlock(&ky0, sizeof(ky0));
      35             : 
      36     4120131 :     const struct log_KY1 ky1{
      37             :         LOG_PACKET_HEADER_INIT(id1),
      38             :         time_us                 : time_us,
      39             :         core                    : core_index,
      40     4120131 :         ivn0                    : EKF[0].innov[0],
      41     4120131 :         ivn1                    : EKF[1].innov[0],
      42     4120131 :         ivn2                    : EKF[2].innov[0],
      43     4120131 :         ivn3                    : EKF[3].innov[0],
      44     4120131 :         ivn4                    : EKF[4].innov[0],
      45     4120131 :         ive0                    : EKF[0].innov[1],
      46     4120131 :         ive1                    : EKF[1].innov[1],
      47     4120131 :         ive2                    : EKF[2].innov[1],
      48     4120131 :         ive3                    : EKF[3].innov[1],
      49     4120131 :         ive4                    : EKF[4].innov[1],
      50     4120131 :     };
      51     4120131 :     AP::logger().WriteBlock(&ky1, sizeof(ky1));
      52             : }
      53             : 
      54             : #endif  // HAL_LOGGING_ENABLED