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

AP_AHRS: log attitude and attitude target at full resolution in degrees

This commit is contained in:
Andy Piper 2024-08-31 10:30:23 +01:00 committed by Peter Barker
parent 65e15f2bd9
commit 8dee817acf
2 changed files with 21 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_AHRS/AP_AHRS_Logging.cpp
View File

@ -48,18 +48,18 @@ void AP_AHRS::Write_AOA_SSA(void) const
AP::logger().WriteBlock(&aoa_ssa, sizeof(aoa_ssa));
}
// Write an attitude packet
// Write an attitude packet, targets in degrees
void AP_AHRS::Write_Attitude(const Vector3f &targets) const
{
const struct log_Attitude pkt{
LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG),
time_us : AP_HAL::micros64(),
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),
control_roll : targets.x,
roll : degrees(roll),
control_pitch : targets.y,
pitch : degrees(pitch),
control_yaw : wrap_360(targets.z),
yaw : wrap_360(degrees(yaw)),
active : uint8_t(active_EKF_type()),
};
AP::logger().WriteBlock(&pkt, sizeof(pkt));
@ -122,18 +122,18 @@ void AP_AHRS::write_video_stabilisation() const
AP::logger().WriteBlock(&pkt, sizeof(pkt));
}
// Write an attitude view packet
// Write an attitude view packet, targets in degrees
void AP_AHRS_View::Write_AttitudeView(const Vector3f &targets) const
{
const struct log_Attitude pkt{
LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG),
time_us : AP_HAL::micros64(),
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),
control_roll : targets.x,
roll : degrees(roll),
control_pitch : targets.y,
pitch : degrees(pitch),
control_yaw : wrap_360(targets.z),
yaw : wrap_360(degrees(yaw)),
active : uint8_t(AP::ahrs().active_EKF_type()),
};
AP::logger().WriteBlock(&pkt, sizeof(pkt));

14
libraries/AP_AHRS/LogStructure.h
View File

@ -61,12 +61,12 @@ struct PACKED log_AOA_SSA {
struct PACKED log_Attitude {
LOG_PACKET_HEADER;
uint64_t time_us;
int16_t control_roll;
int16_t roll;
int16_t control_pitch;
int16_t pitch;
uint16_t control_yaw;
uint16_t yaw;
float control_roll;
float roll;
float control_pitch;
float pitch;
float control_yaw;
float yaw;
uint8_t active;
};
@ -195,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", "QccccCCB", "TimeUS,DesRoll,Roll,DesPitch,Pitch,DesYaw,Yaw,AEKF", "sddddhh-", "FBBBBBB-" , true }, \
"ATT", "QffffffB", "TimeUS,DesRoll,Roll,DesPitch,Pitch,DesYaw,Yaw,AEKF", "sddddhh-", "F000000-" , true }, \
{ LOG_ORGN_MSG, sizeof(log_ORGN), \
"ORGN","QBLLe","TimeUS,Type,Lat,Lng,Alt", "s#DUm", "F-GGB" }, \
{ LOG_POS_MSG, sizeof(log_POS), \