diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index a5aace188f..add71875dc 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -566,7 +566,9 @@ void Copter::loop_rate_logging() // should be run at 10hz void Copter::ten_hz_logging_loop() { - // log attitude data if we're not already logging at the higher rate + // always write AHRS attitude at 10Hz + ahrs.Write_Attitude(attitude_control->get_att_target_euler_rad() * RAD_TO_DEG); + // log attitude controller data if we're not already logging at the higher rate if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) { Log_Write_Attitude(); } diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index cb07bd3d9a..fffea298f1 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -75,7 +75,7 @@ void Copter::Log_Write_Control_Tuning() // Write an attitude packet void Copter::Log_Write_Attitude() { - ahrs.Write_Attitude(attitude_control->get_att_target_euler_rad() * RAD_TO_DEG); + attitude_control->Write_ANG(); attitude_control->Write_Rate(*pos_control); }