Rover: only log when armed

APMrover2/APMrover2.pde

@@ -172,6 +172,7 @@ static DataFlash_File DataFlash("logs");
 DataFlash_Empty DataFlash;
 #endif
 
+static bool in_log_download;
 
 ////////////////////////////////////////////////////////////////////////////////
 // Sensors
@@ -648,10 +649,10 @@ static void ahrs_update()
 
     ahrs.update();
 
-    if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
+    if (should_log(MASK_LOG_ATTITUDE_FAST))
         Log_Write_Attitude();
 
-    if (g.log_bitmask & MASK_LOG_IMU)
+    if (should_log(MASK_LOG_IMU))
         DataFlash.Log_Write_IMU(ins);
 }
 
@@ -697,7 +698,7 @@ static void update_compass(void)
         ahrs.set_compass(&compass);
         // update offsets
         compass.null_offsets();
-        if (g.log_bitmask & MASK_LOG_COMPASS) {
+        if (should_log(MASK_LOG_COMPASS)) {
             Log_Write_Compass();
         }
     } else {
@@ -710,13 +711,13 @@ static void update_compass(void)
  */
 static void update_logging1(void)
 {
-    if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST))
+    if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST))
         Log_Write_Attitude();
     
-    if (g.log_bitmask & MASK_LOG_CTUN)
+    if (should_log(MASK_LOG_CTUN))
         Log_Write_Control_Tuning();
 
-    if (g.log_bitmask & MASK_LOG_NTUN)
+    if (should_log(MASK_LOG_NTUN))
         Log_Write_Nav_Tuning();
 }
 
@@ -725,13 +726,13 @@ static void update_logging1(void)
  */
 static void update_logging2(void)
 {
-    if (g.log_bitmask & MASK_LOG_STEERING) {
+    if (should_log(MASK_LOG_STEERING)) {
         if (control_mode == STEERING || control_mode == AUTO || control_mode == RTL || control_mode == GUIDED) {
             Log_Write_Steering();
         }
     }
 
-    if (g.log_bitmask & MASK_LOG_RC)
+    if (should_log(MASK_LOG_RC))
         Log_Write_RC();
 }
 
@@ -760,7 +761,7 @@ static void update_aux(void)
  */
 static void one_second_loop(void)
 {
-	if (g.log_bitmask & MASK_LOG_CURRENT)
+	if (should_log(MASK_LOG_CURRENT))
 		Log_Write_Current();
 	// send a heartbeat
 	gcs_send_message(MSG_HEARTBEAT);
@@ -789,7 +790,7 @@ static void one_second_loop(void)
         if (scheduler.debug() != 0) {
             hal.console->printf_P(PSTR("G_Dt_max=%lu\n"), (unsigned long)G_Dt_max);
         }
-        if (g.log_bitmask & MASK_LOG_PM)
+        if (should_log(MASK_LOG_PM))
             Log_Write_Performance();
         G_Dt_max = 0;
         resetPerfData();
@@ -811,7 +812,7 @@ static void update_GPS_50Hz(void)
 
     if (g_gps->last_message_time_ms() != last_gps_reading) {
         last_gps_reading = g_gps->last_message_time_ms();
-        if (g.log_bitmask & MASK_LOG_GPS) {
+        if (should_log(MASK_LOG_GPS)) {
             DataFlash.Log_Write_GPS(g_gps, current_loc.alt);
         }
     }

APMrover2/GCS_Mavlink.pde

@@ -1854,7 +1854,17 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             break;
         }
 
-    case MAVLINK_MSG_ID_LOG_REQUEST_LIST ... MAVLINK_MSG_ID_LOG_REQUEST_END:
+    case MAVLINK_MSG_ID_LOG_REQUEST_DATA:
+    case MAVLINK_MSG_ID_LOG_ERASE:
+        in_log_download = true;
+        // fallthru
+    case MAVLINK_MSG_ID_LOG_REQUEST_LIST:
+        if (!in_mavlink_delay) {
+            handle_log_message(msg, DataFlash);
+        }
+        break;
+    case MAVLINK_MSG_ID_LOG_REQUEST_END:
+        in_log_download = false;
         if (!in_mavlink_delay) {
             handle_log_message(msg, DataFlash);
         }

APMrover2/commands_logic.pde

@@ -412,7 +412,7 @@ static void do_take_picture()
 {
 #if CAMERA == ENABLED
     camera.trigger_pic();
-    if (g.log_bitmask & MASK_LOG_CAMERA) {
+    if (should_log(MASK_LOG_CAMERA)) {
         Log_Write_Camera();
     }
 #endif

APMrover2/defines.h

@@ -116,6 +116,7 @@ enum mode {
 #define MASK_LOG_CAMERA   		(1<<12)
 #define MASK_LOG_STEERING  		(1<<13)
 #define MASK_LOG_RC     		(1<<14)
+#define MASK_LOG_WHEN_DISARMED  (1<<30)
 
 // Waypoint Modes
 // ----------------

APMrover2/system.pde

@@ -513,3 +513,25 @@ static void servo_write(uint8_t ch, uint16_t pwm)
     hal.rcout->write(ch, pwm);
 #endif
 }
+
+/*
+  should we log a message type now?
+ */
+static bool should_log(uint32_t mask)
+{
+    if (!(mask & g.log_bitmask) || in_mavlink_delay) {
+        return false;
+    }
+    bool armed;
+    armed = (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED);
+
+    bool ret = armed || (g.log_bitmask & MASK_LOG_WHEN_DISARMED) != 0;
+    if (ret && !DataFlash.logging_started() && !in_log_download) {
+        // we have to set in_mavlink_delay to prevent logging while
+        // writing headers
+        in_mavlink_delay = true;
+        start_logging();
+        in_mavlink_delay = false;
+    }
+    return ret;
+}