From 48385b82dcf00d0fbee11cce674a18d04ac28b13 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Mon, 2 Nov 2020 12:46:43 +1100
Subject: [PATCH] AP_Logger: use instance number for Mag logging

---
 libraries/AP_Logger/AP_Logger.h    |  4 +---
 libraries/AP_Logger/LogFile.cpp    | 19 ++++++-------------
 libraries/AP_Logger/LogStructure.h | 29 ++++++++++-------------------
 3 files changed, 17 insertions(+), 35 deletions(-)

diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h
index 9f7caf5f8c..63fa837f5c 100644
--- a/libraries/AP_Logger/AP_Logger.h
+++ b/libraries/AP_Logger/AP_Logger.h
@@ -460,9 +460,7 @@ private:
 
     void Write_Baro_instance(uint64_t time_us, uint8_t baro_instance);
     void Write_IMU_instance(uint64_t time_us, uint8_t imu_instance);
-    void Write_Compass_instance(uint64_t time_us,
-                                    uint8_t mag_instance,
-                                    enum LogMessages type);
+    void Write_Compass_instance(uint64_t time_us, uint8_t mag_instance);
     void Write_Current_instance(uint64_t time_us, uint8_t battery_instance);
 
     void backend_starting_new_log(const AP_Logger_Backend *backend);
diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp
index 992afbcb47..febe02f56c 100644
--- a/libraries/AP_Logger/LogFile.cpp
+++ b/libraries/AP_Logger/LogFile.cpp
@@ -663,16 +663,17 @@ void AP_Logger::Write_Current()
     }
 }
 
