Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 23:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: use common deferred message handling

Browse Source
This commit is contained in:
Andrew Tridgell 2014-03-19 11:07:15 +11:00 committed by Randy Mackay
parent 4031f7d368
commit 8da72fccba
1 changed files with 5 additions and 92 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

97
APMrover2/GCS_Mavlink.pde
View File

@ -108,7 +108,7 @@ static NOINLINE void send_attitude(mavlink_channel_t chan)
omega.z);
}
static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops)
static NOINLINE void send_extended_status1(mavlink_channel_t chan)
{
uint32_t control_sensors_present;
uint32_t control_sensors_enabled;
@ -517,7 +517,7 @@ static bool telemetry_delayed(mavlink_channel_t chan)
// try to send a message, return false if it won't fit in the serial tx buffer
static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops)
bool GCS_MAVLINK::try_send_message(enum ap_message id)
{
int16_t payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
@ -542,7 +542,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
case MSG_EXTENDED_STATUS1:
CHECK_PAYLOAD_SIZE(SYS_STATUS);
send_extended_status1(chan, packet_drops);
send_extended_status1(chan);
CHECK_PAYLOAD_SIZE(POWER_STATUS);
gcs[chan-MAVLINK_COMM_0].send_power_status();
break;
@ -666,84 +666,6 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
return true;
}
#define MAX_DEFERRED_MESSAGES MSG_RETRY_DEFERRED
static struct mavlink_queue {
enum ap_message deferred_messages[MAX_DEFERRED_MESSAGES];
uint8_t next_deferred_message;
uint8_t num_deferred_messages;
} mavlink_queue[MAVLINK_COMM_NUM_BUFFERS];
// send a message using mavlink
static void mavlink_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops)
{
uint8_t i, nextid;
struct mavlink_queue *q = &mavlink_queue[(uint8_t)chan];
// see if we can send the deferred messages, if any
while (q->num_deferred_messages != 0) {
if (!mavlink_try_send_message(chan,
q->deferred_messages[q->next_deferred_message],
packet_drops)) {
break;
}
q->next_deferred_message++;
if (q->next_deferred_message == MAX_DEFERRED_MESSAGES) {
q->next_deferred_message = 0;
}
q->num_deferred_messages--;
}
if (id == MSG_RETRY_DEFERRED) {
return;
}
// this message id might already be deferred
for (i=0, nextid = q->next_deferred_message; i < q->num_deferred_messages; i++) {
if (q->deferred_messages[nextid] == id) {
// its already deferred, discard
return;
}
nextid++;
if (nextid == MAX_DEFERRED_MESSAGES) {
nextid = 0;
}
}
if (q->num_deferred_messages != 0 ||
!mavlink_try_send_message(chan, id, packet_drops)) {
// can't send it now, so defer it
if (q->num_deferred_messages == MAX_DEFERRED_MESSAGES) {
// the defer buffer is full, discard
return;
}
nextid = q->next_deferred_message + q->num_deferred_messages;
if (nextid >= MAX_DEFERRED_MESSAGES) {
nextid -= MAX_DEFERRED_MESSAGES;
}
q->deferred_messages[nextid] = id;
q->num_deferred_messages++;
}
}
void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char *str)
{
if (telemetry_delayed(chan)) {
return;
}
if (severity == SEVERITY_LOW) {
// send via the deferred queuing system
mavlink_statustext_t *s = &gcs[chan-MAVLINK_COMM_0].pending_status;
s->severity = (uint8_t)severity;
strncpy((char *)s->text, str, sizeof(s->text));
mavlink_send_message(chan, MSG_STATUSTEXT, 0);
} else {
// send immediately
mavlink_msg_statustext_send(chan, severity, str);
}
}
/*
default stream rates to 1Hz
*/
@ -873,9 +795,6 @@ GCS_MAVLINK::update(void)
}
}
// Update packet drops counter
packet_drops += status.packet_rx_drop_count;
if (!waypoint_receiving) {
return;
}
@ -1026,12 +945,6 @@ GCS_MAVLINK::data_stream_send(void)
void
GCS_MAVLINK::send_message(enum ap_message id)
{
mavlink_send_message(chan,id, packet_drops);
}
void GCS_MAVLINK::handle_guided_request(AP_Mission::Mission_Command &cmd)
{
guided_WP = cmd.content.location;
@ -1516,11 +1429,11 @@ void gcs_send_text_fmt(const prog_char_t *fmt, ...)
#if LOGGING_ENABLED == ENABLED
DataFlash.Log_Write_Message(gcs[0].pending_status.text);
#endif
mavlink_send_message(MAVLINK_COMM_0, MSG_STATUSTEXT, 0);
gcs[0].send_message(MSG_STATUSTEXT);
for (uint8_t i=1; i<num_gcs; i++) {
if (gcs[i].initialised) {
gcs[i].pending_status = gcs[0].pending_status;
mavlink_send_message((mavlink_channel_t)i, MSG_STATUSTEXT, 0);
gcs[i].send_message(MSG_STATUSTEXT);
}
}
}