AP_AHRS: added DCM logging

ArduPilot · Sep 15, 2023 · 411b987 · 411b987
1 parent e56ea25
commit 411b987
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 0 deletions.
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -112,6 +112,20 @@ AP_AHRS_DCM::update()
 
     // remember the last origin for fallback support
     IGNORE_RETURN(AP::ahrs().get_origin(last_origin));
+
+    const uint32_t now_ms = AP_HAL::millis();
+    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)));
+    }
 }
 
 void AP_AHRS_DCM::get_results(AP_AHRS_Backend::Estimates &results)

diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h
@@ -282,4 +282,6 @@ class AP_AHRS_DCM : public AP_AHRS_Backend {
     // pre-calculated trig cache:
     float _sin_yaw;
     float _cos_yaw;
+
+    uint32_t last_log_ms;
 };