diff --git a/ArduPlane/GCS.h b/ArduPlane/GCS.h index 4ba240d06a..637a70112c 100644 --- a/ArduPlane/GCS.h +++ b/ArduPlane/GCS.h @@ -133,6 +133,10 @@ public: // see if we should send a stream now. Called at 50Hz bool stream_trigger(enum streams stream_num); + // this costs us 51 bytes per instance, but means that low priority + // messages don't block the CPU + mavlink_statustext_t pending_status; + private: void handleMessage(mavlink_message_t * msg); diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 1cb1cd9adf..b1d90d20fe 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -3,10 +3,6 @@ // use this to prevent recursion during sensor init static bool in_mavlink_delay; -// this costs us 51 bytes, but means that low priority -// messages don't block the CPU -static mavlink_statustext_t pending_status; - // true when we have received at least 1 MAVLink packet static bool mavlink_active; @@ -456,10 +452,11 @@ static void NOINLINE send_current_waypoint(mavlink_channel_t chan) static void NOINLINE send_statustext(mavlink_channel_t chan) { + mavlink_statustext_t *s = (chan == MAVLINK_COMM_0?&gcs0.pending_status:&gcs3.pending_status); mavlink_msg_statustext_send( chan, - pending_status.severity, - pending_status.text); + s->severity, + s->text); } // are we still delaying telemetry to try to avoid Xbee bricking? @@ -705,8 +702,9 @@ void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char if (severity == SEVERITY_LOW) { // send via the deferred queuing system - pending_status.severity = (uint8_t)severity; - strncpy((char *)pending_status.text, str, sizeof(pending_status.text)); + mavlink_statustext_t *s = (chan == MAVLINK_COMM_0?&gcs0.pending_status:&gcs3.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 @@ -2116,10 +2114,11 @@ void gcs_send_text_fmt(const prog_char_t *fmt, ...) if (fmtstr[i] == 0) break; } fmtstr[i] = 0; - pending_status.severity = (uint8_t)SEVERITY_LOW; + gcs0.pending_status.severity = (uint8_t)SEVERITY_LOW; va_start(ap, fmt); - vsnprintf((char *)pending_status.text, sizeof(pending_status.text), fmtstr, ap); + vsnprintf((char *)gcs0.pending_status.text, sizeof(gcs0.pending_status.text), fmtstr, ap); va_end(ap); + gcs3.pending_status = gcs0.pending_status; mavlink_send_message(MAVLINK_COMM_0, MSG_STATUSTEXT, 0); if (gcs3.initialised) { mavlink_send_message(MAVLINK_COMM_1, MSG_STATUSTEXT, 0);