AP_ExternalAHRS: added EAHRS_LOG_RATE

common logging for all EAHRS backends
ArduPilot · Dec 3, 2023 · 6886d61 · 6886d61
1 parent 961d462
commit 6886d61
Show file tree

Hide file tree

Showing 4 changed files with 52 additions and 55 deletions.
diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp
@@ -29,6 +29,7 @@
 
 #include <GCS_MAVLink/GCS.h>
 #include <AP_AHRS/AP_AHRS.h>
+#include <AP_Logger/AP_Logger.h>
 
 extern const AP_HAL::HAL &hal;
 
@@ -80,6 +81,13 @@ const AP_Param::GroupInfo AP_ExternalAHRS::var_info[] = {
     // @Bitmask: 0:GPS,1:IMU,2:Baro,3:Compass
     // @User: Advanced
     AP_GROUPINFO("_SENSORS", 4, AP_ExternalAHRS, sensors, 0xF),
+
+    // @Param: _LOG_RATE
+    // @DisplayName: AHRS logging rate
+    // @Description: Logging rate for EARHS devices
+    // @Units: Hz
+    // @User: Standard
+    AP_GROUPINFO("_LOG_RATE", 5, AP_ExternalAHRS, log_rate, 10),
 
     AP_GROUPEND
 };
@@ -276,6 +284,44 @@ void AP_ExternalAHRS::update(void)
             state.have_origin = true;
         }
     }
+    const uint32_t now_ms = AP_HAL::millis();
+    if (log_rate.get() > 0 && now_ms - last_log_ms >= uint32_t(1000U/log_rate.get())) {
+        last_log_ms = now_ms;
+
+        // @LoggerMessage: EAHR
+        // @Description: External AHRS data
+        // @Field: TimeUS: Time since system startup
+        // @Field: Roll: euler roll
+        // @Field: Pitch: euler pitch
+        // @Field: Yaw: euler yaw
+        // @Field: VN: velocity north
+        // @Field: VE: velocity east
+        // @Field: VD: velocity down
+        // @Field: Lat: latitude
+        // @Field: Lon: longitude
+        // @Field: Alt: altitude AMSL
+        // @Field: Flg: nav status flags
+        // @Field: VVar: velocity variance
+        // @Field: PVar: position variance
+        // @Field: HVar: height variance
+
+        float roll, pitch, yaw;
+        state.quat.to_euler(roll, pitch, yaw);
+        nav_filter_status filterStatus {};
+        get_filter_status(filterStatus);
+
+        AP::logger().WriteStreaming("EAHR", "TimeUS,Roll,Pitch,Yaw,VN,VE,VD,Lat,Lon,Alt,Flg,VVar,PVar,HVar",
+                                    "sdddnnnDUm-nmm",
+                                    "F000000GG0-000",
+                                    "QffffffLLfIfff",
+                                    AP_HAL::micros64(),
+                                    degrees(roll), degrees(pitch), degrees(yaw),
+                                    state.velocity.x, state.velocity.y, state.velocity.z,
+                                    state.location.lat, state.location.lng, state.location.alt*0.01,
+                                    filterStatus.value,
+                                    state.velocity_variance, state.pos_horiz_variance,
+                                    state.pos_vert_variance);
+    }
 }
 
 // Get model/type name

diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h
@@ -187,6 +187,7 @@ class AP_ExternalAHRS {
 
     AP_Enum<DevType> devtype;
     AP_Int16         rate;
+    AP_Int16         log_rate;
     AP_Int16         options;
     AP_Int16         sensors;
 
@@ -201,6 +202,8 @@ class AP_ExternalAHRS {
     void set_default_sensors(uint16_t _sensors) {
         sensors.set_default(_sensors);
     }
+
+    uint32_t last_log_ms;
 };
 
 namespace AP {

diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp
@@ -449,44 +449,22 @@ bool AP_ExternalAHRS_InertialLabs::check_uart()
         // @LoggerMessage: ILB1
         // @Description: InertialLabs AHRS data1
         // @Field: TimeUS: Time since system startup
-        // @Field: Roll: euler roll
-        // @Field: Pitch: euler pitch
-        // @Field: Yaw: euler yaw
-        // @Field: VN: velocity north
-        // @Field: VE: velocity east
-        // @Field: VD: velocity down
-        // @Field: Lat: latitude
-        // @Field: Lon: longitude
-        // @Field: Alt: altitude AMSL
-
-        AP::logger().WriteStreaming("ILB1", "TimeUS,Roll,Pitch,Yaw,VN,VE,VD,Lat,Lon,Alt",
-                                    "sdddnnnDUm",
-                                    "F000000GG0",
-                                    "QffffffLLf",
-                                    now_us,
-                                    degrees(roll), degrees(pitch), degrees(yaw),
-                                    state.velocity.x, state.velocity.y, state.velocity.z,
-                                    state.location.lat, state.location.lng, state.location.alt*0.01);
-
-        // @LoggerMessage: ILB2
-        // @Description: InertialLabs AHRS data2
-        // @Field: TimeUS: Time since system startup
         // @Field: PosVarN: position variance north
         // @Field: PosVarE: position variance east
         // @Field: PosVarD: position variance down
         // @Field: VelVarN: velocity variance north
         // @Field: VelVarE: velocity variance east
         // @Field: VelVarD: velocity variance down
 
-        AP::logger().WriteStreaming("ILB2", "TimeUS,PosVarN,PosVarE,PosVarD,VelVarN,VelVarE,VelVarD",
+        AP::logger().WriteStreaming("ILB1", "TimeUS,PosVarN,PosVarE,PosVarD,VelVarN,VelVarE,VelVarD",
                                     "smmmnnn",
                                     "F000000",
                                     "Qffffff",
                                     now_us,
                                     state2.kf_pos_covariance.x, state2.kf_pos_covariance.x, state2.kf_pos_covariance.z,
                                     state2.kf_vel_covariance.x, state2.kf_vel_covariance.x, state2.kf_vel_covariance.z);
 
-        // @LoggerMessage: ILB3
+        // @LoggerMessage: ILB2
         // @Description: InertialLabs AHRS data3
         // @Field: TimeUS: Time since system startup
         // @Field: Stat1: unit status1
@@ -501,7 +479,7 @@ bool AP_ExternalAHRS_InertialLabs::check_uart()
         // @Field: WVE: Wind velocity east
         // @Field: WVD: Wind velocity down
 
-        AP::logger().WriteStreaming("ILB3", "TimeUS,Stat1,Stat2,FType,SpStat,GI1,GI2,GJS,TAS,WVN,WVE,WVD",
+        AP::logger().WriteStreaming("ILB2", "TimeUS,Stat1,Stat2,FType,SpStat,GI1,GI2,GJS,TAS,WVN,WVE,WVD",
                                     "s-----------",
                                     "F-----------",
                                     "QHHBBBBBffff",

diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp
@@ -509,36 +509,6 @@ void AP_ExternalAHRS_VectorNav::process_packet1(const uint8_t *b)
 
         AP::ins().handle_external(ins);
     }
-
-
-    // @LoggerMessage: EAH1
-    // @Description: External AHRS data
-    // @Field: TimeUS: Time since system startup
-    // @Field: Roll: euler roll
-    // @Field: Pitch: euler pitch
-    // @Field: Yaw: euler yaw
-    // @Field: VN: velocity north
-    // @Field: VE: velocity east
-    // @Field: VD: velocity down
-    // @Field: Lat: latitude
-    // @Field: Lon: longitude
-    // @Field: Alt: altitude AMSL
-    // @Field: UXY: uncertainty in XY position
-    // @Field: UV: uncertainty in velocity
-    // @Field: UR: uncertainty in roll
-    // @Field: UP: uncertainty in pitch
-    // @Field: UY: uncertainty in yaw
-
-    AP::logger().WriteStreaming("EAH1", "TimeUS,Roll,Pitch,Yaw,VN,VE,VD,Lat,Lon,Alt,UXY,UV,UR,UP,UY",
-                       "sdddnnnDUmmnddd", "F000000GG000000",
-                       "QffffffLLffffff",
-                       AP_HAL::micros64(),
-                       pkt1.ypr[2], pkt1.ypr[1], pkt1.ypr[0],
-                       pkt1.velNED[0], pkt1.velNED[1], pkt1.velNED[2],
-                       int32_t(pkt1.positionLLA[0]*1.0e7), int32_t(pkt1.positionLLA[1]*1.0e7),
-                       float(pkt1.positionLLA[2]),
-                       pkt1.posU, pkt1.velU,
-                       pkt1.yprU[2], pkt1.yprU[1], pkt1.yprU[0]);
 }
 
 /*