GCS_MAVLink: refactor code into new retry_deferred method

This commit is contained in:
Peter Barker 2017-07-19 11:36:49 +10:00 committed by Francisco Ferreira
parent fdac13d3d3
commit 0c5f66dc59
2 changed files with 35 additions and 13 deletions

View File

@ -192,6 +192,9 @@ public:
// send queued parameters if needed // send queued parameters if needed
void send_queued_parameters(void); void send_queued_parameters(void);
// push send_message() messages and queued statustext messages etc:
void retry_deferred();
/* /*
send a MAVLink message to all components with this vehicle's system id send a MAVLink message to all components with this vehicle's system id
This is a no-op if no routes to components have been learned This is a no-op if no routes to components have been learned
@ -411,6 +414,8 @@ private:
virtual void handle_change_alt_request(AP_Mission::Mission_Command &cmd) = 0; virtual void handle_change_alt_request(AP_Mission::Mission_Command &cmd) = 0;
void handle_common_mission_message(mavlink_message_t *msg); void handle_common_mission_message(mavlink_message_t *msg);
void push_deferred_messages();
void lock_channel(mavlink_channel_t chan, bool lock); void lock_channel(mavlink_channel_t chan, bool lock);
mavlink_signing_t signing; mavlink_signing_t signing;
@ -458,6 +463,8 @@ public:
void send_message(enum ap_message id); void send_message(enum ap_message id);
void send_mission_item_reached_message(uint16_t mission_index); void send_mission_item_reached_message(uint16_t mission_index);
void send_home(const Location &home) const; void send_home(const Location &home) const;
// push send_message() messages and queued statustext messages etc:
void retry_deferred();
void data_stream_send(); void data_stream_send();
void update(); void update();
virtual void setup_uarts(AP_SerialManager &serial_manager); virtual void setup_uarts(AP_SerialManager &serial_manager);

View File

@ -755,16 +755,8 @@ mission_ack:
return mission_is_complete; return mission_is_complete;
} }
// send a message using mavlink, handling message queueing void GCS_MAVLINK::push_deferred_messages()
void GCS_MAVLINK::send_message(enum ap_message id)
{ {
uint8_t i, nextid;
if (id == MSG_HEARTBEAT) {
save_signing_timestamp(false);
}
// see if we can send the deferred messages, if any:
while (num_deferred_messages != 0) { while (num_deferred_messages != 0) {
if (!try_send_message(deferred_messages[next_deferred_message])) { if (!try_send_message(deferred_messages[next_deferred_message])) {
break; break;
@ -775,12 +767,25 @@ void GCS_MAVLINK::send_message(enum ap_message id)
} }
num_deferred_messages--; num_deferred_messages--;
} }
}
// MSG_RETRY_DEFERRED is a "marker" message only used to push the queue void GCS_MAVLINK::retry_deferred()
if (id == MSG_RETRY_DEFERRED) { {
return; push_deferred_messages();
}
// send a message using mavlink, handling message queueing
void GCS_MAVLINK::send_message(enum ap_message id)
{
uint8_t i, nextid;
if (id == MSG_HEARTBEAT) {
save_signing_timestamp(false);
} }
// see if we can send the deferred messages, if any:
push_deferred_messages();
// if there are no deferred messages, attempt to send straight away: // if there are no deferred messages, attempt to send straight away:
if (num_deferred_messages == 0) { if (num_deferred_messages == 0) {
if (try_send_message(id)) { if (try_send_message(id)) {
@ -1284,6 +1289,16 @@ void GCS::send_message(enum ap_message id)
} }
} }
void GCS::retry_deferred()
{
for (uint8_t i=0; i<num_gcs(); i++) {
if (chan(i).initialised) {
chan(i).retry_deferred();
}
}
service_statustext();
}
void GCS::data_stream_send() void GCS::data_stream_send()
{ {
for (uint8_t i=0; i<num_gcs(); i++) { for (uint8_t i=0; i<num_gcs(); i++) {