|
|
|
@ -127,13 +127,13 @@ void AP_AHRS_View::Write_Rate(const AP_Motors &motors, const AC_AttitudeControl
@@ -127,13 +127,13 @@ void AP_AHRS_View::Write_Rate(const AP_Motors &motors, const AC_AttitudeControl
|
|
|
|
|
time_us : AP_HAL::micros64(), |
|
|
|
|
control_roll : degrees(rate_targets.x), |
|
|
|
|
roll : degrees(get_gyro().x), |
|
|
|
|
roll_out : motors.get_roll(), |
|
|
|
|
roll_out : motors.get_roll()+motors.get_roll_ff(), |
|
|
|
|
control_pitch : degrees(rate_targets.y), |
|
|
|
|
pitch : degrees(get_gyro().y), |
|
|
|
|
pitch_out : motors.get_pitch(), |
|
|
|
|
pitch_out : motors.get_pitch()+motors.get_pitch_ff(), |
|
|
|
|
control_yaw : degrees(rate_targets.z), |
|
|
|
|
yaw : degrees(get_gyro().z), |
|
|
|
|
yaw_out : motors.get_yaw(), |
|
|
|
|
yaw_out : motors.get_yaw()+motors.get_yaw_ff(), |
|
|
|
|
control_accel : (float)accel_target.z, |
|
|
|
|
accel : (float)(-(get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f), |
|
|
|
|
accel_out : motors.get_throttle() |
|
|
|
|