From 21b0b3bc153c7319d7d26cb38fddf0c45a91299a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 4 Jul 2014 12:07:59 +1000 Subject: [PATCH] Copter: log up to 3 mags --- ArduCopter/Log.pde | 27 +++++++++++++++++++++++++++ ArduCopter/defines.h | 1 + 2 files changed, 28 insertions(+) diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 8b0167b6b8..8029332069 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -393,6 +393,27 @@ static void Log_Write_Compass() DataFlash.WriteBlock(&pkt2, sizeof(pkt2)); } #endif +#if COMPASS_MAX_INSTANCES > 2 + if (compass.get_count() > 2) { + const Vector3f &mag3_offsets = compass.get_offsets(2); + const Vector3f &mag3_motor_offsets = compass.get_motor_offsets(2); + const Vector3f &mag3 = compass.get_field(2); + struct log_Compass pkt3 = { + LOG_PACKET_HEADER_INIT(LOG_COMPASS3_MSG), + time_ms : hal.scheduler->millis(), + mag_x : (int16_t)mag3.x, + mag_y : (int16_t)mag3.y, + mag_z : (int16_t)mag3.z, + offset_x : (int16_t)mag3_offsets.x, + offset_y : (int16_t)mag3_offsets.y, + offset_z : (int16_t)mag3_offsets.z, + motor_offset_x : (int16_t)mag3_motor_offsets.x, + motor_offset_y : (int16_t)mag3_motor_offsets.y, + motor_offset_z : (int16_t)mag3_motor_offsets.z + }; + DataFlash.WriteBlock(&pkt3, sizeof(pkt3)); + } +#endif } struct PACKED log_Performance { @@ -649,8 +670,14 @@ static const struct LogStructure log_structure[] PROGMEM = { "CTUN", "Ihhhffecchh", "TimeMS,ThrIn,AngBst,ThrOut,DAlt,Alt,BarAlt,DSAlt,SAlt,DCRt,CRt" }, { LOG_COMPASS_MSG, sizeof(log_Compass), "MAG", "Ihhhhhhhhh", "TimeMS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" }, +#if COMPASS_MAX_INSTANCES > 1 { LOG_COMPASS2_MSG, sizeof(log_Compass), "MAG2","Ihhhhhhhhh", "TimeMS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" }, +#endif +#if COMPASS_MAX_INSTANCES > 2 + { LOG_COMPASS3_MSG, sizeof(log_Compass), + "MAG3","Ihhhhhhhhh", "TimeMS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" }, +#endif { LOG_PERFORMANCE_MSG, sizeof(log_Performance), "PM", "HHIhBHB", "NLon,NLoop,MaxT,PMT,I2CErr,INSErr,INAVErr" }, { LOG_ATTITUDE_MSG, sizeof(log_Attitude), diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 0fa0e44926..959b1beafa 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -230,6 +230,7 @@ enum FlipState { #define LOG_AUTOTUNE_MSG 0x19 #define LOG_AUTOTUNEDETAILS_MSG 0x1A #define LOG_COMPASS2_MSG 0x1B +#define LOG_COMPASS3_MSG 0x1C #define MASK_LOG_ATTITUDE_FAST (1<<0) #define MASK_LOG_ATTITUDE_MED (1<<1)