From ff37274e64e0413c2282bf8b1e9ef09a8cac1423 Mon Sep 17 00:00:00 2001
From: Damjan Adamic <projectkk2glider@gmail.com>
Date: Tue, 30 Aug 2016 21:23:20 +0200
Subject: [PATCH] Horus internal GPS: cmake changes, more CLI options, ability
 to send commands to the module

---
 radio/src/cli.cpp                      | 10 +++++++++-
 radio/src/gps.cpp                      | 25 ++++++++++++++++++++++++-
 radio/src/gps.h                        |  4 ++++
 radio/src/main_arm.cpp                 |  2 +-
 radio/src/targets/horus/CMakeLists.txt | 14 +++++++++++---
 radio/src/targets/horus/board.h        |  9 ++++-----
 radio/src/targets/horus/gps_driver.cpp | 24 ++++++++++++++++++++++--
 7 files changed, 75 insertions(+), 13 deletions(-)

diff --git a/radio/src/cli.cpp b/radio/src/cli.cpp
index eb238df5f..7f8972756 100644
--- a/radio/src/cli.cpp
+++ b/radio/src/cli.cpp
@@ -712,7 +712,15 @@ int cliShowJitter(const char ** argv)
 int cliGps(const char ** argv)
 {
   int baudrate = 0;
-  if (toInt(argv, 1, &baudrate) > 0 && baudrate > 0) {
+  if (argv[1][0] == '$') {
+    // send command to GPS
+    gpsSendFrame(argv[1]);
+  }
+  else if (!strcmp(argv[1], "trace")) {
+    gpsTraceEnabled = !gpsTraceEnabled;
+  }
+
+  else if (toInt(argv, 1, &baudrate) > 0 && baudrate > 0) {
     gpsInit(baudrate);
     serialPrint("GPS baudrate set to %d", baudrate);
   }
diff --git a/radio/src/gps.cpp b/radio/src/gps.cpp
index 81221604c..372427b03 100644
--- a/radio/src/gps.cpp
+++ b/radio/src/gps.cpp
@@ -275,7 +275,30 @@ void gpsWakeup()
 {
   uint8_t byte;
   while (gpsGetByte(&byte)) {
-    TRACE_PING("%c", (char)byte);
     gpsNewData(byte);
   }
 }
+
+#if defined(DEBUG)
+char hex(uint8_t b) {
+  return b > 9 ? b + 'A' - 10 : b + '0';
+}
+
+void gpsSendFrame(const char * frame)
+{
+  // send given frame, add checksum and CRLF
+  uint8_t parity = 0;
+  while (*frame) {
+    if (*frame != '$') parity ^= *frame;
+    gpsSendByte(*frame);
+    ++frame;
+  }
+  gpsSendByte('*');
+  gpsSendByte(hex(parity >> 4));
+  gpsSendByte(hex(parity & 0x0F));
+  gpsSendByte('\r');
+  gpsSendByte('\n');
+  TRACE("parity %02x", parity);
+}
+
+#endif // #if defined(DEBUG)
diff --git a/radio/src/gps.h b/radio/src/gps.h
index 6b6919778..0c1b9d3ab 100644
--- a/radio/src/gps.h
+++ b/radio/src/gps.h
@@ -40,4 +40,8 @@ struct gpsdata_t
 extern gpsdata_t gpsData;
 void gpsWakeup();
 
+#if defined(DEBUG)
+void gpsSendFrame(const char * frame);
+#endif
+
 #endif // _GPS_H_
diff --git a/radio/src/main_arm.cpp b/radio/src/main_arm.cpp
index 28807575e..1c68c078b 100644
--- a/radio/src/main_arm.cpp
+++ b/radio/src/main_arm.cpp
@@ -464,7 +464,7 @@ void perMain()
   bluetoothWakeup();
 #endif
 
-#if INTERNAL_GPS > 0
+#if defined(INTERNAL_GPS)
   gpsWakeup();
 #endif
 }
diff --git a/radio/src/targets/horus/CMakeLists.txt b/radio/src/targets/horus/CMakeLists.txt
index 63f95c013..19cb18ac2 100644
--- a/radio/src/targets/horus/CMakeLists.txt
+++ b/radio/src/targets/horus/CMakeLists.txt
@@ -1,7 +1,14 @@
 set(PCBREV "13" CACHE STRING "PCB Revision")
 set(INTERNAL_GPS_BAUDRATE "9600" CACHE STRING "Baud rate for internal GPS")
-option(INTERNAL_GPS_INSTALLED "Internal GPS installed in beta hardware version" NO)
 option(DISK_CACHE "Enable SD card disk cache" YES)
+if(${PCBREV} GREATER 10)
+  option(INTERNAL_GPS "Internal GPS installed" YES)
+else()
+  option(INTERNAL_GPS "Internal GPS installed" NO)
+  if(NOT INTERNAL_GPS)
+    message("Internal GPS is optional, use INTERNAL_GPS=YES option to enable it")
+  endif()
+endif()
 
 set(CPU_TYPE STM32F4)
 set(HSE_VALUE 12000000)
@@ -52,8 +59,9 @@ if(DISK_CACHE)
   set(SRC ${SRC} disk_cache.cpp)
   add_definitions(-DDISK_CACHE)
 endif()
-if(INTERNAL_GPS_INSTALLED)
-  add_definitions(-DINTERNAL_GPS_INSTALLED)
+if(INTERNAL_GPS)
+  add_definitions(-DINTERNAL_GPS)
+  message("Internal GPS enabled")
 endif()
 set(SERIAL2_DRIVER serial2_driver.cpp)
 set(TARGET_SRC
diff --git a/radio/src/targets/horus/board.h b/radio/src/targets/horus/board.h
index 6adbec507..66b8ed5fe 100644
--- a/radio/src/targets/horus/board.h
+++ b/radio/src/targets/horus/board.h
@@ -389,13 +389,12 @@ void hapticOff(void);
 void hapticOn(uint32_t pwmPercent);
 
 // GPS driver
-#if PCBREV >= 13 || defined(INTERNAL_GPS_INSTALLED)
-#define INTERNAL_GPS                   1
-#else
-#define INTERNAL_GPS                   0
-#endif
 void gpsInit(uint32_t baudrate);
 uint8_t gpsGetByte(uint8_t * byte);
+#if defined(DEBUG)
+  extern uint8_t gpsTraceEnabled;
+  void gpsSendByte(uint8_t byte);
+#endif
 
 // Second serial port driver
 #define SERIAL2
diff --git a/radio/src/targets/horus/gps_driver.cpp b/radio/src/targets/horus/gps_driver.cpp
index 65ec3cfe0..c9805ee42 100644
--- a/radio/src/targets/horus/gps_driver.cpp
+++ b/radio/src/targets/horus/gps_driver.cpp
@@ -60,9 +60,23 @@ void gpsInit(uint32_t baudrate)
   NVIC_Init(&NVIC_InitStructure);
 }
 
+#if defined(DEBUG)
+uint8_t gpsTraceEnabled = false;
+Fifo<uint8_t, 64> gpsTxFifo;
+
+void gpsSendByte(uint8_t byte)
+{
+  while (gpsTxFifo.isFull());
+  gpsTxFifo.push(byte);
+  USART_ITConfig(GPS_USART, USART_IT_TXE, ENABLE);
+}
+
+#endif // #if defined(DEBUG)
+
+
 extern "C" void GPS_USART_IRQHandler(void)
 {
-#if 0
+#if defined(DEBUG)
   // Send
   if (USART_GetITStatus(GPS_USART, USART_IT_TXE) != RESET) {
     uint8_t txchar;
@@ -89,5 +103,11 @@ extern "C" void GPS_USART_IRQHandler(void)
 
 uint8_t gpsGetByte(uint8_t * byte)
 {
-  return gpsRxFifo.pop(*byte);
+  uint8_t result = gpsRxFifo.pop(*byte);
+#if defined(DEBUG)
+  if (gpsTraceEnabled) {
+    serialPutc(*byte);
+  }
+#endif
+  return result;
 }
\ No newline at end of file