From 4ebc2a11f6a82574089a8242cbd327f86ac67147 Mon Sep 17 00:00:00 2001
From: Rustom Jehangir <rustyj110@gmail.com>
Date: Thu, 21 Apr 2016 16:06:15 -0700
Subject: [PATCH] Sub: Match Copter changes.

---
 ArduSub/Log.cpp       | 37 +++++++++++--------------------------
 ArduSub/Sub.h         |  1 +
 ArduSub/defines.h     |  1 -
 ArduSub/motors.cpp    | 12 ++++++++----
 ArduSub/perf_info.cpp |  8 ++++++++
 5 files changed, 28 insertions(+), 31 deletions(-)

diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp
index 378648f7ec78f2..46ffcce0e0e5bc 100644
--- a/ArduSub/Log.cpp
+++ b/ArduSub/Log.cpp
@@ -339,6 +339,7 @@ struct PACKED log_Performance {
     int16_t  pm_test;
     uint8_t i2c_lockup_count;
     uint16_t ins_error_count;
+    uint32_t log_dropped;
 };
 
 // Write a performance monitoring packet
@@ -352,9 +353,10 @@ void Sub::Log_Write_Performance()
         max_time         : perf_info_get_max_time(),
         pm_test          : pmTest1,
         i2c_lockup_count : hal.i2c->lockup_count(),
-        ins_error_count  : ins.error_count()
+        ins_error_count  : ins.error_count(),
+		log_dropped      : DataFlash.num_dropped() - perf_info_get_num_dropped(),
     };
-    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+    DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
 }
 
 // Write an attitude packet
@@ -399,21 +401,6 @@ void Sub::Log_Write_MotBatt()
     DataFlash.WriteBlock(&pkt_mot, sizeof(pkt_mot));
 }
 
-struct PACKED log_Startup {
-    LOG_PACKET_HEADER;
-    uint64_t time_us;
-};
-
-// Write Startup packet
-void Sub::Log_Write_Startup()
-{
-    struct log_Startup pkt = {
-        LOG_PACKET_HEADER_INIT(LOG_STARTUP_MSG),
-        time_us         : AP_HAL::micros64()
-    };
-    DataFlash.WriteBlock(&pkt, sizeof(pkt));
-}
-
 struct PACKED log_Event {
     LOG_PACKET_HEADER;
     uint64_t time_us;
@@ -451,7 +438,7 @@ void Sub::Log_Write_Data(uint8_t id, int16_t value)
             id          : id,
             data_value  : value
         };
-        DataFlash.WriteBlock(&pkt, sizeof(pkt));
+        DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
     }
 }
 
@@ -473,7 +460,7 @@ void Sub::Log_Write_Data(uint8_t id, uint16_t value)
             id          : id,
             data_value  : value
         };
-        DataFlash.WriteBlock(&pkt, sizeof(pkt));
+        DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
     }
 }
 
@@ -494,7 +481,7 @@ void Sub::Log_Write_Data(uint8_t id, int32_t value)
             id          : id,
             data_value  : value
         };
-        DataFlash.WriteBlock(&pkt, sizeof(pkt));
+        DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
     }
 }
 
@@ -515,7 +502,7 @@ void Sub::Log_Write_Data(uint8_t id, uint32_t value)
             id          : id,
             data_value  : value
         };
-        DataFlash.WriteBlock(&pkt, sizeof(pkt));
+        DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
     }
 }
 
@@ -537,7 +524,7 @@ void Sub::Log_Write_Data(uint8_t id, float value)
             id          : id,
             data_value  : value
         };
-        DataFlash.WriteBlock(&pkt, sizeof(pkt));
+        DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
     }
 }
 
@@ -557,7 +544,7 @@ void Sub::Log_Write_Error(uint8_t sub_system, uint8_t error_code)
         sub_system    : sub_system,
         error_code    : error_code,
     };
-    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+    DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
 }
 
 void Sub::Log_Write_Baro(void)
