From 67ed5b73ef3102e039723098c7e0421513437512 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sat, 5 Apr 2014 07:33:34 +1100
Subject: [PATCH] AP_GPS: added lock_port() interface

used by SERIAL_CONTROL message
---
 libraries/AP_GPS/AP_GPS.cpp | 23 +++++++++++++++++++++++
 libraries/AP_GPS/AP_GPS.h   |  6 ++++++
 2 files changed, 29 insertions(+)

diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp
index 28c0c4aa56..28a223122a 100644
--- a/libraries/AP_GPS/AP_GPS.cpp
+++ b/libraries/AP_GPS/AP_GPS.cpp
@@ -207,6 +207,11 @@ AP_GPS::update_instance(uint8_t instance)
         state[instance].status = NO_GPS;
         return;
     }
+    if (locked_ports & (1U<<instance)) {
+        // the port is locked by another driver
+        return;
+    }
+
     if (drivers[instance] == NULL || state[instance].status == NO_GPS) {
         // we don't yet know the GPS type of this one, or it has timed
         // out and needs to be re-initialised
@@ -311,3 +316,21 @@ AP_GPS::setHIL(uint8_t instance, GPS_Status _status, uint64_t time_epoch_ms,
     timing[instance].last_fix_time_ms = tnow;
     _type[instance].set(GPS_TYPE_HIL);
 }
+
+/**
+   Lock a GPS port, prevening the GPS driver from using it. This can
+   be used to allow a user to control a GPS port via the
+   SERIAL_CONTROL protocol
+ */
+void 
+AP_GPS::lock_port(uint8_t instance, bool lock)
+{
+    if (instance >= GPS_MAX_INSTANCES) {
+        return;
+    }
+    if (lock) {
+        locked_ports |= (1U<<instance);
+    } else {
+        locked_ports &= ~(1U<<instance);
+    }
+}
diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h
index 2f296235bc..839813da46 100644
--- a/libraries/AP_GPS/AP_GPS.h
+++ b/libraries/AP_GPS/AP_GPS.h
@@ -258,6 +258,9 @@ public:
     void send_blob_start(uint8_t instance, const prog_char *_blob, uint16_t size);
     void send_blob_update(uint8_t instance);
 
+    // lock out a GPS port, allowing another application to use the port
+    void lock_port(uint8_t instance, bool locked);
+
 private:
     struct GPS_timing {
         // the time we got our last fix in system milliseconds
@@ -276,6 +279,9 @@ private:
     /// number of GPS instances present
     uint8_t num_instances:2;
 
+    // which ports are locked
+    uint8_t locked_ports:2;
+
     // state of auto-detection process, per instance
     struct detect_state {
         uint32_t detect_started_ms;