Rover: Change Compass logging to use DataFlash library method.

APMrover2/Log.pde

@@ -358,80 +358,19 @@ static void Log_Write_Current()
     DataFlash.Log_Write_Power();
 }
 
-struct PACKED log_Compass {
+// Write a Compass packet
-    LOG_PACKET_HEADER;
-    uint32_t time_ms;
-    int16_t mag_x;
-    int16_t mag_y;
-    int16_t mag_z;
-    int16_t offset_x;
-    int16_t offset_y;
-    int16_t offset_z;
-    int16_t motor_offset_x;
-    int16_t motor_offset_y;
-    int16_t motor_offset_z;
-};
-
-// Write a Compass packet. Total length : 15 bytes
 static void Log_Write_Compass()
 {
-    const Vector3f &mag_offsets = compass.get_offsets();
+    DataFlash.Log_Write_Compass(compass, 0);
-    const Vector3f &mag_motor_offsets = compass.get_motor_offsets();
+    
-    const Vector3f &mag = compass.get_field();
-    struct log_Compass pkt = {
-        LOG_PACKET_HEADER_INIT(LOG_COMPASS_MSG),
-        time_ms         : millis(),
-        mag_x           : (int16_t)mag.x,
-        mag_y           : (int16_t)mag.y,
-        mag_z           : (int16_t)mag.z,
-        offset_x        : (int16_t)mag_offsets.x,
-        offset_y        : (int16_t)mag_offsets.y,
-        offset_z        : (int16_t)mag_offsets.z,
-        motor_offset_x  : (int16_t)mag_motor_offsets.x,
-        motor_offset_y  : (int16_t)mag_motor_offsets.y,
-        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);
+        DataFlash.Log_Write_Compass(compass, 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),
-            time_ms         : millis(),
-            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
 #if COMPASS_MAX_INSTANCES > 2
     if (compass.get_count() > 2) {
-        const Vector3f &mag3_offsets = compass.get_offsets(2);
+        DataFlash.Log_Write_Compass(compass, 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         : 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
 }
@@ -460,10 +399,6 @@ static const struct LogStructure log_structure[] PROGMEM = {
       "NTUN", "IHfHHb",     "TimeMS,Yaw,WpDist,TargBrg,NavBrg,Thr" },
     { LOG_SONAR_MSG, sizeof(log_Sonar),             
       "SONR", "IfHHHbHCb",  "TimeMS,LatAcc,S1Dist,S2Dist,DCnt,TAng,TTim,Spd,Thr" },
-    { LOG_COMPASS_MSG, sizeof(log_Compass),             
-      "MAG", "Ihhhhhhhhh",  "TimeMS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" },
-    { LOG_COMPASS2_MSG, sizeof(log_Compass),             
-      "MAG2", "Ihhhhhhhhh",   "TimeMS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" },
     { LOG_STEERING_MSG, sizeof(log_Steering),             
       "STER", "Iff",   "TimeMS,Demanded,Achieved" },
 };

APMrover2/defines.h

@@ -67,10 +67,7 @@ enum mode {
 #define LOG_PERFORMANCE_MSG		0x03
 #define LOG_STARTUP_MSG 		0x06
 #define LOG_SONAR_MSG 		    0x07
-#define LOG_COMPASS_MSG         0x0A
-#define LOG_COMPASS2_MSG        0x0C
 #define LOG_STEERING_MSG        0x0D
-#define LOG_COMPASS3_MSG        0x0E
 
 #define TYPE_AIRSTART_MSG		0x00
 #define TYPE_GROUNDSTART_MSG	0x01