|
|
|
@ -40,12 +40,12 @@ bool AP_Logger::should_handle_log_message()
@@ -40,12 +40,12 @@ bool AP_Logger::should_handle_log_message()
|
|
|
|
|
/**
|
|
|
|
|
handle all types of log download requests from the GCS |
|
|
|
|
*/ |
|
|
|
|
void AP_Logger::handle_log_message(GCS_MAVLINK &link, mavlink_message_t *msg) |
|
|
|
|
void AP_Logger::handle_log_message(GCS_MAVLINK &link, const mavlink_message_t &msg) |
|
|
|
|
{ |
|
|
|
|
if (!should_handle_log_message()) { |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
switch (msg->msgid) { |
|
|
|
|
switch (msg.msgid) { |
|
|
|
|
case MAVLINK_MSG_ID_LOG_REQUEST_LIST: |
|
|
|
|
handle_log_request_list(link, msg); |
|
|
|
|
break; |
|
|
|
@ -64,7 +64,7 @@ void AP_Logger::handle_log_message(GCS_MAVLINK &link, mavlink_message_t *msg)
@@ -64,7 +64,7 @@ void AP_Logger::handle_log_message(GCS_MAVLINK &link, mavlink_message_t *msg)
|
|
|
|
|
/**
|
|
|
|
|
handle all types of log download requests from the GCS |
|
|
|
|
*/ |
|
|
|
|
void AP_Logger::handle_log_request_list(GCS_MAVLINK &link, mavlink_message_t *msg) |
|
|
|
|
void AP_Logger::handle_log_request_list(GCS_MAVLINK &link, const mavlink_message_t &msg) |
|
|
|
|
{ |
|
|
|
|
WITH_SEMAPHORE(_log_send_sem); |
|
|
|
|
|
|
|
|
@ -74,7 +74,7 @@ void AP_Logger::handle_log_request_list(GCS_MAVLINK &link, mavlink_message_t *ms
@@ -74,7 +74,7 @@ void AP_Logger::handle_log_request_list(GCS_MAVLINK &link, mavlink_message_t *ms
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
mavlink_log_request_list_t packet; |
|
|
|
|
mavlink_msg_log_request_list_decode(msg, &packet); |
|
|
|
|
mavlink_msg_log_request_list_decode(&msg, &packet); |
|
|
|
|
|
|
|
|
|
_log_num_logs = get_num_logs(); |
|
|
|
|
if (_log_num_logs == 0) { |
|
|
|
@ -102,7 +102,7 @@ void AP_Logger::handle_log_request_list(GCS_MAVLINK &link, mavlink_message_t *ms
@@ -102,7 +102,7 @@ void AP_Logger::handle_log_request_list(GCS_MAVLINK &link, mavlink_message_t *ms
|
|
|
|
|
/**
|
|
|
|
|
handle request for log data |
|
|
|
|
*/ |
|
|
|
|
void AP_Logger::handle_log_request_data(GCS_MAVLINK &link, mavlink_message_t *msg) |
|
|
|
|
void AP_Logger::handle_log_request_data(GCS_MAVLINK &link, const mavlink_message_t &msg) |
|
|
|
|
{ |
|
|
|
|
WITH_SEMAPHORE(_log_send_sem); |
|
|
|
|
|
|
|
|
@ -118,7 +118,7 @@ void AP_Logger::handle_log_request_data(GCS_MAVLINK &link, mavlink_message_t *ms
@@ -118,7 +118,7 @@ void AP_Logger::handle_log_request_data(GCS_MAVLINK &link, mavlink_message_t *ms
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
mavlink_log_request_data_t packet; |
|
|
|
|
mavlink_msg_log_request_data_decode(msg, &packet); |
|
|
|
|
mavlink_msg_log_request_data_decode(&msg, &packet); |
|
|
|
|
|
|
|
|
|
// consider opening or switching logs:
|
|
|
|
|
if (transfer_activity != SENDING || _log_num_data != packet.id) { |
|
|
|
@ -158,10 +158,10 @@ void AP_Logger::handle_log_request_data(GCS_MAVLINK &link, mavlink_message_t *ms
@@ -158,10 +158,10 @@ void AP_Logger::handle_log_request_data(GCS_MAVLINK &link, mavlink_message_t *ms
|
|
|
|
|
/**
|
|
|
|
|
handle request to erase log data |
|
|
|
|
*/ |
|
|
|
|
void AP_Logger::handle_log_request_erase(GCS_MAVLINK &link, mavlink_message_t *msg) |
|
|
|
|
void AP_Logger::handle_log_request_erase(GCS_MAVLINK &link, const mavlink_message_t &msg) |
|
|
|
|
{ |
|
|
|
|
// mavlink_log_erase_t packet;
|
|
|
|
|
// mavlink_msg_log_erase_decode(msg, &packet);
|
|
|
|
|
// mavlink_msg_log_erase_decode(&msg, &packet);
|
|
|
|
|
|
|
|
|
|
EraseAll(); |
|
|
|
|
} |
|
|
|
@ -169,11 +169,11 @@ void AP_Logger::handle_log_request_erase(GCS_MAVLINK &link, mavlink_message_t *m
@@ -169,11 +169,11 @@ void AP_Logger::handle_log_request_erase(GCS_MAVLINK &link, mavlink_message_t *m
|
|
|
|
|
/**
|
|
|
|
|
handle request to stop transfer and resume normal logging |
|
|
|
|
*/ |
|
|
|
|
void AP_Logger::handle_log_request_end(GCS_MAVLINK &link, mavlink_message_t *msg) |
|
|
|
|
void AP_Logger::handle_log_request_end(GCS_MAVLINK &link, const mavlink_message_t &msg) |
|
|
|
|
{ |
|
|
|
|
WITH_SEMAPHORE(_log_send_sem); |
|
|
|
|
mavlink_log_request_end_t packet; |
|
|
|
|
mavlink_msg_log_request_end_decode(msg, &packet); |
|
|
|
|
mavlink_msg_log_request_end_decode(&msg, &packet); |
|
|
|
|
|
|
|
|
|
transfer_activity = IDLE; |
|
|
|
|
_log_sending_link = nullptr; |
|
|
|
|