diff --git a/AntennaTracker/Log.cpp b/AntennaTracker/Log.cpp index 48048dc1a17cdc..b1adb3ba11af27 100644 --- a/AntennaTracker/Log.cpp +++ b/AntennaTracker/Log.cpp @@ -7,9 +7,7 @@ // Write an attitude packet void Tracker::Log_Write_Attitude() { - Vector3f targets; - targets.y = nav_status.pitch * 100.0f; - targets.z = wrap_360_cd(nav_status.bearing * 100.0f); + const Vector3f targets{0.0f, nav_status.pitch, nav_status.bearing}; ahrs.Write_Attitude(targets); AP::ahrs().Log_Write(); } diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index 63f238f2d7612c..335b4bcc9d4667 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -75,9 +75,7 @@ void Copter::Log_Write_Control_Tuning() // Write an attitude packet void Copter::Log_Write_Attitude() { - Vector3f targets = attitude_control->get_att_target_euler_cd(); - targets.z = wrap_360_cd(targets.z); - ahrs.Write_Attitude(targets); + ahrs.Write_Attitude(attitude_control->get_att_target_euler_rad() * RAD_TO_DEG); ahrs_view->Write_Rate(*motors, *attitude_control, *pos_control); } diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index c9fdd0783b767c..b06bb99cb655f4 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -5,10 +5,11 @@ // Write an attitude packet void Plane::Log_Write_Attitude(void) { - Vector3f targets; // Package up the targets into a vector for commonality with Copter usage of Log_Wrote_Attitude - targets.x = nav_roll_cd; - targets.y = nav_pitch_cd; - targets.z = 0; //Plane does not have the concept of navyaw. This is a placeholder. + Vector3f targets { // Package up the targets into a vector for commonality with Copter usage of Log_Wrote_Attitude + nav_roll_cd * 0.01f, + nav_pitch_cd * 0.01f, + 0 //Plane does not have the concept of navyaw. This is a placeholder. + }; #if HAL_QUADPLANE_ENABLED if (quadplane.show_vtol_view()) { @@ -18,8 +19,7 @@ void Plane::Log_Write_Attitude(void) // since Euler angles are not used and it is a waste of cpu to compute them at the loop rate. // Get them from the quaternion instead: quadplane.attitude_control->get_attitude_target_quat().to_euler(targets.x, targets.y, targets.z); - targets *= degrees(100.0f); - quadplane.ahrs_view->Write_AttitudeView(targets); + quadplane.ahrs_view->Write_AttitudeView(targets * RAD_TO_DEG); } else #endif { diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp index e197ad3ba4d9cb..88642afe26e3d2 100644 --- a/ArduSub/Log.cpp +++ b/ArduSub/Log.cpp @@ -59,9 +59,7 @@ void Sub::Log_Write_Control_Tuning() // Write an attitude packet void Sub::Log_Write_Attitude() { - Vector3f targets = attitude_control.get_att_target_euler_cd(); - targets.z = wrap_360_cd(targets.z); - ahrs.Write_Attitude(targets); + ahrs.Write_Attitude(attitude_control.get_att_target_euler_rad() * RAD_TO_DEG); AP::ahrs().Log_Write(); } diff --git a/Rover/Log.cpp b/Rover/Log.cpp index 3490e2a703d33d..f13ad700add8d5 100644 --- a/Rover/Log.cpp +++ b/Rover/Log.cpp @@ -7,8 +7,8 @@ // Write an attitude packet void Rover::Log_Write_Attitude() { - float desired_pitch_cd = degrees(g2.attitude_control.get_desired_pitch()) * 100.0f; - const Vector3f targets(0.0f, desired_pitch_cd, 0.0f); + float desired_pitch = degrees(g2.attitude_control.get_desired_pitch()); + const Vector3f targets(0.0f, desired_pitch, 0.0f); ahrs.Write_Attitude(targets); diff --git a/libraries/AP_AHRS/AP_AHRS_Logging.cpp b/libraries/AP_AHRS/AP_AHRS_Logging.cpp index 46fd26f49fe951..afeef1fa32d369 100644 --- a/libraries/AP_AHRS/AP_AHRS_Logging.cpp +++ b/libraries/AP_AHRS/AP_AHRS_Logging.cpp @@ -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)); diff --git a/libraries/AP_AHRS/LogStructure.h b/libraries/AP_AHRS/LogStructure.h index 9769eee06bab94..08da53e1c2ea7a 100644 --- a/libraries/AP_AHRS/LogStructure.h +++ b/libraries/AP_AHRS/LogStructure.h @@ -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), \