From c4dd556f3d49868ae29680d0b2387deeedc9dae5 Mon Sep 17 00:00:00 2001
From: Kemal Hadimli <kemal@hadimli.com>
Date: Tue, 29 Oct 2013 19:35:13 +0200
Subject: [PATCH] gps passthrough cli command

---
 src/cli.c | 11 +++++++++++
 src/gps.c | 26 ++++++++++++++++++++++++++
 src/mw.h  |  1 +
 3 files changed, 38 insertions(+)

diff --git a/src/cli.c b/src/cli.c
index 0d38a190d1..52ac928037 100644
--- a/src/cli.c
+++ b/src/cli.c
@@ -9,6 +9,7 @@ static void cliDefaults(char *cmdline);
 static void cliDump(char *cmdLine);
 static void cliExit(char *cmdline);
 static void cliFeature(char *cmdline);
+static void cliGpsPassthrough(char *cmdline);
 static void cliHelp(char *cmdline);
 static void cliMap(char *cmdline);
 static void cliMixer(char *cmdline);
@@ -73,6 +74,7 @@ const clicmd_t cmdTable[] = {
     { "dump", "print configurable settings in a pastable form", cliDump },
     { "exit", "", cliExit },
     { "feature", "list or -val or val", cliFeature },
+    { "gpspassthrough", "passthrough gps to serial", cliGpsPassthrough },
     { "help", "", cliHelp },
     { "map", "mapping of rc channel order", cliMap },
     { "mixer", "mixer name or list", cliMixer },
@@ -657,6 +659,15 @@ static void cliFeature(char *cmdline)
     }
 }
 
+static void cliGpsPassthrough(char *cmdline)
+{
+    cliPrint("Enabling GPS passthrough...");
+
+    if (gpsSetPassthrough() == -1) {
+        cliPrint("Error: Enable and plug in GPS first\r\n");
+    }
+}
+
 static void cliHelp(char *cmdline)
 {
     uint32_t i = 0;
diff --git a/src/gps.c b/src/gps.c
index 51f265e5c5..9c44be1922 100644
--- a/src/gps.c
+++ b/src/gps.c
@@ -521,6 +521,32 @@ void gpsSetPIDs(void)
     navPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
 }
 
+int8_t gpsSetPassthrough(void)
+{
+    if (gpsData.state != GPS_RECEIVINGDATA)
+        return -1;
+
+// get rid of callback
+    core.gpsport->callback = NULL;
+
+    LED0_OFF;
+    LED1_OFF;
+
+    while(1) {
+        if (serialTotalBytesWaiting(core.gpsport)) {
+            LED0_ON;
+            serialWrite(core.mainport, serialRead(core.gpsport));
+            LED0_OFF;
+            }
+        if (serialTotalBytesWaiting(core.mainport)) {
+            LED1_ON;
+            serialWrite(core.gpsport, serialRead(core.mainport));
+            LED1_OFF;
+            }
+        }
+}
+
+
 // OK here is the onboard GPS code
 
 ////////////////////////////////////////////////////////////////////////////////////
diff --git a/src/mw.h b/src/mw.h
index 414b2e9fc1..df32c7185a 100755
--- a/src/mw.h
+++ b/src/mw.h
@@ -456,6 +456,7 @@ void cliProcess(void);
 void gpsInit(uint8_t baudrate);
 void gpsThread(void);
 void gpsSetPIDs(void);
+int8_t gpsSetPassthrough(void);
 void GPS_reset_home_position(void);
 void GPS_reset_nav(void);
 void GPS_set_next_wp(int32_t* lat, int32_t* lon);