-void AP_Logger::Write_Compass_instance(const uint64_t time_us, const uint8_t mag_instance, const enum LogMessages type)
+void AP_Logger::Write_Compass_instance(const uint64_t time_us, const uint8_t mag_instance)
 {
     const Compass &compass = AP::compass();
 
     const Vector3f &mag_field = compass.get_field(mag_instance);
     const Vector3f &mag_offsets = compass.get_offsets(mag_instance);
     const Vector3f &mag_motor_offsets = compass.get_motor_offsets(mag_instance);
-    const struct log_Compass pkt{
-        LOG_PACKET_HEADER_INIT(type),
+    const struct log_MAG pkt{
+        LOG_PACKET_HEADER_INIT(LOG_MAG_MSG),
         time_us         : time_us,
+        instance        : mag_instance,
         mag_x           : (int16_t)mag_field.x,
         mag_y           : (int16_t)mag_field.y,
         mag_z           : (int16_t)mag_field.z,
@@ -695,16 +696,8 @@ void AP_Logger::Write_Compass(uint64_t time_us)
         time_us = AP_HAL::micros64();
     }
     const Compass &compass = AP::compass();
-    if (compass.get_count() > 0) {
-        Write_Compass_instance(time_us, 0, LOG_COMPASS_MSG);
-    }
-
-    if (compass.get_count() > 1) {
-        Write_Compass_instance(time_us, 1, LOG_COMPASS2_MSG);
-    }
-
-    if (compass.get_count() > 2) {
-        Write_Compass_instance(time_us, 2, LOG_COMPASS3_MSG);
+    for (uint8_t i=0; i<compass.get_count(); i++) {
+        Write_Compass_instance(time_us, i);
     }
 }
 
diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h
index b293156519..3e7417d7fa 100644
--- a/libraries/AP_Logger/LogStructure.h
+++ b/libraries/AP_Logger/LogStructure.h
@@ -838,9 +838,10 @@ struct PACKED log_Current_Cells {
     uint16_t cell_voltages[12];
 };
 
-struct PACKED log_Compass {
+struct PACKED log_MAG {
     LOG_PACKET_HEADER;
     uint64_t time_us;
+    uint8_t  instance;
     int16_t  mag_x;
     int16_t  mag_y;
     int16_t  mag_z;
@@ -1337,11 +1338,6 @@ struct PACKED log_PSC {
 #define ISBD_UNITS  "s--ooo"
 #define ISBD_MULTS  "F--???"
 
-#define MAG_LABELS "TimeUS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ,Health,S"
-#define MAG_FMT   "QhhhhhhhhhBI"
-#define MAG_UNITS "sGGGGGGGGG-s"
-#define MAG_MULTS "FCCCCCCCCC-F"
-
 #define PID_LABELS "TimeUS,Tar,Act,Err,P,I,D,FF,Dmod"
 #define PID_FMT    "Qffffffff"
 #define PID_UNITS  "s--------"
@@ -1759,18 +1755,19 @@ struct PACKED log_PSC {
 // @Field: LandingGear: Current landing gear state
 // @Field: WeightOnWheels: True if there is weight on wheels
 
-// @LoggerMessage: MAG,MAG2,MAG3
+// @LoggerMessage: MAG
 // @Description: Information received from compasses
 // @Field: TimeUS: Time since system startup
+// @Field: I: magnetometer sensor instance number
 // @Field: MagX: magnetic field strength in body frame
 // @Field: MagY: magnetic field strength in body frame
 // @Field: MagZ: magnetic field strength in body frame
 // @Field: OfsX: magnetic field offset in body frame
 // @Field: OfsY: magnetic field offset in body frame
 // @Field: OfsZ: magnetic field offset in body frame
-// @Field: MOfsX: motor interference magnetic field offset in body frame
-// @Field: MOfsY: motor interference magnetic field offset in body frame
-// @Field: MOfsZ: motor interference magnetic field offset in body frame
+// @Field: MOX: motor interference magnetic field offset in body frame
+// @Field: MOY: motor interference magnetic field offset in body frame
+// @Field: MOZ: motor interference magnetic field offset in body frame
 // @Field: Health: true if the compass is considered healthy
 // @Field: S: time measurement was taken
 
@@ -2553,8 +2550,8 @@ struct PACKED log_PSC {
       "BCL", "QBfHHHHHHHHHHHH", "TimeUS,Instance,Volt,V1,V2,V3,V4,V5,V6,V7,V8,V9,V10,V11,V12", "s#vvvvvvvvvvvvv", "F-0CCCCCCCCCCCC" }, \
 	{ LOG_ATTITUDE_MSG, sizeof(log_Attitude),\
       "ATT", "QccccCCCC", "TimeUS,DesRoll,Roll,DesPitch,Pitch,DesYaw,Yaw,ErrRP,ErrYaw", "sddddhhdh", "FBBBBBBBB" }, \
-    { LOG_COMPASS_MSG, sizeof(log_Compass), \
-      "MAG", MAG_FMT,    MAG_LABELS, MAG_UNITS, MAG_MULTS }, \
+    { LOG_MAG_MSG, sizeof(log_MAG), \
+      "MAG", "QBhhhhhhhhhBI",    "TimeUS,I,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOX,MOY,MOZ,Health,S", "s#GGGGGGGGG-s", "F-CCCCCCCCC-F" }, \
     { LOG_MODE_MSG, sizeof(log_Mode), \
       "MODE", "QMBB",         "TimeUS,Mode,ModeNum,Rsn", "s---", "F---" }, \
     { LOG_RFND_MSG, sizeof(log_RFND), \
@@ -2633,10 +2630,6 @@ struct PACKED log_PSC {
       "CSRV","QBfffB","TimeUS,Id,Pos,Force,Speed,Pow", "s#---%", "F-0000" }, \
     { LOG_CESC_MSG, sizeof(log_CESC), \
       "CESC","QBIfffiB","TimeUS,Id,ECnt,Voltage,Curr,Temp,RPM,Pow", "s#-vAOq%", "F-000000" }, \
-    { LOG_COMPASS2_MSG, sizeof(log_Compass), \
-      "MAG2",MAG_FMT,    MAG_LABELS, MAG_UNITS, MAG_MULTS }, \
-    { LOG_COMPASS3_MSG, sizeof(log_Compass), \
-      "MAG3",MAG_FMT,    MAG_LABELS, MAG_UNITS, MAG_MULTS }, \
     { LOG_ACC_MSG, sizeof(log_ACC), \
       "ACC", "QBQfff",        "TimeUS,I,SampleUS,AccX,AccY,AccZ", "s#sooo", "F-F000" }, \
     { LOG_GYR_MSG, sizeof(log_GYR), \
@@ -2784,9 +2777,7 @@ enum LogMessages : uint8_t {
     LOG_ATTITUDE_MSG,
     LOG_CURRENT_MSG,
     LOG_CURRENT_CELLS_MSG,
-    LOG_COMPASS_MSG,
-    LOG_COMPASS2_MSG,
-    LOG_COMPASS3_MSG,
+    LOG_MAG_MSG,
     LOG_MODE_MSG,
     LOG_GPS_RAW_MSG,