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

AC_AttitudeControl: ensure Write_Rate() can be called from autotune

This commit is contained in:
Andy Piper 2024-09-22 13:57:29 +01:00 committed by Peter Barker
parent 02a2c788da
commit 053db86abc
1 changed files with 8 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AC_AttitudeControl/AC_AttitudeControl_Logging.cpp
View File

@ -30,20 +30,20 @@ void AC_AttitudeControl::Write_ANG() const
// Write a rate packet // Write a rate packet
void AC_AttitudeControl::Write_Rate(const AC_PosControl &pos_control) const 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 &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{ const struct log_Rate pkt_rate{
LOG_PACKET_HEADER_INIT(LOG_RATE_MSG), LOG_PACKET_HEADER_INIT(LOG_RATE_MSG),
time_us : _rate_gyro_time_us, time_us : _rate_gyro_time_us,
control_roll : degrees(rate_targets.x), control_roll : rate_targets.x,
roll : degrees(gyro_rate.x), roll : gyro_rate.x,
roll_out : _motors.get_roll()+_motors.get_roll_ff(), roll_out : _motors.get_roll()+_motors.get_roll_ff(),
control_pitch : degrees(rate_targets.y), control_pitch : rate_targets.y,
pitch : degrees(gyro_rate.y), pitch : gyro_rate.y,
pitch_out : _motors.get_pitch()+_motors.get_pitch_ff(), pitch_out : _motors.get_pitch()+_motors.get_pitch_ff(),
control_yaw : degrees(rate_targets.z), control_yaw : rate_targets.z,
yaw : degrees(gyro_rate.z), yaw : gyro_rate.z,
yaw_out : _motors.get_yaw()+_motors.get_yaw_ff(), yaw_out : _motors.get_yaw()+_motors.get_yaw_ff(),
control_accel : (float)accel_target.z, control_accel : (float)accel_target.z,
accel : (float)(-(_ahrs.get_accel_ef().z + GRAVITY_MSS) * 100.0f), accel : (float)(-(_ahrs.get_accel_ef().z + GRAVITY_MSS) * 100.0f),