diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp index 3f6c61b0adebf7..37a57e68117ca0 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp @@ -30,6 +30,7 @@ #include #include +#include extern const AP_HAL::HAL &hal; @@ -81,6 +82,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 }; @@ -287,6 +295,39 @@ 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 + + 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", + "sdddnnnDUm-", + "F000000GG0-", + "QffffffLLfI", + 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); + } } // Get model/type name diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h index d49515d524e0c5..dfc81532c27344 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h @@ -178,6 +178,7 @@ class AP_ExternalAHRS { AP_Enum devtype; AP_Int16 rate; + AP_Int16 log_rate; AP_Int16 options; AP_Int16 sensors; @@ -192,6 +193,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 index d10ca6997c7cd7..43e858a74007ce 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp @@ -501,28 +501,6 @@ 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 @@ -530,7 +508,7 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() // @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", @@ -538,7 +516,7 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() 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 @@ -553,7 +531,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 index 26f379868a2dff..179f05fd9f1e2c 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp @@ -511,38 +511,6 @@ void AP_ExternalAHRS_VectorNav::process_packet1(const uint8_t *b) AP::ins().handle_external(ins); } - - -#if HAL_LOGGING_ENABLED - // @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]); -#endif // HAL_LOGGING_ENABLED } /*