@@ -685,11 +672,9 @@ const struct LogStructure Sub::log_structure[] = {
     { LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning),
       "CTUN", "Qfffffecchh", "TimeUS,ThrIn,AngBst,ThrOut,DAlt,Alt,BarAlt,DSAlt,SAlt,DCRt,CRt" },
     { LOG_PERFORMANCE_MSG, sizeof(log_Performance), 
-      "PM",  "QHHIhBH",    "TimeUS,NLon,NLoop,MaxT,PMT,I2CErr,INSErr" },
+      "PM",  "QHHIhBHI",    "TimeUS,NLon,NLoop,MaxT,PMT,I2CErr,INSErr,LogDrop" },
     { LOG_MOTBATT_MSG, sizeof(log_MotBatt),
       "MOTB", "Qffff",  "TimeUS,LiftMax,BatVolt,BatRes,ThLimit" },
-    { LOG_STARTUP_MSG, sizeof(log_Startup),         
-      "STRT", "Q",            "TimeUS" },
     { LOG_EVENT_MSG, sizeof(log_Event),         
       "EV",   "QB",           "TimeUS,Id" },
     { LOG_DATA_INT16_MSG, sizeof(log_Data_Int16t),         
diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h
index 3762bb4c169018..68cd925348f762 100644
--- a/ArduSub/Sub.h
+++ b/ArduSub/Sub.h
@@ -882,6 +882,7 @@ class Sub : public AP_HAL::HAL::Callbacks {
     uint32_t perf_info_get_max_time();
     uint32_t perf_info_get_min_time();
     uint16_t perf_info_get_num_long_running();
+    uint32_t perf_info_get_num_dropped();
     Vector3f pv_location_to_vector(const Location& loc);
     Vector3f pv_location_to_vector_with_default(const Location& loc, const Vector3f& default_posvec);
     float pv_alt_above_origin(float alt_above_home_cm);
diff --git a/ArduSub/defines.h b/ArduSub/defines.h
index f263bb29b63ad4..7138cd74f217ab 100644
--- a/ArduSub/defines.h
+++ b/ArduSub/defines.h
@@ -268,7 +268,6 @@ enum ThrowModeState {
 #define LOG_CONTROL_TUNING_MSG          0x04
 #define LOG_NAV_TUNING_MSG              0x05
 #define LOG_PERFORMANCE_MSG             0x06
-#define LOG_STARTUP_MSG                 0x0A
 #define LOG_OPTFLOW_MSG                 0x0C
 #define LOG_EVENT_MSG                   0x0D
 #define LOG_PID_MSG                     0x0E    // deprecated
diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp
index 0cc4271b077709..2b6ba6478e31ea 100644
--- a/ArduSub/motors.cpp
+++ b/ArduSub/motors.cpp
@@ -214,10 +214,14 @@ void Sub::init_disarm_motors()
 #endif
 
     // save compass offsets learned by the EKF
-    Vector3f magOffsets;
-    if (ahrs.use_compass() && ahrs.getMagOffsets(magOffsets)) {
-        compass.set_and_save_offsets(compass.get_primary(), magOffsets);
-    }
+    if (ahrs.use_compass()) {
+        for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
+            Vector3f magOffsets;
+            if (ahrs.getMagOffsets(i, magOffsets)) {
+                compass.set_and_save_offsets(i, magOffsets);
+            }
+        }
+	 }
 
 #if AUTOTUNE_ENABLED == ENABLED
     // save auto tuned parameters
diff --git a/ArduSub/perf_info.cpp b/ArduSub/perf_info.cpp
index 5d31061df1e9b6..130dec61a3bfcb 100644
--- a/ArduSub/perf_info.cpp
+++ b/ArduSub/perf_info.cpp
@@ -15,6 +15,7 @@ static uint16_t perf_info_loop_count;
 static uint32_t perf_info_max_time;
 static uint32_t perf_info_min_time;
 static uint16_t perf_info_long_running;
+static uint32_t perf_info_log_dropped;
 static bool perf_ignore_loop = false;
 
 // perf_info_reset - reset all records of loop time to zero
@@ -24,6 +25,7 @@ void Sub::perf_info_reset()
     perf_info_max_time = 0;
     perf_info_min_time = 0;
     perf_info_long_running = 0;
+    perf_info_log_dropped = DataFlash.num_dropped();
 }
 
 // perf_ignore_loop - ignore this loop from performance measurements (used to reduce false positive when arming)
@@ -77,3 +79,9 @@ uint16_t Sub::perf_info_get_num_long_running()
 {
     return perf_info_long_running;
 }
+
+// perf_info_get_num_dropped - get number of dropped log messages
+uint32_t Sub::perf_info_get_num_dropped()
+{
+    return perf_info_log_dropped;
+}