From 39a36fe94668b2bd7af99ca275fab89d8887062f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 4 Dec 2011 09:06:08 +1100 Subject: [PATCH] fixed the "reboot on GPS lock" bug the problem was that with APM2 we may not have a 2nd GCS instance. The "gps alt: xxx" message was being unconditionally sent to both GCS instances, which caused a crash --- ArduCopter/GCS_Mavlink.pde | 4 +++- ArduPlane/GCS_Mavlink.pde | 4 +++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 14e8cf1ce3..1eef88ec3f 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1799,5 +1799,7 @@ static void gcs_send_text_fmt(const prog_char_t *fmt, ...) vsnprintf((char *)pending_status.text, sizeof(pending_status.text), fmtstr, ap); va_end(ap); mavlink_send_message(MAVLINK_COMM_0, MSG_STATUSTEXT, 0); - mavlink_send_message(MAVLINK_COMM_1, MSG_STATUSTEXT, 0); + if (gcs3.initialised) { + mavlink_send_message(MAVLINK_COMM_1, MSG_STATUSTEXT, 0); + } } diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index eee368cbf2..b2f5884cba 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -2103,5 +2103,7 @@ static void gcs_send_text_fmt(const prog_char_t *fmt, ...) vsnprintf((char *)pending_status.text, sizeof(pending_status.text), fmtstr, ap); va_end(ap); mavlink_send_message(MAVLINK_COMM_0, MSG_STATUSTEXT, 0); - mavlink_send_message(MAVLINK_COMM_1, MSG_STATUSTEXT, 0); + if (gcs3.initialised) { + mavlink_send_message(MAVLINK_COMM_1, MSG_STATUSTEXT, 0); + } }