Skip to content

Commit

Permalink
AP_AHRS: use last sample time for Write_Attitude()
Browse files Browse the repository at this point in the history
log attitude dt
  • Loading branch information
andyp1per committed Sep 9, 2024
1 parent 637d3e3 commit f874287
Show file tree
Hide file tree
Showing 4 changed files with 11 additions and 7 deletions.
2 changes: 1 addition & 1 deletion libraries/AP_AHRS/AP_AHRS.h
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
10 changes: 6 additions & 4 deletions libraries/AP_AHRS/AP_AHRS_Logging.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -49,18 +49,19 @@ 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,
pitch : (int16_t)pitch_sensor,
control_yaw : (uint16_t)wrap_360_cd(targets.z),
yaw : (uint16_t)wrap_360_cd(yaw_sensor),
active : uint8_t(active_EKF_type()),
sensor_dt : dt
};
AP::logger().WriteBlock(&pkt, sizeof(pkt));
}
Expand Down Expand Up @@ -123,18 +124,19 @@ 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,
pitch : (int16_t)pitch_sensor,
control_yaw : (uint16_t)wrap_360_cd(targets.z),
yaw : (uint16_t)wrap_360_cd(yaw_sensor),
active : uint8_t(AP::ahrs().active_EKF_type()),
sensor_dt : dt
};
AP::logger().WriteBlock(&pkt, sizeof(pkt));
}
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_AHRS/AP_AHRS_View.h
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
4 changes: 3 additions & 1 deletion libraries/AP_AHRS/LogStructure.h
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,7 @@ struct PACKED log_AOA_SSA {
// @Field: DesYaw: vehicle desired yaw
// @Field: Yaw: achieved vehicle yaw
// @Field: AEKF: active EKF type
// @Field: Dt: attitude delta time
struct PACKED log_Attitude {
LOG_PACKET_HEADER;
uint64_t time_us;
Expand All @@ -68,6 +69,7 @@ struct PACKED log_Attitude {
uint16_t control_yaw;
uint16_t yaw;
uint8_t active;
float sensor_dt;
};

// @LoggerMessage: ORGN
Expand Down Expand Up @@ -195,7 +197,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", "QccccCCB", "TimeUS,DesRoll,Roll,DesPitch,Pitch,DesYaw,Yaw,AEKF", "sddddhh-", "FBBBBBB-" , true }, \
"ATT", "QccccCCBf", "TimeUS,DesRoll,Roll,DesPitch,Pitch,DesYaw,Yaw,AEKF,Dt", "sddddhh-s", "FBBBBBB-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), \
Expand Down

0 comments on commit f874287

Please sign in to comment.