rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Add GPS Telemetry support to GHST 

Squashed version
This commit is contained in:
Tony Cake 2021-05-17 08:15:27 -07:00
parent 26b74c3350
commit 3b3f4893f8
2 changed files with 140 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/main/rx/ghst_protocol.h
View File

@ -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;

128
src/main/telemetry/ghst.c
View File

@ -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;
}