diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 3652c20431..c7a81270a2 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -319,6 +319,8 @@ private: void handle_log_request_list(mavlink_message_t *msg, DataFlash_Class &dataflash); void handle_log_request_data(mavlink_message_t *msg, DataFlash_Class &dataflash); + void handle_log_request_erase(mavlink_message_t *msg, DataFlash_Class &dataflash); + void handle_log_request_end(mavlink_message_t *msg, DataFlash_Class &dataflash); void handle_log_message(mavlink_message_t *msg, DataFlash_Class &dataflash); void handle_log_send(DataFlash_Class &dataflash); void handle_log_send_listing(DataFlash_Class &dataflash); diff --git a/libraries/GCS_MAVLink/GCS_Logs.cpp b/libraries/GCS_MAVLink/GCS_Logs.cpp index b32b499c52..2ab57726f1 100644 --- a/libraries/GCS_MAVLink/GCS_Logs.cpp +++ b/libraries/GCS_MAVLink/GCS_Logs.cpp @@ -38,13 +38,12 @@ void GCS_MAVLINK::handle_log_message(mavlink_message_t *msg, DataFlash_Class &da handle_log_request_data(msg, dataflash); break; case MAVLINK_MSG_ID_LOG_ERASE: - dataflash.EraseAll(); + handle_log_request_erase(msg, dataflash); break; case MAVLINK_MSG_ID_LOG_REQUEST_END: - _log_sending = false; + handle_log_request_end(msg, dataflash); break; } - } @@ -126,6 +125,31 @@ void GCS_MAVLINK::handle_log_request_data(mavlink_message_t *msg, DataFlash_Clas handle_log_send(dataflash); } +/** + handle request to erase log data + */ +void GCS_MAVLINK::handle_log_request_erase(mavlink_message_t *msg, DataFlash_Class &dataflash) +{ + mavlink_log_request_data_t packet; + mavlink_msg_log_request_data_decode(msg, &packet); + if (mavlink_check_target(packet.target_system, packet.target_component)) + return; + + dataflash.EraseAll(); +} + +/** + handle request to stop transfer and resume normal logging + */ +void GCS_MAVLINK::handle_log_request_end(mavlink_message_t *msg, DataFlash_Class &dataflash) +{ + mavlink_log_request_data_t packet; + mavlink_msg_log_request_data_decode(msg, &packet); + if (mavlink_check_target(packet.target_system, packet.target_component)) + return; + _log_sending = false; +} + /** trigger sending of log messages if there are some pending */