From 61de480f6299ed63a023bdc60dad275df39b9b56 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 6 Sep 2024 14:24:30 +1000 Subject: [PATCH] AP_AHRS: log DCM errrp and erryaw into DCM message --- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 30 ++++++++++++++++++++------- libraries/AP_AHRS/AP_AHRS_Logging.cpp | 4 ---- libraries/AP_AHRS/LogStructure.h | 6 +----- 3 files changed, 23 insertions(+), 17 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index ad7f01e7be2d46..fd0b8677561eb8 100644 --- 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 index a5d77476238d53..46fd26f49fe951 100644 --- 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 index 618ade0a5c78cb..9769eee06bab94 100644 --- 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), \