From 1180fcbe5fe480b524e15d4a5e80c2c6ed018018 Mon Sep 17 00:00:00 2001 From: Tony Cake Date: Wed, 10 Mar 2021 07:16:55 -0800 Subject: [PATCH] Add GHST GPS Telemetry Sensors (#8352) * Add GHST GPS Telemetry Sensors --- radio/src/telemetry/ghost.cpp | 66 ++++++++++++++++++++++++++++++++--- radio/src/telemetry/ghost.h | 2 ++ 2 files changed, 64 insertions(+), 4 deletions(-) diff --git a/radio/src/telemetry/ghost.cpp b/radio/src/telemetry/ghost.cpp index 098e9c990..1ff6acd48 100755 --- a/radio/src/telemetry/ghost.cpp +++ b/radio/src/telemetry/ghost.cpp @@ -49,6 +49,13 @@ enum GHOST_ID_PACK_VOLTS = 0x000c, // Battery Pack Voltage GHOST_ID_PACK_AMPS = 0x000d, // Battery Pack Current GHOST_ID_PACK_MAH = 0x000e, // Battery Pack mAh consumed + + GHOST_ID_GPS_LAT = 0x000f, // GPS Latitude + GHOST_ID_GPS_LONG = 0x0010, // GPS Longitude + GHOST_ID_GPS_ALT = 0x0011, // GPS Altitude + GHOST_ID_GPS_HDG = 0x0012, // GPS Heading + GHOST_ID_GPS_GSPD = 0x0013, // GPS Ground Speed + GHOST_ID_GPS_SATS = 0x0014 // GPS Satellite Count }; const GhostSensor ghostSensors[] = { @@ -70,6 +77,13 @@ const GhostSensor ghostSensors[] = { {GHOST_ID_PACK_AMPS, ZSTR_CURR, UNIT_AMPS, 2}, {GHOST_ID_PACK_MAH, ZSTR_CAPACITY, UNIT_MAH, 0}, + {GHOST_ID_GPS_LAT, ZSTR_GPS, UNIT_GPS_LATITUDE, 0}, + {GHOST_ID_GPS_LONG, ZSTR_GPS, UNIT_GPS_LONGITUDE, 0}, + {GHOST_ID_GPS_GSPD, ZSTR_GSPD, UNIT_KMH, 1}, + {GHOST_ID_GPS_HDG, ZSTR_HDG, UNIT_DEGREE, 3}, + {GHOST_ID_GPS_ALT, ZSTR_ALT, UNIT_METERS, 0}, + {GHOST_ID_GPS_SATS, ZSTR_SATELLITES, UNIT_RAW, 0}, + {0x00, NULL, UNIT_RAW, 0}, }; @@ -88,7 +102,11 @@ void processGhostTelemetryValue(uint8_t index, int32_t value) return; const GhostSensor * sensor = getGhostSensor(index); - setTelemetryValue(PROTOCOL_TELEMETRY_GHOST, sensor->id, 0, 0, value, sensor->unit, sensor->precision); + uint16_t id = sensor->id; + if (id == GHOST_ID_GPS_LONG) { + id = GHOST_ID_GPS_LAT; + } + setTelemetryValue(PROTOCOL_TELEMETRY_GHOST, id, 0, 0, value, sensor->unit, sensor->precision); } void processGhostTelemetryValueString(const GhostSensor * sensor, const char * str) @@ -105,16 +123,19 @@ bool checkGhostTelemetryFrameCRC() return (crc == telemetryRxBuffer[len + 1]); } +// hifirst uint16_t getTelemetryValue_u16(uint8_t index) { return (telemetryRxBuffer[index] << 8) | telemetryRxBuffer[index + 1]; } +// lofirst uint16_t getTelemetryValue_u16le(uint8_t index) { return (telemetryRxBuffer[index + 1] << 8) | telemetryRxBuffer[index]; } +// hifirst uint32_t getTelemetryValue_s32(uint8_t index) { uint32_t val = 0; @@ -123,6 +144,15 @@ uint32_t getTelemetryValue_s32(uint8_t index) return val; } +// lofirst +uint32_t getTelemetryValue_s32le(uint8_t index) +{ + uint32_t val = 0; + for (int i = 0; i < 4; ++i) + val <<= 8, val |= telemetryRxBuffer[index + 3 - i]; + return val; +} + void processGhostTelemetryFrame() { if (!checkGhostTelemetryFrameCRC()) { @@ -213,15 +243,40 @@ void processGhostTelemetryFrame() case GHST_DL_PACK_STAT: { #if defined(BLUETOOTH) if (g_eeGeneral.bluetoothMode == BLUETOOTH_TELEMETRY && bluetooth.state == BLUETOOTH_STATE_CONNECTED) { - bluetooth.write(telemetryRxBuffer, telemetryRxBufferCount); - } + bluetooth.write(telemetryRxBuffer, telemetryRxBufferCount); + } #endif processGhostTelemetryValue(GHOST_ID_PACK_VOLTS, getTelemetryValue_u16le(3)); processGhostTelemetryValue(GHOST_ID_PACK_AMPS, getTelemetryValue_u16le(5)); processGhostTelemetryValue(GHOST_ID_PACK_MAH, getTelemetryValue_u16le(7) * 10); - break; } + + case GHST_DL_GPS_PRIMARY: { +#if defined(BLUETOOTH) + if (g_eeGeneral.bluetoothMode == BLUETOOTH_TELEMETRY && bluetooth.state == BLUETOOTH_STATE_CONNECTED) { + bluetooth.write(telemetryRxBuffer, telemetryRxBufferCount); + } +#endif + processGhostTelemetryValue(GHOST_ID_GPS_LAT, getTelemetryValue_s32le(3) / 10); + processGhostTelemetryValue(GHOST_ID_GPS_LONG, getTelemetryValue_s32le(7) / 10); + processGhostTelemetryValue(GHOST_ID_GPS_ALT, (int16_t) getTelemetryValue_u16le(11)); + break; + } + + case GHST_DL_GPS_SECONDARY: { +#if defined(BLUETOOTH) + if (g_eeGeneral.bluetoothMode == BLUETOOTH_TELEMETRY && bluetooth.state == BLUETOOTH_STATE_CONNECTED) { + bluetooth.write(telemetryRxBuffer, telemetryRxBufferCount); + } +#endif + processGhostTelemetryValue(GHOST_ID_GPS_HDG, getTelemetryValue_u16le(5) / 10); + + // ground speed is passed via GHST as cm/s, converted to km/h for OpenTx + processGhostTelemetryValue(GHOST_ID_GPS_GSPD, (getTelemetryValue_u16le(3) * 36 + 50) / 100); + processGhostTelemetryValue(GHOST_ID_GPS_SATS, telemetryRxBuffer[7]); + break; + } } } @@ -272,6 +327,9 @@ void ghostSetDefault(int index, uint8_t id, uint8_t subId) const GhostSensor * sensor = getGhostSensor(id); if (sensor) { TelemetryUnit unit = sensor->unit; + if (unit == UNIT_GPS_LATITUDE || unit == UNIT_GPS_LONGITUDE) { + unit = UNIT_GPS; + } uint8_t prec = min(2, sensor->precision); telemetrySensor.init(sensor->name, unit, prec); } diff --git a/radio/src/telemetry/ghost.h b/radio/src/telemetry/ghost.h index 0a3195835..9a43da9b7 100755 --- a/radio/src/telemetry/ghost.h +++ b/radio/src/telemetry/ghost.h @@ -44,6 +44,8 @@ #define GHST_DL_VTX_STAT 0x22 #define GHST_DL_PACK_STAT 0x23 #define GHST_DL_MENU_DESC 0x24 +#define GHST_DL_GPS_PRIMARY 0x25 +#define GHST_DL_GPS_SECONDARY 0x26 #define GHST_RC_CTR_VAL_12BIT 0x7C0 // 0x3e0 << 1 #define GHST_RC_CTR_VAL_8BIT 0x7C