diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 9b641c75b7..3dab4fa706 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -164,6 +164,9 @@ DataFlash_Empty DataFlash; #endif #endif +// has a log download started? +static bool in_log_download; + // scaled roll limit based on pitch static int32_t roll_limit_cd; static int32_t pitch_limit_min_cd; diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 94abbbfd78..c6354a1341 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -2182,7 +2182,17 @@ mission_failed: 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); } diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 2a1b1706c3..852cf9380f 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -142,9 +142,6 @@ static void init_ardupilot() gcs[i].reset_cli_timeout(); } } - if (g.log_bitmask != 0) { - start_logging(); - } #endif // Register mavlink_delay_cb, which will run anytime you have @@ -624,5 +621,9 @@ static bool should_log(uint32_t mask) } else { armed = arming.is_armed(); } - return armed || (g.log_bitmask & MASK_LOG_WHEN_DISARMED) != 0; + bool ret = armed || (g.log_bitmask & MASK_LOG_WHEN_DISARMED) != 0; + if (ret && !DataFlash.logging_started() && !in_log_download) { + start_logging(); + } + return ret; }