Browse Source

DataFlash: handle knowledge of in_log_download in DataFlash

master
Peter Barker 8 years ago committed by Francisco Ferreira
parent
commit
ad17709390
  1. 3
      libraries/DataFlash/DataFlash.cpp
  2. 5
      libraries/DataFlash/DataFlash.h
  3. 19
      libraries/DataFlash/DataFlash_MAVLinkLogTransfer.cpp

3
libraries/DataFlash/DataFlash.cpp

@ -312,6 +312,9 @@ bool DataFlash_Class::should_log() const @@ -312,6 +312,9 @@ bool DataFlash_Class::should_log() const
if (!vehicle_is_armed() && !log_while_disarmed()) {
return false;
}
if (in_log_download()) {
return false;
}
if (_next_backend == 0) {
return false;
}

5
libraries/DataFlash/DataFlash.h

@ -213,6 +213,7 @@ public: @@ -213,6 +213,7 @@ public:
bool vehicle_is_armed() const { return _armed; }
void handle_log_send(class GCS_MAVLINK &);
bool in_log_download() const { return _in_log_download; }
protected:
@ -294,6 +295,9 @@ private: @@ -294,6 +295,9 @@ private:
uint8_t _log_listing:1; // sending log list
uint8_t _log_sending:1; // sending log data
// bolean replicating old vehicle in_log_download flag:
bool _in_log_download:1;
// next log list entry to send
uint16_t _log_next_list_entry;
@ -318,6 +322,7 @@ private: @@ -318,6 +322,7 @@ private:
// start page of log data
uint16_t _log_data_page;
bool should_handle_log_message();
void handle_log_message(class GCS_MAVLINK &, mavlink_message_t *msg);
void handle_log_request_list(class GCS_MAVLINK &, mavlink_message_t *msg);

19
libraries/DataFlash/DataFlash_MAVLinkLogTransfer.cpp

@ -24,11 +24,27 @@ @@ -24,11 +24,27 @@
extern const AP_HAL::HAL& hal;
// We avoid doing log messages when timing is critical:
bool DataFlash_Class::should_handle_log_message()
{
if (!WritesEnabled()) {
// this is currently used as a proxy for "in_mavlink_delay"
return false;
}
if (vehicle_is_armed()) {
return false;
}
return true;
}
/**
handle all types of log download requests from the GCS
*/
void DataFlash_Class::handle_log_message(GCS_MAVLINK &link, mavlink_message_t *msg)
{
if (!should_handle_log_message()) {
return;
}
switch (msg->msgid) {
case MAVLINK_MSG_ID_LOG_REQUEST_LIST:
handle_log_request_list(link, msg);
@ -85,6 +101,8 @@ void DataFlash_Class::handle_log_request_data(GCS_MAVLINK &link, mavlink_message @@ -85,6 +101,8 @@ void DataFlash_Class::handle_log_request_data(GCS_MAVLINK &link, mavlink_message
mavlink_log_request_data_t packet;
mavlink_msg_log_request_data_decode(msg, &packet);
_in_log_download = true;
_log_listing = false;
if (!_log_sending || _log_num_data != packet.id) {
_log_sending = false;
@ -135,6 +153,7 @@ void DataFlash_Class::handle_log_request_end(GCS_MAVLINK &link, mavlink_message_ @@ -135,6 +153,7 @@ void DataFlash_Class::handle_log_request_end(GCS_MAVLINK &link, mavlink_message_
{
mavlink_log_request_end_t packet;
mavlink_msg_log_request_end_decode(msg, &packet);
_in_log_download = false;
_log_sending = false;
}

Loading…
Cancel
Save