diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp index fd8aec5f61..7512ddd283 100644 --- a/ArduSub/Log.cpp +++ b/ArduSub/Log.cpp @@ -468,12 +468,17 @@ void Sub::Log_Write_Vehicle_Startup_Messages() // start a new log void Sub::start_logging() { - if (g.log_bitmask != 0) { + if (g.log_bitmask != 0 && !in_log_download) { if (!ap.logging_started) { ap.logging_started = true; DataFlash.set_mission(&mission); DataFlash.setVehicle_Startup_Log_Writer(FUNCTOR_BIND(&sub, &Sub::Log_Write_Vehicle_Startup_Messages, void)); DataFlash.StartNewLog(); + } else if (!DataFlash.logging_started()) { + hal.console->println("Starting new log"); + // dataflash may have stopped logging - when we get_log_data, + // for example. Try to restart: + DataFlash.StartNewLog(); } // enable writes DataFlash.EnableWrites(true); diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp index 1e6590784e..c0a73a46fe 100644 --- a/ArduSub/motors.cpp +++ b/ArduSub/motors.cpp @@ -12,6 +12,7 @@ void Sub::enable_motor_output() // returns false if arming failed because of pre-arm checks, arming checks or a gyro calibration failure bool Sub::init_arm_motors(bool arming_from_gcs) { + start_logging(); static bool in_arm_motors = false; // exit immediately if already in this function @@ -27,6 +28,9 @@ bool Sub::init_arm_motors(bool arming_from_gcs) return false; } + // let dataflash know that we're armed (it may open logs e.g.) + DataFlash_Class::instance()->set_vehicle_armed(true); + // disable cpu failsafe because initialising everything takes a while mainloop_failsafe_disable(); @@ -118,6 +122,7 @@ void Sub::init_disarm_motors() if (!DataFlash.log_while_disarmed()) { DataFlash.EnableWrites(false); } + DataFlash_Class::instance()->set_vehicle_armed(false); // disable gps velocity based centrefugal force compensation ahrs.set_correct_centrifugal(false);