diff --git a/src/main/rx/ghst_protocol.h b/src/main/rx/ghst_protocol.h index 3af184a4f2..e06e2b3f55 100644 --- a/src/main/rx/ghst_protocol.h +++ b/src/main/rx/ghst_protocol.h @@ -63,6 +63,9 @@ typedef enum { GHST_DL_LINK_STAT = 0x21, GHST_DL_VTX_STAT = 0x22, GHST_DL_PACK_STAT = 0x23, // Battery (Pack) Status + GHST_DL_GPS_PRIMARY = 0x25, // Primary GPS data (position) + GHST_DL_GPS_SECONDARY = 0x26, + GHST_DL_MAGBARO = 0x27 } ghstDl_e; #define GHST_RC_CTR_VAL_12BIT 0x7C0 // servo center for 12 bit values (0x3e0 << 1) @@ -70,9 +73,16 @@ typedef enum { #define GHST_FRAME_SIZE 14 // including addr, type, len, crc, and payload -#define GHST_PAYLOAD_SIZE_MAX 14 +#define GHST_PAYLOAD_SIZE_MAX 14 -#define GHST_FRAME_SIZE_MAX 24 +#define GHST_FRAME_SIZE_MAX 24 + +#define GPS_FLAGS_FIX 0x01 +#define GPS_FLAGS_FIX_HOME 0x02 + +#define MISC_FLAGS_MAGHEAD 0x01 +#define MISC_FLAGS_BAROALT 0x02 +#define MISC_FLAGS_VARIO 0x04 typedef struct ghstFrameDef_s { uint8_t addr; diff --git a/src/main/telemetry/ghst.c b/src/main/telemetry/ghst.c index e7dc1a6c35..6e372bed2f 100644 --- a/src/main/telemetry/ghst.c +++ b/src/main/telemetry/ghst.c @@ -67,6 +67,7 @@ #define GHST_CYCLETIME_US 100000 // 10x/sec #define GHST_FRAME_PACK_PAYLOAD_SIZE 10 +#define GHST_FRAME_GPS_PAYLOAD_SIZE 10 #define GHST_FRAME_LENGTH_CRC 1 #define GHST_FRAME_LENGTH_TYPE 1 @@ -112,10 +113,97 @@ void ghstFramePackTelemetry(sbuf_t *dst) sbufWriteU8(dst, 0x00); // tbd3 } +// GPS data, primary, positional data +void ghstFrameGpsPrimaryTelemetry(sbuf_t *dst) +{ + // use sbufWrite since CRC does not include frame length + sbufWriteU8(dst, GHST_FRAME_GPS_PAYLOAD_SIZE + GHST_FRAME_LENGTH_CRC + GHST_FRAME_LENGTH_TYPE); + sbufWriteU8(dst, GHST_DL_GPS_PRIMARY); + + sbufWriteU32(dst, gpsSol.llh.lat); + sbufWriteU32(dst, gpsSol.llh.lon); + + int32_t altitudeCm = gpsSol.llh.altCm; // gps Altitude (absolute) + if (!STATE(GPS_FIX)) { + altitudeCm = 0; + } + + const int16_t altitude = altitudeCm / 100; + sbufWriteU16(dst, altitude); +} + +// GPS data, secondary, auxiliary data +void ghstFrameGpsSecondaryTelemetry(sbuf_t *dst) +{ + // use sbufWrite since CRC does not include frame length + sbufWriteU8(dst, GHST_FRAME_GPS_PAYLOAD_SIZE + GHST_FRAME_LENGTH_CRC + GHST_FRAME_LENGTH_TYPE); + sbufWriteU8(dst, GHST_DL_GPS_SECONDARY); + + sbufWriteU16(dst, gpsSol.groundSpeed); // speed in 0.1m/s + sbufWriteU16(dst, gpsSol.groundCourse); // degrees * 10 + sbufWriteU8(dst, gpsSol.numSat); + + sbufWriteU16(dst, (uint16_t) (GPS_distanceToHome / 10)); // use units of 10m to increase range of U16 to 655.36km + sbufWriteU16(dst, GPS_directionToHome); + + uint8_t gpsFlags = 0; + if(STATE(GPS_FIX)) + gpsFlags |= GPS_FLAGS_FIX; + if(STATE(GPS_FIX_HOME)) + gpsFlags |= GPS_FLAGS_FIX_HOME; + sbufWriteU8(dst, gpsFlags); +} + +// Mag, Baro (and Vario) data +void ghstFrameMagBaro(sbuf_t *dst) +{ + int16_t vario = 0; + int16_t altitude = 0; + int16_t yaw = 0; + uint8_t flags = 0; + +#ifdef USE_VARIO + if (sensors(SENSOR_VARIO) && telemetryIsSensorEnabled(SENSOR_VARIO)) { + vario = getEstimatedVario(); // vario, cm/s + flags |= MISC_FLAGS_VARIO; + } +#endif + +#ifdef USE_BARO + if (sensors(SENSOR_BARO) && telemetryIsSensorEnabled(SENSOR_ALTITUDE)) { + flags |= MISC_FLAGS_BAROALT; + altitude = (constrain(getEstimatedAltitudeCm(), -32000 * 100, 32000 * 100) / 100); + } +#endif + +#ifdef USE_MAG + if (sensors(SENSOR_MAG) && telemetryIsSensorEnabled(SENSOR_HEADING)) { + flags |= MISC_FLAGS_MAGHEAD; + yaw = attitude.values.yaw; + } +#endif + + // use sbufWrite since CRC does not include frame length + sbufWriteU8(dst, GHST_FRAME_GPS_PAYLOAD_SIZE + GHST_FRAME_LENGTH_CRC + GHST_FRAME_LENGTH_TYPE); + sbufWriteU8(dst, GHST_DL_MAGBARO); + + sbufWriteU16(dst, yaw); // magHeading, deci-degrees + sbufWriteU16(dst, altitude); // baroAltitude, m + sbufWriteU8(dst, vario); // cm/s + + sbufWriteU16(dst, 0); + sbufWriteU16(dst, 0); + + sbufWriteU8(dst, flags); +} + // schedule array to decide how often each type of frame is sent typedef enum { GHST_FRAME_START_INDEX = 0, GHST_FRAME_PACK_INDEX = GHST_FRAME_START_INDEX, // Battery (Pack) data + GHST_FRAME_GPS_PRIMARY_INDEX, // GPS, primary values (Lat, Long, Alt) + GHST_FRAME_GPS_SECONDARY_INDEX, // GPS, secondary values (Sat Count, HDOP, etc.) + GHST_FRAME_MAGBARO_INDEX, // Magnetometer/Baro values GHST_SCHEDULE_COUNT_MAX } ghstFrameTypeIndex_e; @@ -136,6 +224,25 @@ static void processGhst(void) ghstFramePackTelemetry(dst); ghstFinalize(dst); } + + if (currentSchedule & BIT(GHST_FRAME_GPS_PRIMARY_INDEX)) { + ghstInitializeFrame(dst); + ghstFrameGpsPrimaryTelemetry(dst); + ghstFinalize(dst); + } + + if (currentSchedule & BIT(GHST_FRAME_GPS_SECONDARY_INDEX)) { + ghstInitializeFrame(dst); + ghstFrameGpsSecondaryTelemetry(dst); + ghstFinalize(dst); + } + + if (currentSchedule & BIT(GHST_FRAME_MAGBARO_INDEX)) { + ghstInitializeFrame(dst); + ghstFrameMagBaro(dst); + ghstFinalize(dst); + } + ghstScheduleIndex = (ghstScheduleIndex + 1) % ghstScheduleCount; } @@ -153,6 +260,27 @@ void initGhstTelemetry(void) || (isAmperageConfigured() && telemetryIsSensorEnabled(SENSOR_CURRENT | SENSOR_FUEL))) { ghstSchedule[index++] = BIT(GHST_FRAME_PACK_INDEX); } + +#ifdef USE_GPS + if (featureIsEnabled(FEATURE_GPS) + && telemetryIsSensorEnabled(SENSOR_ALTITUDE | SENSOR_LAT_LONG)) { + ghstSchedule[index++] = BIT(GHST_FRAME_GPS_PRIMARY_INDEX); + } + + if (featureIsEnabled(FEATURE_GPS) + && telemetryIsSensorEnabled(SENSOR_GROUND_SPEED | SENSOR_HEADING)) { + ghstSchedule[index++] = BIT(GHST_FRAME_GPS_SECONDARY_INDEX); + } +#endif + +#if defined(USE_BARO) || defined(USE_MAG) || defined(USE_VARIO) + if ((sensors(SENSOR_BARO) && telemetryIsSensorEnabled(SENSOR_ALTITUDE)) + || (sensors(SENSOR_MAG) && telemetryIsSensorEnabled(SENSOR_HEADING)) + || (sensors(SENSOR_VARIO) && telemetryIsSensorEnabled(SENSOR_VARIO))) { + ghstSchedule[index++] = BIT(GHST_FRAME_MAGBARO_INDEX); + } +#endif + ghstScheduleCount = (uint8_t)index; }