diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index e6d40a6fc5fa47..0e0bd300d0e843 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -503,7 +503,7 @@ class AP_AHRS { // Logging functions void Log_Write_Home_And_Origin(); - void Write_Attitude(const Vector3f &targets) const; + void Write_Attitude(const Vector3f &targets, uint64_t sample_time_us, float dt) const; enum class LogOriginType { ekf_origin = 0, diff --git a/libraries/AP_AHRS/AP_AHRS_Logging.cpp b/libraries/AP_AHRS/AP_AHRS_Logging.cpp index a5d77476238d53..a63da5bf253eda 100644 --- a/libraries/AP_AHRS/AP_AHRS_Logging.cpp +++ b/libraries/AP_AHRS/AP_AHRS_Logging.cpp @@ -49,11 +49,11 @@ void AP_AHRS::Write_AOA_SSA(void) const } // Write an attitude packet -void AP_AHRS::Write_Attitude(const Vector3f &targets) const +void AP_AHRS::Write_Attitude(const Vector3f &targets, uint64_t sample_time_us, float dt) const { const struct log_Attitude pkt{ LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG), - time_us : AP_HAL::micros64(), + time_us : sample_time_us, control_roll : (int16_t)targets.x, roll : (int16_t)roll_sensor, control_pitch : (int16_t)targets.y, @@ -63,6 +63,7 @@ void AP_AHRS::Write_Attitude(const Vector3f &targets) const error_rp : (uint16_t)(get_error_rp() * 100), error_yaw : (uint16_t)(get_error_yaw() * 100), active : uint8_t(active_EKF_type()), + sensor_dt : dt }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); } @@ -125,11 +126,11 @@ void AP_AHRS::write_video_stabilisation() const } // Write an attitude view packet -void AP_AHRS_View::Write_AttitudeView(const Vector3f &targets) const +void AP_AHRS_View::Write_AttitudeView(const Vector3f &targets, uint64_t sample_time_us, float dt) const { const struct log_Attitude pkt{ LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG), - time_us : AP_HAL::micros64(), + time_us : sample_time_us, control_roll : (int16_t)targets.x, roll : (int16_t)roll_sensor, control_pitch : (int16_t)targets.y, @@ -139,6 +140,7 @@ void AP_AHRS_View::Write_AttitudeView(const Vector3f &targets) const error_rp : (uint16_t)(get_error_rp() * 100), error_yaw : (uint16_t)(get_error_yaw() * 100), active : uint8_t(AP::ahrs().active_EKF_type()), + sensor_dt : dt }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); } diff --git a/libraries/AP_AHRS/AP_AHRS_View.h b/libraries/AP_AHRS/AP_AHRS_View.h index ff0f2293c16510..4a5fc851a65e30 100644 --- a/libraries/AP_AHRS/AP_AHRS_View.h +++ b/libraries/AP_AHRS/AP_AHRS_View.h @@ -173,7 +173,7 @@ class AP_AHRS_View } // Logging Functions - void Write_AttitudeView(const Vector3f &targets) const; + void Write_AttitudeView(const Vector3f &targets, uint64_t sample_time_us, float dt) const; void Write_Rate(const class AP_Motors &motors, const class AC_AttitudeControl &attitude_control, const AC_PosControl &pos_control) const; diff --git a/libraries/AP_AHRS/LogStructure.h b/libraries/AP_AHRS/LogStructure.h index 618ade0a5c78cb..28b79d416f859e 100644 --- a/libraries/AP_AHRS/LogStructure.h +++ b/libraries/AP_AHRS/LogStructure.h @@ -53,13 +53,14 @@ struct PACKED log_AOA_SSA { // @Field: TimeUS: Time since system startup // @Field: DesRoll: vehicle desired roll // @Field: Roll: achieved vehicle roll -// @Field: DesPitch: vehicle desired pitch -// @Field: Pitch: achieved vehicle pitch +// @Field: DesPit: vehicle desired pitch +// @Field: Pit: 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 +// @Field: Dt: attitude delta time struct PACKED log_Attitude { LOG_PACKET_HEADER; uint64_t time_us; @@ -72,6 +73,7 @@ struct PACKED log_Attitude { uint16_t error_rp; uint16_t error_yaw; uint8_t active; + float sensor_dt; }; // @LoggerMessage: ORGN @@ -199,7 +201,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", "QccccCCCCBf", "TimeUS,DesRoll,Roll,DesPit,Pit,DesYaw,Yaw,ErrRP,ErrYaw,AEKF,Dt", "sddddhhdh-s", "FBBBBBBBB-0" , true }, \ { LOG_ORGN_MSG, sizeof(log_ORGN), \ "ORGN","QBLLe","TimeUS,Type,Lat,Lng,Alt", "s#DUm", "F-GGB" }, \ { LOG_POS_MSG, sizeof(log_POS), \