diff --git a/src/cli.c b/src/cli.c index 831822d68..234d5b9b2 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); @@ -77,6 +78,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 }, @@ -665,6 +667,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 0faa04f49..e976f554f 100644 --- a/src/gps.c +++ b/src/gps.c @@ -537,6 +537,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 7f9d7d7e0..f9efeee58 100755 --- a/src/mw.h +++ b/src/mw.h @@ -458,6 +458,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);