From 9398fa730edc8662a35a56d044f59a74b00e4b66 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 7 Nov 2013 10:19:13 +1100 Subject: [PATCH] Plane: log the AHRS error terms in dataflash also log raw IMU at 10Hz with default config --- ArduPlane/ArduPlane.pde | 3 +++ ArduPlane/Log.pde | 8 ++++++-- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 7cc32c048e..4eb878d345 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -893,6 +893,9 @@ static void update_logging(void) { if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST)) Log_Write_Attitude(); + + if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_IMU)) + Log_Write_IMU(); if (g.log_bitmask & MASK_LOG_CTUN) Log_Write_Control_Tuning(); diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde index 2bb17db785..c03dff5d0b 100644 --- a/ArduPlane/Log.pde +++ b/ArduPlane/Log.pde @@ -166,6 +166,8 @@ struct PACKED log_Attitude { int16_t roll; int16_t pitch; uint16_t yaw; + uint16_t error_rp; + uint16_t error_yaw; }; // Write an attitude packet. Total length : 10 bytes @@ -176,7 +178,9 @@ static void Log_Write_Attitude(void) time_ms : hal.scheduler->millis(), roll : (int16_t)ahrs.roll_sensor, pitch : (int16_t)ahrs.pitch_sensor, - 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)); } @@ -450,7 +454,7 @@ static void Log_Write_IMU() static const struct LogStructure log_structure[] PROGMEM = { LOG_COMMON_STRUCTURES, { LOG_ATTITUDE_MSG, sizeof(log_Attitude), - "ATT", "IccC", "TimeMS,Roll,Pitch,Yaw" }, + "ATT", "IccCCC", "TimeMS,Roll,Pitch,Yaw,ErrorRP,ErrorYaw" }, { LOG_PERFORMANCE_MSG, sizeof(log_Performance), "PM", "IHIBBBhhhB", "LTime,MLC,gDt,RNCnt,RNBl,GPScnt,GDx,GDy,GDz,I2CErr" }, { LOG_CMD_MSG, sizeof(log_Cmd),