From 550523c3bd1e3f01f1d6ebeaa848a6638bc5eb07 Mon Sep 17 00:00:00 2001
From: MUSTARDTIGER FPV <122312693+MUSTARDTIGERFPV@users.noreply.github.com>
Date: Mon, 13 Jan 2025 15:43:32 -0800
Subject: [PATCH] [Telemetry] Support directly-attached GPS inputs to RX
(#3086)
* First draft of NMEA GPS support for RXes
* Change to integer math
* Add GPS as an option on Serial0
* Minor formatting change
---
src/html/index.html | 2 +
src/include/common.h | 2 +
src/lib/LUA/rx_devLUA.cpp | 4 +-
src/src/rx-serial/SerialGPS.cpp | 160 ++++++++++++++++++++++++++++++++++++++++
src/src/rx-serial/SerialGPS.h | 32 ++++++++
src/src/rx_main.cpp | 13 ++++
6 files changed, 211 insertions(+), 2 deletions(-)
create mode 100644 src/src/rx-serial/SerialGPS.cpp
create mode 100644 src/src/rx-serial/SerialGPS.h
diff --git a/src/html/index.html b/src/html/index.html
index fc421297..ed187a6d 100644
--- a/src/html/index.html
+++ b/src/html/index.html
@@ -259,6 +259,7 @@
+
@@ -277,6 +278,7 @@
+
diff --git a/src/include/common.h b/src/include/common.h
index 775b8d9c..d3b73c69 100644
--- a/src/include/common.h
+++ b/src/include/common.h
@@ -240,6 +240,7 @@ enum eSerialProtocol : uint8_t
PROTOCOL_HOTT_TLM,
PROTOCOL_MAVLINK,
PROTOCOL_MSP_DISPLAYPORT,
+ PROTOCOL_GPS
};
#if defined(PLATFORM_ESP32)
@@ -256,6 +257,7 @@ enum eSerial1Protocol : uint8_t
PROTOCOL_SERIAL1_TRAMP,
PROTOCOL_SERIAL1_SMARTAUDIO,
PROTOCOL_SERIAL1_MSP_DISPLAYPORT,
+ PROTOCOL_SERIAL1_GPS
};
#endif
diff --git a/src/lib/LUA/rx_devLUA.cpp b/src/lib/LUA/rx_devLUA.cpp
index 91e3ba93..9ef5309a 100644
--- a/src/lib/LUA/rx_devLUA.cpp
+++ b/src/lib/LUA/rx_devLUA.cpp
@@ -19,7 +19,7 @@ static char pwmModes[] = "50Hz;60Hz;100Hz;160Hz;333Hz;400Hz;10kHzDuty;On/Off;DSh
static struct luaItem_selection luaSerialProtocol = {
{"Protocol", CRSF_TEXT_SELECTION},
0, // value
- "CRSF;Inverted CRSF;SBUS;Inverted SBUS;SUMD;DJI RS Pro;HoTT Telemetry;MAVLink;DisplayPort",
+ "CRSF;Inverted CRSF;SBUS;Inverted SBUS;SUMD;DJI RS Pro;HoTT Telemetry;MAVLink;DisplayPort;GPS",
STR_EMPTYSPACE
};
@@ -27,7 +27,7 @@ static struct luaItem_selection luaSerialProtocol = {
static struct luaItem_selection luaSerial1Protocol = {
{"Protocol2", CRSF_TEXT_SELECTION},
0, // value
- "Off;CRSF;Inverted CRSF;SBUS;Inverted SBUS;SUMD;DJI RS Pro;HoTT Telemetry;Tramp;SmartAudio;DisplayPort",
+ "Off;CRSF;Inverted CRSF;SBUS;Inverted SBUS;SUMD;DJI RS Pro;HoTT Telemetry;Tramp;SmartAudio;DisplayPort;GPS",
STR_EMPTYSPACE
};
#endif
diff --git a/src/src/rx-serial/SerialGPS.cpp b/src/src/rx-serial/SerialGPS.cpp
new file mode 100644
index 00000000..f260b0b8
--- /dev/null
+++ b/src/src/rx-serial/SerialGPS.cpp
@@ -0,0 +1,160 @@
+#include "SerialGPS.h"
+#include "msptypes.h"
+#include
+#include
+
+extern Telemetry telemetry;
+
+void SerialGPS::sendQueuedData(uint32_t maxBytesToSend)
+{
+ sendTelemetryFrame();
+}
+
+void SerialGPS::queueMSPFrameTransmission(uint8_t* data)
+{
+}
+
+// Parses a decimal string with optional decimal point and returns the value scaled by the given factor as an integer
+// Ex: "0.442" with scale 100 returns 44
+// Ex: "123.456" with scale 1000 returns 123456
+int32_t parseDecimalToScaled(const char* str, int32_t scale) {
+ char *end;
+ int32_t whole = strtol(str, &end, 10);
+ int32_t result = whole * scale;
+
+ if (*end == '.') {
+ const char* dec = end + 1;
+ int32_t divisor = 1;
+ int32_t decimalPart = 0;
+
+ // Count decimal places in scale
+ int32_t scaleDecimals = 0;
+ int32_t tempScale = scale;
+ while (tempScale > 1) {
+ scaleDecimals++;
+ tempScale /= 10;
+ }
+
+ // Process up to scaleDecimals digits
+ for (int i = 0; i < scaleDecimals && dec[i]; i++) {
+ decimalPart = decimalPart * 10 + (dec[i] - '0');
+ divisor *= 10;
+ }
+
+ // Scale the decimal part
+ if (divisor > 1) {
+ while (divisor < scale) {
+ decimalPart *= 10;
+ divisor *= 10;
+ }
+ result += decimalPart;
+ }
+ }
+ return result;
+}
+
+void SerialGPS::processSentence(uint8_t *sentence, uint8_t size)
+{
+ if (size < 6) {
+ return;
+ }
+
+ if (sentence[3] == 'G' && sentence[4] == 'G' && sentence[5] == 'A') {
+ char *ptr = (char*)sentence;
+ ptr = strchr(ptr, ',') + 1;
+ ptr = strchr(ptr, ',') + 1;
+
+ // Parse lat
+ if (ptr != NULL) {
+ int32_t degrees = atoi(ptr) / 100;
+ char minutes[20];
+ strncpy(minutes, ptr + 2, 19);
+ int32_t minutesPart = parseDecimalToScaled(minutes, 10000000) / 60;
+
+ gpsData.lat = degrees * 10000000 + minutesPart;
+ }
+ ptr = strchr(ptr, ',') + 1;
+
+ if (ptr != NULL && *ptr == 'S') {
+ gpsData.lat = -gpsData.lat;
+ }
+ ptr = strchr(ptr, ',') + 1;
+
+ // Parse lon - similar to lat
+ if (ptr != NULL) {
+ int32_t degrees = atoi(ptr) / 100;
+ char minutes[20];
+ strncpy(minutes, ptr + 2, 19);
+ int32_t minutesPart = parseDecimalToScaled(minutes, 10000000) / 60;
+
+ gpsData.lon = degrees * 10000000 + minutesPart;
+ }
+ ptr = strchr(ptr, ',') + 1;
+
+ if (ptr != NULL && *ptr == 'W') {
+ gpsData.lon = -gpsData.lon;
+ }
+ ptr = strchr(ptr, ',') + 1;
+
+ ptr = strchr(ptr, ',') + 1;
+
+ if (ptr != NULL) {
+ gpsData.satellites = atoi(ptr);
+ }
+ ptr = strchr(ptr, ',') + 1;
+ ptr = strchr(ptr, ',') + 1;
+
+ // Parse altitude into centimeters
+ if (ptr != NULL) {
+ gpsData.alt = parseDecimalToScaled(ptr, 100);
+ }
+ }
+ else if (sentence[3] == 'V' && sentence[4] == 'T' && sentence[5] == 'G') {
+ char *ptr = (char*)sentence;
+ ptr = strchr(ptr, ',') + 1;
+
+ // Parse heading (into degrees * 100)
+ if (ptr != NULL && *ptr != ',') {
+ gpsData.heading = parseDecimalToScaled(ptr, 100);
+ }
+
+ // Skip to speed
+ for (int i = 0; i < 6; i++) {
+ ptr = strchr(ptr, ',') + 1;
+ }
+
+ // Parse speed (into km/h * 100)
+ if (ptr != NULL && *ptr != ',') {
+ gpsData.speed = parseDecimalToScaled(ptr, 100);
+ }
+ }
+}
+
+void SerialGPS::processBytes(uint8_t *bytes, uint16_t size)
+{
+ static uint8_t nmeaBuffer[128];
+ static uint8_t nmeaBufferIndex = 0;
+
+ for (uint16_t i = 0; i < size; i++) {
+ if (nmeaBufferIndex < sizeof(nmeaBuffer)) {
+ nmeaBuffer[nmeaBufferIndex++] = bytes[i];
+ }
+ if (bytes[i] == '\n') {
+ processSentence(nmeaBuffer, nmeaBufferIndex);
+ nmeaBufferIndex = 0;
+ }
+ }
+}
+
+void SerialGPS::sendTelemetryFrame()
+{
+ CRSF_MK_FRAME_T(crsf_sensor_gps_t) crsfgps = { 0 };
+ crsfgps.p.latitude = htobe32(gpsData.lat);
+ crsfgps.p.longitude = htobe32(gpsData.lon);
+ crsfgps.p.altitude = htobe16((int16_t)(gpsData.alt / 100 + 1000));
+ crsfgps.p.groundspeed = htobe16((uint16_t)(gpsData.speed / 10));
+ crsfgps.p.satellites_in_use = gpsData.satellites;
+ crsfgps.p.gps_heading = htobe16((uint16_t)gpsData.heading);
+ CRSF::SetHeaderAndCrc((uint8_t *)&crsfgps, CRSF_FRAMETYPE_GPS, CRSF_FRAME_SIZE(sizeof(crsf_sensor_gps_t)), CRSF_ADDRESS_CRSF_TRANSMITTER);
+ telemetry.AppendTelemetryPackage((uint8_t *)&crsfgps);
+}
\ No newline at end of file
diff --git a/src/src/rx-serial/SerialGPS.h b/src/src/rx-serial/SerialGPS.h
new file mode 100644
index 00000000..f6d9a9f6
--- /dev/null
+++ b/src/src/rx-serial/SerialGPS.h
@@ -0,0 +1,32 @@
+#include "SerialIO.h"
+
+typedef struct {
+ // Latitude in decimal degrees
+ uint32_t lat;
+ // Longitude in decimal degrees
+ uint32_t lon;
+ // Altitude in meters
+ uint32_t alt;
+ // Speed in km/h
+ uint32_t speed;
+ // Heading in degrees, positive. 0 is north.
+ uint32_t heading;
+ // Number of satellites
+ uint8_t satellites;
+} GpsData;
+
+class SerialGPS : public SerialIO {
+public:
+ explicit SerialGPS(Stream &out, Stream &in) : SerialIO(&out, &in) {}
+ virtual ~SerialGPS() {}
+
+ void queueLinkStatisticsPacket() override {}
+ void queueMSPFrameTransmission(uint8_t* data) override;
+ void sendQueuedData(uint32_t maxBytesToSend) override;
+ uint32_t sendRCFrame(bool frameAvailable, bool frameMissed, uint32_t *channelData) override { return DURATION_IMMEDIATELY; }
+private:
+ void processBytes(uint8_t *bytes, uint16_t size) override;
+ void sendTelemetryFrame();
+ void processSentence(uint8_t *sentence, uint8_t size);
+ GpsData gpsData = {0};
+};
diff --git a/src/src/rx_main.cpp b/src/src/rx_main.cpp
index 8d3f7650..4b92e748 100644
--- a/src/src/rx_main.cpp
+++ b/src/src/rx_main.cpp
@@ -27,6 +27,7 @@
#include "rx-serial/SerialTramp.h"
#include "rx-serial/SerialSmartAudio.h"
#include "rx-serial/SerialDisplayport.h"
+#include "rx-serial/SerialGPS.h"
#include "rx-serial/devSerialIO.h"
#include "devLED.h"
@@ -1432,6 +1433,10 @@ static void setupSerial()
hottTlmSerial = true;
serialBaud = 19200;
}
+ else if (config.GetSerialProtocol() == PROTOCOL_GPS)
+ {
+ serialBaud = 115200;
+ }
bool invert = config.GetSerialProtocol() == PROTOCOL_SBUS || config.GetSerialProtocol() == PROTOCOL_INVERTED_CRSF || config.GetSerialProtocol() == PROTOCOL_DJI_RS_PRO;
#if defined(PLATFORM_ESP8266)
@@ -1492,6 +1497,10 @@ static void setupSerial()
{
serialIO = new SerialDisplayport(SERIAL_PROTOCOL_TX, SERIAL_PROTOCOL_RX);
}
+ else if (config.GetSerialProtocol() == PROTOCOL_GPS)
+ {
+ serialIO = new SerialGPS(SERIAL_PROTOCOL_TX, SERIAL_PROTOCOL_RX);
+ }
else if (hottTlmSerial)
{
serialIO = new SerialHoTT_TLM(SERIAL_PROTOCOL_TX, SERIAL_PROTOCOL_RX);
@@ -1592,6 +1601,10 @@ static void setupSerial1()
Serial1.begin(115200, SERIAL_8N1, UNDEF_PIN, serial1TXpin, false);
serial1IO = new SerialDisplayport(SERIAL1_PROTOCOL_TX, SERIAL1_PROTOCOL_RX);
break;
+ case PROTOCOL_SERIAL1_GPS:
+ Serial1.begin(115200, SERIAL_8N1, serial1RXpin, serial1TXpin, false);
+ serial1IO = new SerialGPS(SERIAL1_PROTOCOL_TX, SERIAL1_PROTOCOL_RX);
+ break;
}
}
--
2.11.4.GIT