From 9d73cfb28f7a8a7177099a944e534f9c1c63fb56 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 16 Apr 2022 10:11:32 +0900 Subject: [PATCH] AP_NavEKF: GSF logging in deg from 0 to 360 --- libraries/AP_NavEKF/EKFGSF_Logging.cpp | 14 +++++++------- libraries/AP_NavEKF/LogStructure.h | 16 ++++++++-------- 2 files changed, 15 insertions(+), 15 deletions(-) diff --git a/libraries/AP_NavEKF/EKFGSF_Logging.cpp b/libraries/AP_NavEKF/EKFGSF_Logging.cpp index 31cc095d92..64673085ce 100644 --- a/libraries/AP_NavEKF/EKFGSF_Logging.cpp +++ b/libraries/AP_NavEKF/EKFGSF_Logging.cpp @@ -16,13 +16,13 @@ void EKFGSF_yaw::Log_Write(uint64_t time_us, LogMessages id0, LogMessages id1, u LOG_PACKET_HEADER_INIT(id0), time_us : time_us, core : core_index, - yaw_composite : GSF.yaw, - yaw_composite_variance : sqrtF(MAX(GSF.yaw_variance, 0.0f)), - yaw0 : EKF[0].X[2], - yaw1 : EKF[1].X[2], - yaw2 : EKF[2].X[2], - yaw3 : EKF[3].X[2], - yaw4 : EKF[4].X[2], + yaw_composite : wrap_360(degrees(GSF.yaw)), + yaw_composite_variance : sqrtF(MAX(degrees(GSF.yaw_variance), 0.0f)), + yaw0 : wrap_360(degrees(EKF[0].X[2])), + yaw1 : wrap_360(degrees(EKF[1].X[2])), + yaw2 : wrap_360(degrees(EKF[2].X[2])), + yaw3 : wrap_360(degrees(EKF[3].X[2])), + yaw4 : wrap_360(degrees(EKF[4].X[2])), wgt0 : GSF.weights[0], wgt1 : GSF.weights[1], wgt2 : GSF.weights[2], diff --git a/libraries/AP_NavEKF/LogStructure.h b/libraries/AP_NavEKF/LogStructure.h index 85beb5e979..119ea0f9ea 100644 --- a/libraries/AP_NavEKF/LogStructure.h +++ b/libraries/AP_NavEKF/LogStructure.h @@ -6,13 +6,13 @@ // @Description: EKF Yaw Estimator States // @Field: TimeUS: Time since system startup // @Field: C: EKF core this data is for -// @Field: YC: GSF yaw estimate (rad) -// @Field: YCS: GSF yaw estimate 1-Sigma uncertainty (rad) -// @Field: Y0: Yaw estimate from individual EKF filter 0 (rad) -// @Field: Y1: Yaw estimate from individual EKF filter 1 (rad) -// @Field: Y2: Yaw estimate from individual EKF filter 2 (rad) -// @Field: Y3: Yaw estimate from individual EKF filter 3 (rad) -// @Field: Y4: Yaw estimate from individual EKF filter 4 (rad) +// @Field: YC: GSF yaw estimate (deg) +// @Field: YCS: GSF yaw estimate 1-Sigma uncertainty (deg) +// @Field: Y0: Yaw estimate from individual EKF filter 0 (deg) +// @Field: Y1: Yaw estimate from individual EKF filter 1 (deg) +// @Field: Y2: Yaw estimate from individual EKF filter 2 (deg) +// @Field: Y3: Yaw estimate from individual EKF filter 3 (deg) +// @Field: Y4: Yaw estimate from individual EKF filter 4 (deg) // @Field: W0: Weighting applied to yaw estimate from individual EKF filter 0 // @Field: W1: Weighting applied to yaw estimate from individual EKF filter 1 // @Field: W2: Weighting applied to yaw estimate from individual EKF filter 2 @@ -69,7 +69,7 @@ struct PACKED log_KY1 { #define KY0_FMT "QBffffffffffff" #define KY0_LABELS "TimeUS,C,YC,YCS,Y0,Y1,Y2,Y3,Y4,W0,W1,W2,W3,W4" -#define KY0_UNITS "s#rrrrrrr-----" +#define KY0_UNITS "s#hdhhhhh-----" #define KY0_MULTS "F-000000000000" #define KY1_FMT "QBffffffffff"