diff --git a/ArduCopter/AP_State.pde b/ArduCopter/AP_State.pde index 86f490bea1..d0f2590c8e 100644 --- a/ArduCopter/AP_State.pde +++ b/ArduCopter/AP_State.pde @@ -13,21 +13,6 @@ void set_home_is_set(bool b) } } -// --------------------------------------------- -void set_armed(bool b) -{ - // if no change, exit immediately - if( ap.armed == b ) - return; - - ap.armed = b; - if(b){ - Log_Write_Event(DATA_ARMED); - }else{ - Log_Write_Event(DATA_DISARMED); - } -} - // --------------------------------------------- void set_auto_armed(bool b) { diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 01914d0345..4c1c53c226 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -366,7 +366,7 @@ static union { uint8_t low_battery : 1; // 4 // Used to track if the battery is low - LED output flashes when the batt is low uint8_t pre_arm_check : 1; // 5 // true if the radio and accel calibration have been performed - uint8_t armed : 1; // 6 + uint8_t logging_started : 1; // 6 // true if dataflash logging has started uint8_t auto_armed : 1; // 7 // stops auto missions from beginning until throttle is raised uint8_t failsafe_radio : 1; // 8 // A status flag for the radio failsafe diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 4f371ef329..fa99470eb1 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -850,7 +850,10 @@ static void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page) // start a new log static void start_logging() { - DataFlash.StartNewLog(sizeof(log_structure)/sizeof(log_structure[0]), log_structure); + if (g.log_bitmask != 0 && !ap.logging_started) { + ap.logging_started = true; + DataFlash.StartNewLog(sizeof(log_structure)/sizeof(log_structure[0]), log_structure); + } } #else // LOGGING_ENABLED diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 2547fa16bc..e6b43e0cce 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -87,10 +87,12 @@ static void init_arm_motors() // which calibrates the IMU static bool did_ground_start = false; - // disable failsafe because initialising everything takes a while + // disable cpu failsafe because initialising everything takes a while failsafe_disable(); + + // start dataflash + start_logging(); - //cliSerial->printf("\nARM\n"); #if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL gcs_send_text_P(SEVERITY_HIGH, PSTR("ARMING MOTORS")); #endif @@ -144,7 +146,6 @@ static void init_arm_motors() // finally actually arm the motors motors.armed(true); - set_armed(true); // reenable failsafe failsafe_enable(); @@ -191,7 +192,6 @@ static void init_disarm_motors() #endif motors.armed(false); - set_armed(false); compass.save_offsets(); diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 8f20a04527..1ffe71f74f 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -176,9 +176,6 @@ static void init_ardupilot() do_erase_logs(); gcs0.reset_cli_timeout(); } - if (g.log_bitmask != 0) { - start_logging(); - } #endif #if FRAME_CONFIG == HELI_FRAME