Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: log DCM errrp and erryaw into DCM message

This commit is contained in:
Peter Barker 2024-09-06 14:24:30 +10:00 committed by Peter Barker
parent 9726e8e218
commit 61de480f62
3 changed files with 23 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -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
}

4
libraries/AP_AHRS/AP_AHRS_Logging.cpp
View File

@ -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));

6
libraries/AP_AHRS/LogStructure.h
View File

@ -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), \