From b11b83e2cfcd68e9d48cf40d589bee747d1f8468 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 4 Dec 2011 08:48:59 +1100 Subject: [PATCH 1/2] GPS: added FAKE_GPS_LOCK_TIME this allows you to fake up a GPS lock after a specific number of seconds --- libraries/AP_GPS/AP_GPS_MTK16.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/libraries/AP_GPS/AP_GPS_MTK16.cpp b/libraries/AP_GPS/AP_GPS_MTK16.cpp index 96cc189b9c..659f60f71b 100644 --- a/libraries/AP_GPS/AP_GPS_MTK16.cpp +++ b/libraries/AP_GPS/AP_GPS_MTK16.cpp @@ -13,6 +13,7 @@ #include "AP_GPS_MTK16.h" #include +#include // Constructors //////////////////////////////////////////////////////////////// AP_GPS_MTK16::AP_GPS_MTK16(Stream *s) : GPS(s) @@ -141,6 +142,15 @@ restart: parsed = true; +#ifdef FAKE_GPS_LOCK_TIME + if (millis() > FAKE_GPS_LOCK_TIME*1000) { + fix = true; + latitude = -35000000UL; + longitude = 149000000UL; + altitude = 584; + } +#endif + /* Waiting on clarification of MAVLink protocol! if(!_offset_calculated && parsed) { long tempd1 = date; From c1569e49a869a11f7c15629f292fbcade3a8689b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 4 Dec 2011 09:06:08 +1100 Subject: [PATCH 2/2] 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); + } }