aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
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)); 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 void AP_AHRS::Write_Attitude(const Vector3f &targets) const
{ {
const struct log_Attitude pkt{ const struct log_Attitude pkt{
LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG), LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG),
time_us : AP_HAL::micros64(), time_us : AP_HAL::micros64(),
control_roll : (int16_t)targets.x, control_roll : targets.x,
roll : (int16_t)roll_sensor, roll : degrees(roll),
control_pitch : (int16_t)targets.y, control_pitch : targets.y,
pitch : (int16_t)pitch_sensor, pitch : degrees(pitch),
control_yaw : (uint16_t)wrap_360_cd(targets.z), control_yaw : wrap_360(targets.z),
yaw : (uint16_t)wrap_360_cd(yaw_sensor), yaw : wrap_360(degrees(yaw)),
active : uint8_t(active_EKF_type()), active : uint8_t(active_EKF_type()),
}; };
AP::logger().WriteBlock(&pkt, sizeof(pkt)); AP::logger().WriteBlock(&pkt, sizeof(pkt));
@ -122,18 +122,18 @@ void AP_AHRS::write_video_stabilisation() const
AP::logger().WriteBlock(&pkt, sizeof(pkt)); 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 void AP_AHRS_View::Write_AttitudeView(const Vector3f &targets) const
{ {
const struct log_Attitude pkt{ const struct log_Attitude pkt{
LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG), LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG),
time_us : AP_HAL::micros64(), time_us : AP_HAL::micros64(),
control_roll : (int16_t)targets.x, control_roll : targets.x,
roll : (int16_t)roll_sensor, roll : degrees(roll),
control_pitch : (int16_t)targets.y, control_pitch : targets.y,
pitch : (int16_t)pitch_sensor, pitch : degrees(pitch),
control_yaw : (uint16_t)wrap_360_cd(targets.z), control_yaw : wrap_360(targets.z),
yaw : (uint16_t)wrap_360_cd(yaw_sensor), yaw : wrap_360(degrees(yaw)),
active : uint8_t(AP::ahrs().active_EKF_type()), active : uint8_t(AP::ahrs().active_EKF_type()),
}; };
AP::logger().WriteBlock(&pkt, sizeof(pkt)); 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 { struct PACKED log_Attitude {
LOG_PACKET_HEADER; LOG_PACKET_HEADER;
uint64_t time_us; uint64_t time_us;
int16_t control_roll; float control_roll;
int16_t roll; float roll;
int16_t control_pitch; float control_pitch;
int16_t pitch; float pitch;
uint16_t control_yaw; float control_yaw;
uint16_t yaw; float yaw;
uint8_t active; uint8_t active;
}; };
@ -195,7 +195,7 @@ struct PACKED log_ATSC {
{ LOG_AOA_SSA_MSG, sizeof(log_AOA_SSA), \ { LOG_AOA_SSA_MSG, sizeof(log_AOA_SSA), \
"AOA", "Qff", "TimeUS,AOA,SSA", "sdd", "F00" , true }, \ "AOA", "Qff", "TimeUS,AOA,SSA", "sdd", "F00" , true }, \
{ LOG_ATTITUDE_MSG, sizeof(log_Attitude),\ { 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), \ { LOG_ORGN_MSG, sizeof(log_ORGN), \
"ORGN","QBLLe","TimeUS,Type,Lat,Lng,Alt", "s#DUm", "F-GGB" }, \ "ORGN","QBLLe","TimeUS,Type,Lat,Lng,Alt", "s#DUm", "F-GGB" }, \
{ LOG_POS_MSG, sizeof(log_POS), \ { LOG_POS_MSG, sizeof(log_POS), \