diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index 4fa47eab0f..f091215690 100644 --- a/APMrover2/APMrover2.pde +++ b/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); } } diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde index 93a7fd0e97..1fd5417741 100644 --- a/APMrover2/GCS_Mavlink.pde +++ b/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); } diff --git a/APMrover2/commands_logic.pde b/APMrover2/commands_logic.pde index 9c666b11a8..541de62fbd 100644 --- a/APMrover2/commands_logic.pde +++ b/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 diff --git a/APMrover2/defines.h b/APMrover2/defines.h index 9d34089630..b439a8f170 100644 --- a/APMrover2/defines.h +++ b/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 // ---------------- diff --git a/APMrover2/system.pde b/APMrover2/system.pde index e6d11c116a..7db33eaf1b 100644 --- a/APMrover2/system.pde +++ b/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; +}