Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 04:15:44 +03:00
Code Issues Wiki Activity

Merge pull request #10801 from daleckystepan/ghst

Browse source
This commit is contained in:
J Blackman 2022-06-27 21:25:11 +10:00 • committed by GitHub
commit ccbb2491b3
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
5 changed files with 203 additions and 106 deletions
Download patch file Download diff file Expand all files Collapse all files

216
src/main/rx/ghst.c
View file

@ -52,7 +52,7 @@
#define GHST_PORT_MODE MODE_RXTX // bidirectional on single pin #define GHST_PORT_MODE MODE_RXTX // bidirectional on single pin
#define GHST_MAX_FRAME_TIME_US 500 // 14 bytes @ 420k = ~450us #define GHST_MAX_FRAME_TIME_US 500 // 14 bytes @ 420k = ~450us
#define GHST_TIME_BETWEEN_FRAMES_US 4500 // fastest frame rate = 222.22Hz, or 4500us #define GHST_TIME_BETWEEN_FRAMES_US 2000 // fastest frame rate = 500Hz, or 2000us
#define GHST_RSSI_DBM_MIN (-117) // Long Range mode value #define GHST_RSSI_DBM_MIN (-117) // Long Range mode value
#define GHST_RSSI_DBM_MAX (-60) // Typical RSSI with typical power levels, typical antennas, and a few feet/meters between Tx and Rx #define GHST_RSSI_DBM_MAX (-60) // Typical RSSI with typical power levels, typical antennas, and a few feet/meters between Tx and Rx
@ -65,14 +65,20 @@
#define GHST_PAYLOAD_OFFSET offsetof(ghstFrameDef_t, type) #define GHST_PAYLOAD_OFFSET offsetof(ghstFrameDef_t, type)
#define GHST_RC_CTR_VAL_12BIT_PRIMARY 2048
#define GHST_RC_CTR_VAL_12BIT_AUX (128 << 2)
STATIC_UNIT_TESTED volatile bool ghstFrameAvailable = false; STATIC_UNIT_TESTED volatile bool ghstFrameAvailable = false;
STATIC_UNIT_TESTED volatile bool ghstValidatedFrameAvailable = false; STATIC_UNIT_TESTED volatile bool ghstValidatedFrameAvailable = false;
STATIC_UNIT_TESTED volatile bool ghstTransmittingTelemetry = false; STATIC_UNIT_TESTED volatile bool ghstTransmittingTelemetry = false;
STATIC_UNIT_TESTED ghstFrame_t ghstIncomingFrame; // incoming frame, raw, not CRC checked, destination address not checked STATIC_UNIT_TESTED ghstFrame_t ghstFrameBuffer[2];
STATIC_UNIT_TESTED ghstFrame_t ghstValidatedFrame; // validated frame, CRC is ok, destination address is ok, ready for decode
STATIC_UNIT_TESTED uint32_t ghstChannelData[GHST_MAX_NUM_CHANNELS]; ghstFrame_t *ghstIncomingFrame = &ghstFrameBuffer[0]; // incoming frame, raw, not CRC checked, destination address not checked
ghstFrame_t *ghstValidatedFrame = &ghstFrameBuffer[1]; // validated frame, CRC is ok, destination address is ok, ready for decode
STATIC_UNIT_TESTED uint16_t ghstChannelData[GHST_MAX_NUM_CHANNELS];
static ghstRfProtocol_e ghstRfProtocol = GHST_RF_PROTOCOL_UNDEFINED;
enum { enum {
DEBUG_GHST_CRC_ERRORS = 0, DEBUG_GHST_CRC_ERRORS = 0,
@ -84,7 +90,7 @@ enum {
static serialPort_t *serialPort; static serialPort_t *serialPort;
static timeUs_t ghstRxFrameStartAtUs = 0; static timeUs_t ghstRxFrameStartAtUs = 0;
static timeUs_t ghstRxFrameEndAtUs = 0; static timeUs_t ghstRxFrameEndAtUs = 0;
static uint8_t telemetryBuf[GHST_FRAME_SIZE_MAX]; static uint8_t telemetryBuf[GHST_FRAME_SIZE];
static uint8_t telemetryBufLen = 0; static uint8_t telemetryBufLen = 0;
/* GHST Protocol /* GHST Protocol
@ -104,18 +110,14 @@ static uint8_t telemetryBufLen = 0;
* the Ghost receiver to Betaflight as and when they arrive. Betaflight itself is responsible for * the Ghost receiver to Betaflight as and when they arrive. Betaflight itself is responsible for
* determining when a failsafe is necessary based on dropped packets. * determining when a failsafe is necessary based on dropped packets.
* *
*/ */
#define GHST_FRAME_LENGTH_ADDRESS 1
#define GHST_FRAME_LENGTH_FRAMELENGTH 1
#define GHST_FRAME_LENGTH_TYPE_CRC 1
// called from telemetry/ghst.c // called from telemetry/ghst.c
void ghstRxWriteTelemetryData(const void *data, int len) void ghstRxWriteTelemetryData(const void *const data, const int len)
{ {
len = MIN(len, (int)sizeof(telemetryBuf)); telemetryBufLen = MIN(len, (int)sizeof(telemetryBuf));
memcpy(telemetryBuf, data, len); memcpy(telemetryBuf, data, telemetryBufLen);
telemetryBufLen = len;
} }
void ghstRxSendTelemetryData(void) void ghstRxSendTelemetryData(void)
@ -127,16 +129,23 @@ void ghstRxSendTelemetryData(void)
} }
} }
STATIC_UNIT_TESTED uint8_t ghstFrameCRC(ghstFrame_t *pGhstFrame) STATIC_UNIT_TESTED uint8_t ghstFrameCRC(const ghstFrame_t *const pGhstFrame)
{ {
// CRC includes type and payload // CRC includes type and payload
uint8_t crc = crc8_dvb_s2(0, pGhstFrame->frame.type); uint8_t crc = crc8_dvb_s2(0, pGhstFrame->frame.type);
for (int i = 0; i < pGhstFrame->frame.len - GHST_FRAME_LENGTH_TYPE_CRC - 1; ++i) { for (int i = 0; i < pGhstFrame->frame.len - GHST_FRAME_LENGTH_TYPE - GHST_FRAME_LENGTH_CRC; ++i) {
crc = crc8_dvb_s2(crc, pGhstFrame->frame.payload[i]); crc = crc8_dvb_s2(crc, pGhstFrame->frame.payload[i]);
} }
return crc; return crc;
} }
static void rxSwapFrameBuffers()
{
ghstFrame_t *const tmp = ghstIncomingFrame;
ghstIncomingFrame = ghstValidatedFrame;
ghstValidatedFrame = tmp;
}
// Receive ISR callback, called back from serial port // Receive ISR callback, called back from serial port
STATIC_UNIT_TESTED void ghstDataReceive(uint16_t c, void *data) STATIC_UNIT_TESTED void ghstDataReceive(uint16_t c, void *data)
{ {
@ -157,20 +166,26 @@ STATIC_UNIT_TESTED void ghstDataReceive(uint16_t c, void *data)
// assume frame is 5 bytes long until we have received the frame length // assume frame is 5 bytes long until we have received the frame length
// full frame length includes the length of the address and framelength fields // full frame length includes the length of the address and framelength fields
const int fullFrameLength = ghstFrameIdx < 3 ? 5 : ghstIncomingFrame.frame.len + GHST_FRAME_LENGTH_ADDRESS + GHST_FRAME_LENGTH_FRAMELENGTH; const int fullFrameLength = ghstFrameIdx < 3 ? 5 : ghstIncomingFrame->frame.len + GHST_FRAME_LENGTH_ADDRESS + GHST_FRAME_LENGTH_FRAMELENGTH;
if (ghstFrameIdx < fullFrameLength) { if (ghstFrameIdx < fullFrameLength && ghstFrameIdx < sizeof(ghstFrame_t)) {
ghstIncomingFrame.bytes[ghstFrameIdx++] = (uint8_t)c; ghstIncomingFrame->bytes[ghstFrameIdx++] = (uint8_t)c;
if (ghstFrameIdx >= fullFrameLength) { if (ghstFrameIdx >= fullFrameLength) {
ghstFrameIdx = 0; ghstFrameIdx = 0;
// NOTE: this data is not yet CRC checked, nor do we know whether we are the correct recipient, this is // NOTE: this data is not yet CRC checked, nor do we know whether we are the correct recipient, this is
// handled in ghstFrameStatus // handled in ghstFrameStatus
memcpy(&ghstValidatedFrame, &ghstIncomingFrame, sizeof(ghstIncomingFrame));
ghstFrameAvailable = true;
// remember what time the incoming (Rx) packet ended, so that we can ensure a quite bus before sending telemetry // Not CRC checked but we are interested just in frame for us
ghstRxFrameEndAtUs = microsISR(); // eg. telemetry frames are read back here also, skip them
if (ghstIncomingFrame->frame.addr == GHST_ADDR_FC) {
rxSwapFrameBuffers();
ghstFrameAvailable = true;
// remember what time the incoming (Rx) packet ended, so that we can ensure a quite bus before sending telemetry
ghstRxFrameEndAtUs = microsISR();
}
} }
} }
} }
@ -178,7 +193,7 @@ STATIC_UNIT_TESTED void ghstDataReceive(uint16_t c, void *data)
static bool shouldSendTelemetryFrame(void) static bool shouldSendTelemetryFrame(void)
{ {
const timeUs_t now = micros(); const timeUs_t now = micros();
const timeUs_t timeSinceRxFrameEndUs = cmpTimeUs(now, ghstRxFrameEndAtUs); const timeDelta_t timeSinceRxFrameEndUs = cmpTimeUs(now, ghstRxFrameEndAtUs);
return telemetryBufLen > 0 && timeSinceRxFrameEndUs > GHST_RX_TO_TELEMETRY_MIN_US && timeSinceRxFrameEndUs < GHST_RX_TO_TELEMETRY_MAX_US; return telemetryBufLen > 0 && timeSinceRxFrameEndUs > GHST_RX_TO_TELEMETRY_MIN_US && timeSinceRxFrameEndUs < GHST_RX_TO_TELEMETRY_MAX_US;
} }
@ -186,29 +201,25 @@ STATIC_UNIT_TESTED uint8_t ghstFrameStatus(rxRuntimeState_t *rxRuntimeState)
{ {
UNUSED(rxRuntimeState); UNUSED(rxRuntimeState);
static int16_t crcErrorCount = 0; static int16_t crcErrorCount = 0;
uint8_t status = RX_FRAME_PENDING;
if (ghstFrameAvailable) { if (ghstFrameAvailable) {
ghstFrameAvailable = false; ghstFrameAvailable = false;
const uint8_t crc = ghstFrameCRC(&ghstValidatedFrame); const uint8_t crc = ghstFrameCRC(ghstValidatedFrame);
const int fullFrameLength = ghstValidatedFrame.frame.len + GHST_FRAME_LENGTH_ADDRESS + GHST_FRAME_LENGTH_FRAMELENGTH; const int fullFrameLength = ghstValidatedFrame->frame.len + GHST_FRAME_LENGTH_ADDRESS + GHST_FRAME_LENGTH_FRAMELENGTH;
if (crc == ghstValidatedFrame.bytes[fullFrameLength - 1] && ghstValidatedFrame.frame.addr == GHST_ADDR_FC) { if (crc == ghstValidatedFrame->bytes[fullFrameLength - 1]) {
ghstValidatedFrameAvailable = true; ghstValidatedFrameAvailable = true;
return RX_FRAME_COMPLETE | RX_FRAME_PROCESSING_REQUIRED; // request callback through ghstProcessFrame to do the decoding work status = RX_FRAME_COMPLETE | RX_FRAME_PROCESSING_REQUIRED; // request callback through ghstProcessFrame to do the decoding work
} } else {
if (crc != ghstValidatedFrame.bytes[fullFrameLength - 1]) {
DEBUG_SET(DEBUG_GHST, DEBUG_GHST_CRC_ERRORS, ++crcErrorCount); DEBUG_SET(DEBUG_GHST, DEBUG_GHST_CRC_ERRORS, ++crcErrorCount);
status = RX_FRAME_DROPPED; // frame was invalid
} }
} else if (checkGhstTelemetryState() && shouldSendTelemetryFrame()) {
return RX_FRAME_DROPPED; // frame was invalid status = RX_FRAME_PROCESSING_REQUIRED;
} }
if (shouldSendTelemetryFrame()) { return status;
return RX_FRAME_PROCESSING_REQUIRED;
}
return RX_FRAME_PENDING;
} }
static bool ghstProcessFrame(const rxRuntimeState_t *rxRuntimeState) static bool ghstProcessFrame(const rxRuntimeState_t *rxRuntimeState)
@ -217,38 +228,44 @@ static bool ghstProcessFrame(const rxRuntimeState_t *rxRuntimeState)
// is correct, and the message was actually for us. // is correct, and the message was actually for us.
UNUSED(rxRuntimeState); UNUSED(rxRuntimeState);
static int16_t unknownFrameCount = 0; static int16_t unknownFrameCount = 0;
// do we have a telemetry buffer to send? // do we have a telemetry buffer to send?
if (shouldSendTelemetryFrame()) { if (checkGhstTelemetryState() && shouldSendTelemetryFrame()) {
ghstTransmittingTelemetry = true; ghstTransmittingTelemetry = true;
ghstRxSendTelemetryData(); ghstRxSendTelemetryData();
} }
if (ghstValidatedFrameAvailable) { if (ghstValidatedFrameAvailable) {
int startIdx = 0; ghstValidatedFrameAvailable = false;
if (ghstValidatedFrame.frame.type >= GHST_UL_RC_CHANS_HS4_FIRST && const uint8_t ghstFrameType = ghstValidatedFrame->frame.type;
ghstValidatedFrame.frame.type <= GHST_UL_RC_CHANS_HS4_LAST) { const bool scalingLegacy = ghstFrameType >= GHST_UL_RC_CHANS_HS4_FIRST && ghstFrameType <= GHST_UL_RC_CHANS_HS4_LAST;
const ghstPayloadPulses_t* const rcChannels = (ghstPayloadPulses_t*)&ghstValidatedFrame.frame.payload; const bool scaling12bit = ghstFrameType >= GHST_UL_RC_CHANS_HS4_12_FIRST && ghstFrameType <= GHST_UL_RC_CHANS_HS4_12_LAST;
// all uplink frames contain CH1..4 data (12 bit) if ( scaling12bit || scalingLegacy ) {
ghstChannelData[0] = rcChannels->ch1to4.ch1 >> 1;
ghstChannelData[1] = rcChannels->ch1to4.ch2 >> 1;
ghstChannelData[2] = rcChannels->ch1to4.ch3 >> 1;
ghstChannelData[3] = rcChannels->ch1to4.ch4 >> 1;
switch(ghstValidatedFrame.frame.type) { int startIdx = 0;
case GHST_UL_RC_CHANS_HS4_RSSI: {
const ghstPayloadPulsesRssi_t* const rssiFrame = (ghstPayloadPulsesRssi_t*)&ghstValidatedFrame.frame.payload; switch (ghstFrameType) {
case GHST_UL_RC_CHANS_HS4_RSSI:
case GHST_UL_RC_CHANS_HS4_12_RSSI: {
const ghstPayloadPulsesRssi_t* const rssiFrame = (ghstPayloadPulsesRssi_t*)&ghstValidatedFrame->frame.payload;
DEBUG_SET(DEBUG_GHST, DEBUG_GHST_RX_RSSI, -rssiFrame->rssi); DEBUG_SET(DEBUG_GHST, DEBUG_GHST_RX_RSSI, -rssiFrame->rssi);
DEBUG_SET(DEBUG_GHST, DEBUG_GHST_RX_LQ, rssiFrame->lq); DEBUG_SET(DEBUG_GHST, DEBUG_GHST_RX_LQ, rssiFrame->lq);
ghstRfProtocol = rssiFrame->rfProtocol;
// Enable telemetry just for modes that support it
setGhstTelemetryState(ghstRfProtocol == GHST_RF_PROTOCOL_NORMAL
|| ghstRfProtocol == GHST_RF_PROTOCOL_RACE
|| ghstRfProtocol == GHST_RF_PROTOCOL_LONGRANGE
|| ghstRfProtocol == GHST_RF_PROTOCOL_RACE250);
if (rssiSource == RSSI_SOURCE_RX_PROTOCOL) { if (rssiSource == RSSI_SOURCE_RX_PROTOCOL) {
// rssi sent sign-inverted // rssi sent sign-inverted
const uint16_t rssiPercentScaled = scaleRange(-rssiFrame->rssi, GHST_RSSI_DBM_MIN, 0, GHST_RSSI_DBM_MAX, RSSI_MAX_VALUE); uint16_t rssiPercentScaled = scaleRange(-rssiFrame->rssi, GHST_RSSI_DBM_MIN, GHST_RSSI_DBM_MAX, 0, RSSI_MAX_VALUE);
rssiPercentScaled = MIN(rssiPercentScaled, RSSI_MAX_VALUE);
setRssi(rssiPercentScaled, RSSI_SOURCE_RX_PROTOCOL); setRssi(rssiPercentScaled, RSSI_SOURCE_RX_PROTOCOL);
} }
@ -264,22 +281,55 @@ static bool ghstProcessFrame(const rxRuntimeState_t *rxRuntimeState)
break; break;
} }
case GHST_UL_RC_CHANS_HS4_5TO8: startIdx = 4; break; case GHST_UL_RC_CHANS_HS4_5TO8:
case GHST_UL_RC_CHANS_HS4_9TO12: startIdx = 8; break; case GHST_UL_RC_CHANS_HS4_12_5TO8:
case GHST_UL_RC_CHANS_HS4_13TO16: startIdx = 12; break; startIdx = 4;
break;
case GHST_UL_RC_CHANS_HS4_9TO12:
case GHST_UL_RC_CHANS_HS4_12_9TO12:
startIdx = 8;
break;
case GHST_UL_RC_CHANS_HS4_13TO16:
case GHST_UL_RC_CHANS_HS4_12_13TO16:
startIdx = 12;
break;
default: default:
DEBUG_SET(DEBUG_GHST, DEBUG_GHST_UNKNOWN_FRAMES, ++unknownFrameCount); DEBUG_SET(DEBUG_GHST, DEBUG_GHST_UNKNOWN_FRAMES, ++unknownFrameCount);
break; break;
} }
if (startIdx > 0) // We need to wait for the first RSSI frame to know ghstRfProtocol
{ if (ghstRfProtocol != GHST_RF_PROTOCOL_UNDEFINED) {
// remainder of uplink frame contains 4 more channels (8 bit), sent in a round-robin fashion const ghstPayloadPulses_t* const rcChannels = (ghstPayloadPulses_t*)&ghstValidatedFrame->frame.payload;
// all uplink frames contain CH1..4 data (12 bit)
ghstChannelData[0] = rcChannels->ch1to4.ch1;
ghstChannelData[1] = rcChannels->ch1to4.ch2;
ghstChannelData[2] = rcChannels->ch1to4.ch3;
ghstChannelData[3] = rcChannels->ch1to4.ch4;
if (startIdx > 0) {
// remainder of uplink frame contains 4 more channels (8 bit), sent in a round-robin fashion
ghstChannelData[startIdx++] = rcChannels->cha << 2;
ghstChannelData[startIdx++] = rcChannels->chb << 2;
ghstChannelData[startIdx++] = rcChannels->chc << 2;
ghstChannelData[startIdx++] = rcChannels->chd << 2;
}
// Recalculate old scaling to the new one
if (scalingLegacy) {
for (int i = 0; i < 4; i++) {
ghstChannelData[i] = ((5 * ghstChannelData[i]) >> 2) - 430; // Primary
if (startIdx > 4) {
--startIdx;
ghstChannelData[startIdx] = 5 * (ghstChannelData[startIdx] >> 2) - 108; // Aux
}
}
}
ghstChannelData[startIdx++] = rcChannels->cha << 3;
ghstChannelData[startIdx++] = rcChannels->chb << 3;
ghstChannelData[startIdx++] = rcChannels->chc << 3;
ghstChannelData[startIdx++] = rcChannels->chd << 3;
} }
} }
} }
@ -291,16 +341,30 @@ STATIC_UNIT_TESTED float ghstReadRawRC(const rxRuntimeState_t *rxRuntimeState, u
{ {
UNUSED(rxRuntimeState); UNUSED(rxRuntimeState);
// derived from original SBus scaling, with slight correction for offset (now symmetrical // Scaling 12bit channels (8bit channels in brackets)
// around OpenTx 0 value) // OpenTx RC PWM (BF)
// scaling is: // min -1024 0( 0) 988us
// OpenTx RC PWM // ctr 0 2048(128) 1500us
// min -1024 172 988us // max 1024 4096(256) 2012us
// ctr 0 992 1500us
// max 1024 1811 2012us
// //
return (5 * ((float)ghstChannelData[chan] + 1) / 8) + 880; // Scaling legacy (nearly 10bit)
// derived from original SBus scaling, with slight correction for offset
// now symmetrical around OpenTx 0 value
// scaling is:
// OpenTx RC PWM (BF)
// min -1024 172( 22) 988us
// ctr 0 992(124) 1500us
// max 1024 1811(226) 2012us
//
float pwm = ghstChannelData[chan];
if (chan < 4) {
pwm = 0.25f * pwm;
}
return pwm + 988;
} }
// UART idle detected (inter-packet) // UART idle detected (inter-packet)
@ -313,16 +377,20 @@ static void ghstIdle()
bool ghstRxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState) bool ghstRxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
{ {
for (int iChan = 0; iChan < GHST_MAX_NUM_CHANNELS; ++iChan) {
ghstChannelData[iChan] = (16 * rxConfig->midrc) / 10 - 1408;
}
rxRuntimeState->channelCount = GHST_MAX_NUM_CHANNELS; rxRuntimeState->channelCount = GHST_MAX_NUM_CHANNELS;
rxRuntimeState->rcReadRawFn = ghstReadRawRC; rxRuntimeState->rcReadRawFn = ghstReadRawRC;
rxRuntimeState->rcFrameStatusFn = ghstFrameStatus; rxRuntimeState->rcFrameStatusFn = ghstFrameStatus;
rxRuntimeState->rcFrameTimeUsFn = rxFrameTimeUs; rxRuntimeState->rcFrameTimeUsFn = rxFrameTimeUs;
rxRuntimeState->rcProcessFrameFn = ghstProcessFrame; rxRuntimeState->rcProcessFrameFn = ghstProcessFrame;
for (int iChan = 0; iChan < rxRuntimeState->channelCount; ++iChan) {
if (iChan < 4 ) {
ghstChannelData[iChan] = GHST_RC_CTR_VAL_12BIT_PRIMARY;
} else {
ghstChannelData[iChan] = GHST_RC_CTR_VAL_12BIT_AUX;
}
}
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
if (!portConfig) { if (!portConfig) {
return false; return false;

7
src/main/rx/ghst.h
View file

@ -24,10 +24,11 @@
#define GHST_MAX_NUM_CHANNELS 16 #define GHST_MAX_NUM_CHANNELS 16
void ghstRxWriteTelemetryData(const void *data, int len);
void ghstRxSendTelemetryData(void);
struct rxConfig_s; struct rxConfig_s;
struct rxRuntimeState_s; struct rxRuntimeState_s;
void ghstRxWriteTelemetryData(const void *const data, const int len);
void ghstRxSendTelemetryData(void);
bool ghstRxInit(const struct rxConfig_s *initialRxConfig, struct rxRuntimeState_s *rxRuntimeState); bool ghstRxInit(const struct rxConfig_s *initialRxConfig, struct rxRuntimeState_s *rxRuntimeState);
bool ghstRxIsActive(void); bool ghstRxIsActive(void);

53
src/main/rx/ghst_protocol.h
View file

@ -45,18 +45,25 @@ typedef enum {
} ghstAddr_e; } ghstAddr_e;
typedef enum { typedef enum {
// frame types 0x10 - 0x1f always include 4 primary channels, plus either 4 aux channels, // frame types 0x10 - 0x1f always include 10bit 4 primary channels, plus either 4 aux channels,
// or other type-specific data. Expect types 0x14-0x1f to be added in the future, and even though // frame types 0x30 - 0x3f always include 12bit 4 primary channels, plus either 4 aux channels,
// or other type-specific data. Expect types 0x14-0x1f (0x34 - 0x3f) to be added in the future, and even though
// not explicitly supported, the 4 primary channels should always be extracted. // not explicitly supported, the 4 primary channels should always be extracted.
GHST_UL_RC_CHANS_HS4_FIRST = 0x10, // First frame type including 4 primary channels GHST_UL_RC_CHANS_HS4_FIRST = 0x10, // 10 bit first frame
GHST_UL_RC_CHANS_HS4_5TO8 = 0x10, // primary 4 channel, plus CH5-8 GHST_UL_RC_CHANS_HS4_5TO8 = 0x10, // 10 bit primary 4 channel, plus CH5-8
GHST_UL_RC_CHANS_HS4_9TO12 = 0x11, // primary 4 channel, plus CH9-12 GHST_UL_RC_CHANS_HS4_9TO12 = 0x11, // 10 bit primary 4 channel, plus CH9-12
GHST_UL_RC_CHANS_HS4_13TO16 = 0x12, // primary 4 channel, plus CH13-16 GHST_UL_RC_CHANS_HS4_13TO16 = 0x12, // 10 bit primary 4 channel, plus CH13-16
GHST_UL_RC_CHANS_HS4_RSSI = 0x13, // primary 4 channel, plus RSSI, LQ, RF Mode, and Tx Power GHST_UL_RC_CHANS_HS4_RSSI = 0x13, // 10 bit primary 4 channel, plus RSSI, LQ, RF Mode, and Tx Power
GHST_UL_RC_CHANS_HS4_LAST = 0x1f // Last frame type including 4 primary channels GHST_UL_RC_CHANS_HS4_LAST = 0x1f, // 10 bit last frame type
} ghstUl_e;
#define GHST_UL_RC_CHANS_SIZE 12 // 1 (type) + 10 (data) + 1 (crc) GHST_UL_RC_CHANS_HS4_12_FIRST = 0x30, // 12 bit first frame
GHST_UL_RC_CHANS_HS4_12_5TO8 = 0x30, // 12 bit primary 4 channel, plus CH5-8
GHST_UL_RC_CHANS_HS4_12_9TO12 = 0x31, // 12 bit primary 4 channel, plus CH9-12
GHST_UL_RC_CHANS_HS4_12_13TO16 = 0x32, // 12 bit primary 4 channel, plus CH13-16
GHST_UL_RC_CHANS_HS4_12_RSSI = 0x33, // 12 bit primary 4 channel, plus RSSI, LQ, RF Mode, and Tx Power
GHST_UL_RC_CHANS_HS4_12_LAST = 0x3f, // 12 bit last frame type
} ghstUl_e;
typedef enum { typedef enum {
GHST_DL_OPENTX_SYNC = 0x20, GHST_DL_OPENTX_SYNC = 0x20,
@ -68,14 +75,28 @@ typedef enum {
GHST_DL_MAGBARO = 0x27 GHST_DL_MAGBARO = 0x27
} ghstDl_e; } ghstDl_e;
typedef enum {
GHST_RF_PROTOCOL_UNDEFINED = 0x00,
GHST_RF_PROTOCOL_NORMAL = 0x05,
GHST_RF_PROTOCOL_RACE = 0x06,
GHST_RF_PROTOCOL_PURERACE = 0x07,
GHST_RF_PROTOCOL_LONGRANGE = 0x08,
GHST_RF_PROTOCOL_RACE250 = 0x0A, // 10
GHST_RF_PROTOCOL_RACE500 = 0x0B, // 11
GHST_RF_PROTOCOL_SOLID150 = 0x0C, // 12
GHST_RF_PROTOCOL_SOLID250 = 0x0D // 13
} ghstRfProtocol_e;
#define GHST_RC_CTR_VAL_12BIT 0x7C0 // servo center for 12 bit values (0x3e0 << 1) #define GHST_RC_CTR_VAL_12BIT 0x7C0 // servo center for 12 bit values (0x3e0 << 1)
#define GHST_RC_CTR_VAL_8BIT 0x7C // servo center for 8 bit values #define GHST_RC_CTR_VAL_8BIT 0x7C // servo center for 8 bit values
#define GHST_FRAME_SIZE 14 // including addr, type, len, crc, and payload #define GHST_FRAME_SIZE 14 // including addr, type, len, crc, and payload
#define GHST_PAYLOAD_SIZE 10 // just payload size
#define GHST_PAYLOAD_SIZE_MAX 14 #define GHST_FRAME_LENGTH_ADDRESS 1
#define GHST_FRAME_LENGTH_FRAMELENGTH 1
#define GHST_FRAME_SIZE_MAX 24 #define GHST_FRAME_LENGTH_TYPE 1
#define GHST_FRAME_LENGTH_CRC 1
#define GPS_FLAGS_FIX 0x01 #define GPS_FLAGS_FIX 0x01
#define GPS_FLAGS_FIX_HOME 0x02 #define GPS_FLAGS_FIX_HOME 0x02
@ -86,10 +107,10 @@ typedef enum {
typedef struct ghstFrameDef_s { typedef struct ghstFrameDef_s {
uint8_t addr; uint8_t addr;
uint8_t len; uint8_t len; // len = sizeof(type) + sizeof(payload) + sizeof(crc)
uint8_t type; uint8_t type;
uint8_t payload[GHST_PAYLOAD_SIZE_MAX + 1]; // CRC adds 1 uint8_t payload[GHST_PAYLOAD_SIZE + GHST_FRAME_LENGTH_CRC];
} ghstFrameDef_t; } __attribute__ ((__packed__)) ghstFrameDef_t;
typedef union ghstFrame_u { typedef union ghstFrame_u {
uint8_t bytes[GHST_FRAME_SIZE]; uint8_t bytes[GHST_FRAME_SIZE];

31
src/main/telemetry/ghst.c
View file

@ -68,11 +68,12 @@
#define GHST_CYCLETIME_US 100000 // 10x/sec #define GHST_CYCLETIME_US 100000 // 10x/sec
#define GHST_FRAME_PACK_PAYLOAD_SIZE 10 #define GHST_FRAME_PACK_PAYLOAD_SIZE 10
#define GHST_FRAME_GPS_PAYLOAD_SIZE 10 #define GHST_FRAME_GPS_PAYLOAD_SIZE 10
#define GHST_FRAME_MAGBARO_PAYLOAD_SIZE 10
#define GHST_FRAME_LENGTH_CRC 1 #define GHST_FRAME_LENGTH_CRC 1
#define GHST_FRAME_LENGTH_TYPE 1 #define GHST_FRAME_LENGTH_TYPE 1
static bool ghstTelemetryEnabled; static bool ghstTelemetryEnabled;
static uint8_t ghstFrame[GHST_FRAME_SIZE_MAX]; static uint8_t ghstFrame[GHST_FRAME_SIZE];
static void ghstInitializeFrame(sbuf_t *dst) static void ghstInitializeFrame(sbuf_t *dst)
{ {
@ -95,7 +96,7 @@ void ghstFramePackTelemetry(sbuf_t *dst)
{ {
// use sbufWrite since CRC does not include frame length // use sbufWrite since CRC does not include frame length
sbufWriteU8(dst, GHST_FRAME_PACK_PAYLOAD_SIZE + GHST_FRAME_LENGTH_CRC + GHST_FRAME_LENGTH_TYPE); sbufWriteU8(dst, GHST_FRAME_PACK_PAYLOAD_SIZE + GHST_FRAME_LENGTH_CRC + GHST_FRAME_LENGTH_TYPE);
sbufWriteU8(dst, 0x23); // GHST_DL_PACK_STAT sbufWriteU8(dst, GHST_DL_PACK_STAT);
if (telemetryConfig()->report_cell_voltage) { if (telemetryConfig()->report_cell_voltage) {
sbufWriteU16(dst, getBatteryAverageCellVoltage()); // units of 10mV sbufWriteU16(dst, getBatteryAverageCellVoltage()); // units of 10mV
@ -142,8 +143,8 @@ void ghstFrameGpsSecondaryTelemetry(sbuf_t *dst)
sbufWriteU16(dst, gpsSol.groundSpeed); // speed in 0.1m/s sbufWriteU16(dst, gpsSol.groundSpeed); // speed in 0.1m/s
sbufWriteU16(dst, gpsSol.groundCourse); // degrees * 10 sbufWriteU16(dst, gpsSol.groundCourse); // degrees * 10
sbufWriteU8(dst, gpsSol.numSat); 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_distanceToHome / 10); // use units of 10m to increase range of U16 to 655.36km
sbufWriteU16(dst, GPS_directionToHome / 10); sbufWriteU16(dst, GPS_directionToHome / 10);
uint8_t gpsFlags = 0; uint8_t gpsFlags = 0;
@ -186,13 +187,13 @@ void ghstFrameMagBaro(sbuf_t *dst)
#endif #endif
// use sbufWrite since CRC does not include frame length // 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_FRAME_MAGBARO_PAYLOAD_SIZE + GHST_FRAME_LENGTH_CRC + GHST_FRAME_LENGTH_TYPE);
sbufWriteU8(dst, GHST_DL_MAGBARO); sbufWriteU8(dst, GHST_DL_MAGBARO);
sbufWriteU16(dst, yaw); // magHeading, deci-degrees sbufWriteU16(dst, yaw); // magHeading, deci-degrees
sbufWriteU16(dst, altitude); // baroAltitude, m sbufWriteU16(dst, altitude); // baroAltitude, m
sbufWriteU8(dst, vario); // cm/s sbufWriteU8(dst, vario); // cm/s
sbufWriteU16(dst, 0); sbufWriteU16(dst, 0);
sbufWriteU16(dst, 0); sbufWriteU16(dst, 0);
@ -252,13 +253,14 @@ static void processGhst(void)
void initGhstTelemetry(void) void initGhstTelemetry(void)
{ {
// If the GHST Rx driver is active, since tx and rx share the same pin, assume telemetry is enabled. // If the GHST Rx driver is active, since tx and rx share the same pin, assume telemetry
ghstTelemetryEnabled = ghstRxIsActive(); // can be initialized but not enabled yet.
if (!ghstRxIsActive()) {
if (!ghstTelemetryEnabled) {
return; return;
} }
ghstTelemetryEnabled = false;
int index = 0; int index = 0;
if ((isBatteryVoltageConfigured() && telemetryIsSensorEnabled(SENSOR_VOLTAGE)) if ((isBatteryVoltageConfigured() && telemetryIsSensorEnabled(SENSOR_VOLTAGE))
|| (isAmperageConfigured() && telemetryIsSensorEnabled(SENSOR_CURRENT | SENSOR_FUEL))) { || (isAmperageConfigured() && telemetryIsSensorEnabled(SENSOR_CURRENT | SENSOR_FUEL))) {
@ -285,9 +287,14 @@ void initGhstTelemetry(void)
} }
#endif #endif
ghstScheduleCount = (uint8_t)index; ghstScheduleCount = index;
} }
void setGhstTelemetryState(bool state)
{
ghstTelemetryEnabled = state;
}
bool checkGhstTelemetryState(void) bool checkGhstTelemetryState(void)
{ {
return ghstTelemetryEnabled; return ghstTelemetryEnabled;
@ -296,7 +303,7 @@ bool checkGhstTelemetryState(void)
// Called periodically by the scheduler // Called periodically by the scheduler
void handleGhstTelemetry(timeUs_t currentTimeUs) void handleGhstTelemetry(timeUs_t currentTimeUs)
{ {
static uint32_t ghstLastCycleTime; static timeUs_t ghstLastCycleTime;
if (!ghstTelemetryEnabled) { if (!ghstTelemetryEnabled) {
return; return;

2
src/main/telemetry/ghst.h
View file

@ -29,5 +29,5 @@
void initGhstTelemetry(void); void initGhstTelemetry(void);
bool checkGhstTelemetryState(void); bool checkGhstTelemetryState(void);
void setGhstTelemetryState(bool state);
void handleGhstTelemetry(timeUs_t currentTimeUs); void handleGhstTelemetry(timeUs_t currentTimeUs);