From 9d76d3b423c33ef4a01821d63160e7e111d68ba8 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 18 Oct 2014 20:08:46 +0900 Subject: [PATCH] Copter: log DCM reported roll-pitch and yaw error --- ArduCopter/Log.pde | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index d83a9b90c0..9569605f6c 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -464,6 +464,8 @@ struct PACKED log_Attitude { int16_t pitch; uint16_t control_yaw; uint16_t yaw; + uint16_t error_rp; + uint16_t error_yaw; }; // Write an attitude packet @@ -478,7 +480,9 @@ static void Log_Write_Attitude() control_pitch : (int16_t)targets.y, pitch : (int16_t)ahrs.pitch_sensor, control_yaw : (uint16_t)targets.z, - yaw : (uint16_t)ahrs.yaw_sensor + yaw : (uint16_t)ahrs.yaw_sensor, + error_rp : (uint16_t)(ahrs.get_error_rp() * 100), + error_yaw : (uint16_t)(ahrs.get_error_yaw() * 100) }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); @@ -684,7 +688,7 @@ static const struct LogStructure log_structure[] PROGMEM = { { LOG_PERFORMANCE_MSG, sizeof(log_Performance), "PM", "HHIhBHB", "NLon,NLoop,MaxT,PMT,I2CErr,INSErr,INAVErr" }, { LOG_ATTITUDE_MSG, sizeof(log_Attitude), - "ATT", "IccccCC", "TimeMS,DesRoll,Roll,DesPitch,Pitch,DesYaw,Yaw" }, + "ATT", "IccccCCCC", "TimeMS,DesRoll,Roll,DesPitch,Pitch,DesYaw,Yaw,ErrRP,ErrYaw" }, { LOG_MODE_MSG, sizeof(log_Mode), "MODE", "Mh", "Mode,ThrCrs" }, { LOG_STARTUP_MSG, sizeof(log_Startup),