From 053db86abc6effc9480416b17c2509672c3468e0 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sun, 22 Sep 2024 13:57:29 +0100 Subject: [PATCH] AC_AttitudeControl: ensure Write_Rate() can be called from autotune --- .../AC_AttitudeControl_Logging.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Logging.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Logging.cpp index 9106ac49a3..d9327f6abf 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Logging.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Logging.cpp @@ -30,20 +30,20 @@ void AC_AttitudeControl::Write_ANG() const // Write a rate packet void AC_AttitudeControl::Write_Rate(const AC_PosControl &pos_control) const { - const Vector3f rate_targets = rate_bf_targets(); + const Vector3f rate_targets = rate_bf_targets() * RAD_TO_DEG; const Vector3f &accel_target = pos_control.get_accel_target_cmss(); - const Vector3f &gyro_rate = _rate_gyro; + const Vector3f gyro_rate = _rate_gyro * RAD_TO_DEG; const struct log_Rate pkt_rate{ LOG_PACKET_HEADER_INIT(LOG_RATE_MSG), time_us : _rate_gyro_time_us, - control_roll : degrees(rate_targets.x), - roll : degrees(gyro_rate.x), + control_roll : rate_targets.x, + roll : gyro_rate.x, roll_out : _motors.get_roll()+_motors.get_roll_ff(), - control_pitch : degrees(rate_targets.y), - pitch : degrees(gyro_rate.y), + control_pitch : rate_targets.y, + pitch : gyro_rate.y, pitch_out : _motors.get_pitch()+_motors.get_pitch_ff(), - control_yaw : degrees(rate_targets.z), - yaw : degrees(gyro_rate.z), + control_yaw : rate_targets.z, + yaw : gyro_rate.z, yaw_out : _motors.get_yaw()+_motors.get_yaw_ff(), control_accel : (float)accel_target.z, accel : (float)(-(_ahrs.get_accel_ef().z + GRAVITY_MSS) * 100.0f),