AP_AHRS: log DCM errrp and erryaw into DCM message

ArduPilot · Sep 9, 2024 · 61de480 · 61de480
1 parent 9726e8e
commit 61de480
Show file tree

Hide file tree

Showing 3 changed files with 23 additions and 17 deletions.
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -107,14 +107,28 @@ AP_AHRS_DCM::update()
     if (now_ms - last_log_ms >= 100) {
         // log DCM at 10Hz
         last_log_ms = now_ms;
-        AP::logger().WriteStreaming("DCM", "TimeUS,Roll,Pitch,Yaw",
-                                    "sddd",
-                                    "F000",
-                                    "Qfff",
-                                    AP_HAL::micros64(),
-                                    degrees(roll),
-                                    degrees(pitch),
-                                    wrap_360(degrees(yaw)));
+
+// @LoggerMessage: DCM
+// @Description: DCM Estimator Data
+// @Field: TimeUS: Time since system startup
+// @Field: Roll: estimated roll
+// @Field: Pitch: estimated pitch
+// @Field: Yaw: estimated yaw
+// @Field: ErrRP: lowest estimated gyro drift error
+// @Field: ErrYaw: difference between measured yaw and DCM yaw estimate
+        AP::logger().WriteStreaming(
+            "DCM",
+            "TimeUS," "Roll," "Pitch," "Yaw," "ErrRP," "ErrYaw",
+            "s"       "d"     "d"      "d"    "d"      "h",
+            "F"       "0"     "0"      "0"    "0"      "0",
+            "Q"       "f"     "f"      "f"    "f"      "f",
+            AP_HAL::micros64(),
+            degrees(roll),
+            degrees(pitch),
+            wrap_360(degrees(yaw)),
+            get_error_rp(),
+            get_error_yaw()
+       );
     }
 #endif // HAL_LOGGING_ENABLED
 }

diff --git a/libraries/AP_AHRS/AP_AHRS_Logging.cpp b/libraries/AP_AHRS/AP_AHRS_Logging.cpp
@@ -60,8 +60,6 @@ void AP_AHRS::Write_Attitude(const Vector3f &targets) const
         pitch           : (int16_t)pitch_sensor,
         control_yaw     : (uint16_t)wrap_360_cd(targets.z),
         yaw             : (uint16_t)wrap_360_cd(yaw_sensor),
-        error_rp        : (uint16_t)(get_error_rp() * 100),
-        error_yaw       : (uint16_t)(get_error_yaw() * 100),
         active          : uint8_t(active_EKF_type()),
     };
     AP::logger().WriteBlock(&pkt, sizeof(pkt));
@@ -136,8 +134,6 @@ void AP_AHRS_View::Write_AttitudeView(const Vector3f &targets) const
         pitch           : (int16_t)pitch_sensor,
         control_yaw     : (uint16_t)wrap_360_cd(targets.z),
         yaw             : (uint16_t)wrap_360_cd(yaw_sensor),
-        error_rp        : (uint16_t)(get_error_rp() * 100),
-        error_yaw       : (uint16_t)(get_error_yaw() * 100),
         active          : uint8_t(AP::ahrs().active_EKF_type()),
     };
     AP::logger().WriteBlock(&pkt, sizeof(pkt));

diff --git a/libraries/AP_AHRS/LogStructure.h b/libraries/AP_AHRS/LogStructure.h
@@ -57,8 +57,6 @@ struct PACKED log_AOA_SSA {
 // @Field: Pitch: achieved vehicle pitch
 // @Field: DesYaw: vehicle desired yaw
 // @Field: Yaw: achieved vehicle yaw
-// @Field: ErrRP: lowest estimated gyro drift error
-// @Field: ErrYaw: difference between measured yaw and DCM yaw estimate
 // @Field: AEKF: active EKF type
 struct PACKED log_Attitude {
     LOG_PACKET_HEADER;
@@ -69,8 +67,6 @@ struct PACKED log_Attitude {
     int16_t  pitch;
     uint16_t control_yaw;
     uint16_t yaw;
-    uint16_t error_rp;
-    uint16_t error_yaw;
     uint8_t  active;
 };
 
@@ -199,7 +195,7 @@ struct PACKED log_ATSC {
     { LOG_AOA_SSA_MSG, sizeof(log_AOA_SSA), \
         "AOA", "Qff", "TimeUS,AOA,SSA", "sdd", "F00" , true }, \
     { LOG_ATTITUDE_MSG, sizeof(log_Attitude),\
-        "ATT", "QccccCCCCB", "TimeUS,DesRoll,Roll,DesPitch,Pitch,DesYaw,Yaw,ErrRP,ErrYaw,AEKF", "sddddhhdh-", "FBBBBBBBB-" , true }, \
+        "ATT", "QccccCCB", "TimeUS,DesRoll,Roll,DesPitch,Pitch,DesYaw,Yaw,AEKF", "sddddhh-", "FBBBBBB-" , true }, \
     { LOG_ORGN_MSG, sizeof(log_ORGN), \
         "ORGN","QBLLe","TimeUS,Type,Lat,Lng,Alt", "s#DUm", "F-GGB" }, \
     { LOG_POS_MSG, sizeof(log_POS), \