diff --git a/APMrover2/Log.pde b/APMrover2/Log.pde index 762917dbe7..9859051a89 100644 --- a/APMrover2/Log.pde +++ b/APMrover2/Log.pde @@ -447,6 +447,26 @@ static void Log_Write_Compass() motor_offset_z : (int16_t)mag_motor_offsets.z }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); +#if COMPASS_MAX_INSTANCES > 1 + if (compass.get_count() > 1) { + const Vector3f &mag2_offsets = compass.get_offsets(1); + const Vector3f &mag2_motor_offsets = compass.get_motor_offsets(1); + const Vector3f &mag2 = compass.get_field(1); + struct log_Compass pkt2 = { + LOG_PACKET_HEADER_INIT(LOG_COMPASS2_MSG), + mag_x : (int16_t)mag2.x, + mag_y : (int16_t)mag2.y, + mag_z : (int16_t)mag2.z, + offset_x : (int16_t)mag2_offsets.x, + offset_y : (int16_t)mag2_offsets.y, + offset_z : (int16_t)mag2_offsets.z, + motor_offset_x : (int16_t)mag2_motor_offsets.x, + motor_offset_y : (int16_t)mag2_motor_offsets.y, + motor_offset_z : (int16_t)mag2_motor_offsets.z + }; + DataFlash.WriteBlock(&pkt2, sizeof(pkt2)); + } +#endif } @@ -474,6 +494,8 @@ static const struct LogStructure log_structure[] PROGMEM = { "MODE", "MB", "Mode,ModeNum" }, { LOG_COMPASS_MSG, sizeof(log_Compass), "MAG", "hhhhhhhhh", "MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" }, + { LOG_COMPASS2_MSG, sizeof(log_Compass), + "MAG2", "hhhhhhhhh", "MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" }, }; diff --git a/APMrover2/defines.h b/APMrover2/defines.h index 2014c5a3fd..9b4a5b90a4 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -125,6 +125,7 @@ enum ap_message { #define LOG_MODE_MSG 0x09 #define LOG_COMPASS_MSG 0x0A #define LOG_CAMERA_MSG 0x0B +#define LOG_COMPASS2_MSG 0x0C #define TYPE_AIRSTART_MSG 0x00 #define TYPE_GROUNDSTART_MSG 0x01