diff --git a/make/mcu/SITL.mk b/make/mcu/SITL.mk index 2cbd691a5a..32ad3f445a 100644 --- a/make/mcu/SITL.mk +++ b/make/mcu/SITL.mk @@ -35,6 +35,7 @@ MCU_EXCLUDES = \ drivers/rx/rx_xn297.c \ drivers/display_ug2864hsweg01.c \ telemetry/crsf.c \ + telemetry/ghst.c \ telemetry/srxl.c \ io/displayport_oled.c diff --git a/make/source.mk b/make/source.mk index 926a4a15a4..28a93c5b6a 100644 --- a/make/source.mk +++ b/make/source.mk @@ -115,6 +115,7 @@ COMMON_SRC = \ rx/rx_spi.c \ rx/rx_spi_common.c \ rx/crsf.c \ + rx/ghst.c \ rx/sbus.c \ rx/sbus_channels.c \ rx/spektrum.c \ @@ -178,6 +179,7 @@ COMMON_SRC = \ sensors/rangefinder.c \ telemetry/telemetry.c \ telemetry/crsf.c \ + telemetry/ghst.c \ telemetry/srxl.c \ telemetry/frsky_hub.c \ telemetry/hott.c \ diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index fe833fab43..ec36308538 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -239,6 +239,7 @@ static const char * const lookupTableSerialRX[] = { "CUSTOM", "FPORT", "SRXL2", + "GHST", }; #endif diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c index 6794d236d0..58715a7e93 100644 --- a/src/main/fc/tasks.c +++ b/src/main/fc/tasks.c @@ -322,7 +322,7 @@ void tasksInit(void) } else if (rxRuntimeState.serialrxProvider == SERIALRX_CRSF) { // Reschedule telemetry to 500hz, 2ms for CRSF rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(500)); - } + } } #endif diff --git a/src/main/rx/ghst.c b/src/main/rx/ghst.c new file mode 100644 index 0000000000..16fd647d2d --- /dev/null +++ b/src/main/rx/ghst.c @@ -0,0 +1,311 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include +#include +#include + +#include "platform.h" + +#ifdef USE_SERIALRX_GHST + +#include "build/build_config.h" +#include "build/debug.h" + +#include "common/crc.h" +#include "common/maths.h" +#include "common/utils.h" + +#include "pg/rx.h" + +#include "drivers/serial.h" +#include "drivers/serial_uart.h" +#include "drivers/system.h" +#include "drivers/time.h" + +#include "io/serial.h" + +#include "rx/rx.h" +#include "rx/ghst.h" + +#include "telemetry/ghst.h" + +#define GHST_PORT_OPTIONS (SERIAL_STOPBITS_1 | SERIAL_PARITY_NO | SERIAL_BIDIR | SERIAL_BIDIR_PP) +#define GHST_PORT_MODE MODE_RXTX // bidirectional on single pin + +#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 the time window after the end of the last received packet where telemetry packets may be sent +// NOTE: This allows the Rx to double-up on Rx packets to transmit data other than servo data, but +// only if sent < 1ms after the servo data packet. +#define GHST_RX_TO_TELEMETRY_MIN_US 1000 +#define GHST_RX_TO_TELEMETRY_MAX_US 2000 + +#define GHST_PAYLOAD_OFFSET offsetof(ghstFrameDef_t, type) + +STATIC_UNIT_TESTED volatile bool ghstFrameAvailable = false; +STATIC_UNIT_TESTED volatile bool ghstValidatedFrameAvailable = 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 ghstValidatedFrame; // validated frame, CRC is ok, destination address is ok, ready for decode + +STATIC_UNIT_TESTED uint32_t ghstChannelData[GHST_MAX_NUM_CHANNELS]; + +static serialPort_t *serialPort; +static timeUs_t ghstRxFrameStartAtUs = 0; +static timeUs_t ghstRxFrameEndAtUs = 0; +static uint8_t telemetryBuf[GHST_FRAME_SIZE_MAX]; +static uint8_t telemetryBufLen = 0; + +static timeUs_t lastRcFrameTimeUs = 0; + +/* GHST Protocol + * Ghost uses 420k baud single-wire, half duplex connection, connected to a FC UART 'Tx' pin + * Each control packet is interleaved with one or more corresponding downlink packets + * + * Uplink packet format (Control packets) + * + * + * Addr: u8 Destination address + * Len u8 Length includes the packet ID, but not the CRC + * CRC u8 + * + * Ghost packets are designed to be as short as possible, for minimum latency. + * + * Note that the GHST protocol does not handle, itself, failsafe conditions. Packets are passed from + * 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. + * + */ + +#define GHST_FRAME_LENGTH_ADDRESS 1 +#define GHST_FRAME_LENGTH_FRAMELENGTH 1 +#define GHST_FRAME_LENGTH_TYPE_CRC 1 + +// called from telemetry/ghst.c +void ghstRxWriteTelemetryData(const void *data, int len) +{ + len = MIN(len, (int)sizeof(telemetryBuf)); + memcpy(telemetryBuf, data, len); + telemetryBufLen = len; +} + +void ghstRxSendTelemetryData(void) +{ + // if there is telemetry data to write + if (telemetryBufLen > 0) { + serialWriteBuf(serialPort, telemetryBuf, telemetryBufLen); + telemetryBufLen = 0; // reset telemetry buffer + } +} + +STATIC_UNIT_TESTED uint8_t ghstFrameCRC(ghstFrame_t *pGhstFrame) +{ + // CRC includes type and payload + 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) { + crc = crc8_dvb_s2(crc, pGhstFrame->frame.payload[i]); + } + return crc; +} + +// Receive ISR callback, called back from serial port +STATIC_UNIT_TESTED void ghstDataReceive(uint16_t c, void *data) +{ + UNUSED(data); + + static uint8_t ghstFrameIdx = 0; + const timeUs_t currentTimeUs = microsISR(); + + if (cmpTimeUs(currentTimeUs, ghstRxFrameStartAtUs) > GHST_MAX_FRAME_TIME_US) { + // Character received after the max. frame time, assume that this is a new frame + ghstFrameIdx = 0; + } + + if (ghstFrameIdx == 0) { + // timestamp the start of the frame, to allow us to detect frame sync issues + ghstRxFrameStartAtUs = currentTimeUs; + } + + // 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 + const int fullFrameLength = ghstFrameIdx < 3 ? 5 : ghstIncomingFrame.frame.len + GHST_FRAME_LENGTH_ADDRESS + GHST_FRAME_LENGTH_FRAMELENGTH; + + if (ghstFrameIdx < fullFrameLength) { + ghstIncomingFrame.bytes[ghstFrameIdx++] = (uint8_t)c; + if (ghstFrameIdx >= fullFrameLength) { + ghstFrameIdx = 0; + + // NOTE: this data is not yet CRC checked, nor do we know whether we are the correct recipient, this is + // 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 + ghstRxFrameEndAtUs = microsISR(); + } + } +} +STATIC_UNIT_TESTED uint8_t ghstFrameStatus(rxRuntimeState_t *rxRuntimeState) +{ + UNUSED(rxRuntimeState); + + if (ghstFrameAvailable) { + ghstFrameAvailable = false; + + const uint8_t crc = ghstFrameCRC(&ghstValidatedFrame); + 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) { + ghstValidatedFrameAvailable = true; + return RX_FRAME_COMPLETE | RX_FRAME_PROCESSING_REQUIRED; // request callback through ghstProcessFrame to do the decoding work + } + + return RX_FRAME_DROPPED; // frame was invalid + } + + // do we have a telemetry buffer to send? + if (telemetryBufLen > 0) { + return RX_FRAME_PROCESSING_REQUIRED; + } + + return RX_FRAME_PENDING; +} + +static bool ghstProcessFrame(const rxRuntimeState_t *rxRuntimeState) +{ + // Assume that the only way we get here is if ghstFrameStatus returned RX_FRAME_PROCESSING_REQUIRED, which indicates that the CRC + // is correct, and the message was actually for us. + + UNUSED(rxRuntimeState); + + // if we are in the time window after receiving a packet, where we can send telemetry, send it now + const uint32_t now = micros(); + const uint32_t timeSinceRxFrameEnd = cmpTimeUs(now, ghstRxFrameEndAtUs); + if (timeSinceRxFrameEnd > GHST_RX_TO_TELEMETRY_MIN_US && timeSinceRxFrameEnd < GHST_RX_TO_TELEMETRY_MAX_US) { + ghstTransmittingTelemetry = true; + ghstRxSendTelemetryData(); + } + + if(ghstValidatedFrameAvailable) { + int startIdx = 4; + switch (ghstValidatedFrame.frame.type) { + case GHST_UL_RC_CHANS_HS4_5TO8: + case GHST_UL_RC_CHANS_HS4_9TO12: + case GHST_UL_RC_CHANS_HS4_13TO16: { + const ghstPayloadPulses_t* const rcChannels = (ghstPayloadPulses_t*)&ghstValidatedFrame.frame.payload; + + // all uplink frames contain CH1..4 data (12 bit) + ghstChannelData[0] = rcChannels->ch1 >> 1; + ghstChannelData[1] = rcChannels->ch2 >> 1; + ghstChannelData[2] = rcChannels->ch3 >> 1; + ghstChannelData[3] = rcChannels->ch4 >> 1; + + // remainder of uplink frame contains 4 more channels (8 bit), sent in a round-robin fashion + switch(ghstValidatedFrame.frame.type) { + case GHST_UL_RC_CHANS_HS4_5TO8: startIdx = 4; break; + case GHST_UL_RC_CHANS_HS4_9TO12: startIdx = 8; break; + case GHST_UL_RC_CHANS_HS4_13TO16: startIdx = 12; break; + } + + ghstChannelData[startIdx++] = rcChannels->cha << 3; + ghstChannelData[startIdx++] = rcChannels->chb << 3; + ghstChannelData[startIdx++] = rcChannels->chc << 3; + ghstChannelData[startIdx++] = rcChannels->chd << 3; + return true; + } + break; + + default: + break; + } + } + + return false; +} + +STATIC_UNIT_TESTED uint16_t ghstReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan) +{ + UNUSED(rxRuntimeState); + + // derived from original SBus scaling, with slight correction for offset (now symmetrical + // around OpenTx 0 value) + // scaling is: + // OpenTx RC PWM + // min -1024 172 988us + // ctr 0 992 1500us + // max 1024 1811 2012us + // + + return (5 * (ghstChannelData[chan]+1) / 8) + 880; +} + +static timeUs_t ghstFrameTimeUs(void) +{ + return lastRcFrameTimeUs; +} + +// UART idle detected (inter-packet) +static void ghstIdle() +{ + if (ghstTransmittingTelemetry) { + ghstTransmittingTelemetry = false; + } +} + +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->rxRefreshRate = GHST_TIME_BETWEEN_FRAMES_US; + + rxRuntimeState->rcReadRawFn = ghstReadRawRC; + rxRuntimeState->rcFrameStatusFn = ghstFrameStatus; + rxRuntimeState->rcFrameTimeUsFn = ghstFrameTimeUs; + rxRuntimeState->rcProcessFrameFn = ghstProcessFrame; + + const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); + if (!portConfig) { + return false; + } + + serialPort = openSerialPort(portConfig->identifier, + FUNCTION_RX_SERIAL, + ghstDataReceive, + NULL, + GHST_RX_BAUDRATE, + GHST_PORT_MODE, + GHST_PORT_OPTIONS | (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0) + ); + serialPort->idleCallback = ghstIdle; + + return serialPort != NULL; +} + +bool ghstRxIsActive(void) +{ + return serialPort != NULL; +} +#endif diff --git a/src/main/rx/ghst.h b/src/main/rx/ghst.h new file mode 100644 index 0000000000..360b1ec659 --- /dev/null +++ b/src/main/rx/ghst.h @@ -0,0 +1,33 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#include "rx/ghst_protocol.h" + +#define GHST_MAX_NUM_CHANNELS 16 + +void ghstRxWriteTelemetryData(const void *data, int len); +void ghstRxSendTelemetryData(void); + +struct rxConfig_s; +struct rxRuntimeState_s; +bool ghstRxInit(const struct rxConfig_s *initialRxConfig, struct rxRuntimeState_s *rxRuntimeState); +bool ghstRxIsActive(void); diff --git a/src/main/rx/ghst_protocol.h b/src/main/rx/ghst_protocol.h new file mode 100644 index 0000000000..62783f2ec8 --- /dev/null +++ b/src/main/rx/ghst_protocol.h @@ -0,0 +1,95 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#include +#include + +#define GHST_RX_BAUDRATE 420000 + +#define GHST_TX_BAUDRATE_FAST 400000 +#define GHST_TX_BAUDRATE_SLOW 115200 +#define GHST_BYTE_TIME_FAST_US ((1000000/GHST_TX_BAUDRATE_FAST)*10) // 10 bit words (8 data, 1 start, 1 stop) +#define GHST_BYTE_TIME_SLOW_US ((1000000/GHST_TX_BAUDRATE_SLOW)*10) +#define GHST_UART_WORDLENGTH UART_WORDLENGTH_8B + +typedef enum { + GHST_ADDR_RADIO = 0x80, + GHST_ADDR_TX_MODULE_SYM = 0x81, // symmetrical, 400k pulses, 400k telemetry + GHST_ADDR_TX_MODULE_ASYM = 0x88, // asymmetrical, 400k pulses, 115k telemetry + GHST_ADDR_FC = 0x82, + GHST_ADDR_GOGGLES = 0x83, + GHST_ADDR_QUANTUM_TEE1 = 0x84, // phase 2 + GHST_ADDR_QUANTUM_TEE2 = 0x85, + GHST_ADDR_QUANTUM_GW1 = 0x86, + GHST_ADDR_5G_CLK = 0x87, // phase 3 + GHST_ADDR_RX = 0x89 +} ghstAddr_e; + +typedef enum { + GHST_UL_RC_CHANS_HS4_5TO8 = 0x10, // High Speed 4 channel, plus CH5-8 + GHST_UL_RC_CHANS_HS4_9TO12 = 0x11, // High Speed 4 channel, plus CH9-12 + GHST_UL_RC_CHANS_HS4_13TO16 = 0x12 // High Speed 4 channel, plus CH13-16 +} ghstUl_e; + +#define GHST_UL_RC_CHANS_SIZE 12 // 1 (type) + 10 (data) + 1 (crc) + +typedef enum { + GHST_DL_OPENTX_SYNC = 0x20, + GHST_DL_LINK_STAT = 0x21, + GHST_DL_VTX_STAT = 0x22, + GHST_DL_PACK_STAT = 0x23, // Battery (Pack) Status +} ghstDl_e; + +#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_FRAME_SIZE 14 // including addr, type, len, crc, and payload + +#define GHST_PAYLOAD_SIZE_MAX 14 + +#define GHST_FRAME_SIZE_MAX 24 + +typedef struct ghstFrameDef_s { + uint8_t addr; + uint8_t len; + uint8_t type; + uint8_t payload[GHST_PAYLOAD_SIZE_MAX + 1]; // CRC adds 1 +} ghstFrameDef_t; + +typedef union ghstFrame_u { + uint8_t bytes[GHST_FRAME_SIZE]; + ghstFrameDef_t frame; +} ghstFrame_t; + +/* Pulses payload (channel data). Includes 4x high speed control channels, plus 4 channels from CH5-CH12 */ +typedef struct ghstPayloadPulses_s { + // 80 bits, or 10 bytes + unsigned int ch1: 12; + unsigned int ch2: 12; + unsigned int ch3: 12; + unsigned int ch4: 12; + + unsigned int cha: 8; + unsigned int chb: 8; + unsigned int chc: 8; + unsigned int chd: 8; +} __attribute__ ((__packed__)) ghstPayloadPulses_t; diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 31438d821c..2a33c77f12 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -65,6 +65,7 @@ #include "rx/ibus.h" #include "rx/jetiexbus.h" #include "rx/crsf.h" +#include "rx/ghst.h" #include "rx/rx_spi.h" #include "rx/targetcustomserial.h" #include "rx/msp_override.h" @@ -233,6 +234,11 @@ static bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntime enabled = crsfRxInit(rxConfig, rxRuntimeState); break; #endif +#ifdef USE_SERIALRX_GHST + case SERIALRX_GHST: + enabled = ghstRxInit(rxConfig, rxRuntimeState); + break; +#endif #ifdef USE_SERIALRX_TARGET_CUSTOM case SERIALRX_TARGET_CUSTOM: enabled = targetCustomSerialRxInit(rxConfig, rxRuntimeState); diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index c697991b2f..da5d400378 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -67,6 +67,7 @@ typedef enum { SERIALRX_TARGET_CUSTOM = 11, SERIALRX_FPORT = 12, SERIALRX_SRXL2 = 13, + SERIALRX_GHST = 14 } SerialRXType; #define MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT 12 diff --git a/src/main/target/CRAZYBEEF3FR/target.h b/src/main/target/CRAZYBEEF3FR/target.h index be0c37cb6b..aadf330f87 100644 --- a/src/main/target/CRAZYBEEF3FR/target.h +++ b/src/main/target/CRAZYBEEF3FR/target.h @@ -32,10 +32,12 @@ #endif #undef USE_SERIALRX_CRSF +#undef USE_SERIALRX_GHST #undef USE_SERIALRX_SUMD #undef USE_SERIALRX_SUMH #undef USE_SERIALRX_XBUS #undef USE_TELEMETRY_CRSF +#undef USE_TELEMETRY_GHST #undef USE_TELEMETRY_MAVLINK #undef USE_PWM diff --git a/src/main/target/MIDELICF3/target.h b/src/main/target/MIDELICF3/target.h index 5452eaa645..988116ddc3 100644 --- a/src/main/target/MIDELICF3/target.h +++ b/src/main/target/MIDELICF3/target.h @@ -26,6 +26,7 @@ #undef USE_SERIAL_RX #undef USE_CRSF_CMS_TELEMETRY #undef USE_TELEMETRY_CRSF +#undef USE_TELEMETRY_GHST #undef USE_TELEMETRY_HOTT #undef USE_TELEMETRY_IBUS #undef USE_TELEMETRY_IBUS_EXTENDED diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index dd69ec3c8b..34978b6363 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -100,6 +100,7 @@ #undef USE_PWM #undef USE_SERIAL_RX #undef USE_SERIALRX_CRSF +#undef USE_SERIALRX_GHST #undef USE_SERIALRX_IBUS #undef USE_SERIALRX_SBUS #undef USE_SERIALRX_SPEKTRUM @@ -114,6 +115,7 @@ #undef USE_RESOURCE_MGMT #undef USE_CMS #undef USE_TELEMETRY_CRSF +#undef USE_TELEMETRY_GHST #undef USE_TELEMETRY_IBUS #undef USE_TELEMETRY_JETIEXBUS #undef USE_TELEMETRY_SRXL diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index 1282e7664c..99ca620ae2 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -92,6 +92,7 @@ #if !defined(USE_TELEMETRY) #undef USE_CRSF_CMS_TELEMETRY #undef USE_TELEMETRY_CRSF +#undef USE_TELEMETRY_GHST #undef USE_TELEMETRY_FRSKY_HUB #undef USE_TELEMETRY_HOTT #undef USE_TELEMETRY_IBUS @@ -110,6 +111,10 @@ #undef USE_RX_RSSI_DBM #endif +#if !defined(USE_SERIALRX_GHST) +#undef USE_TELEMETRY_GHST +#endif + #if !defined(USE_TELEMETRY_CRSF) || !defined(USE_CMS) #undef USE_CRSF_CMS_TELEMETRY #endif diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h index c91af01625..f91d6b6fb2 100644 --- a/src/main/target/common_pre.h +++ b/src/main/target/common_pre.h @@ -234,6 +234,7 @@ #define USE_PPM #define USE_SERIAL_RX #define USE_SERIALRX_CRSF // Team Black Sheep Crossfire protocol +#define USE_SERIALRX_GHST // ImmersionRC Ghost Protocol #define USE_SERIALRX_IBUS // FlySky and Turnigy receivers #define USE_SERIALRX_SBUS // Frsky and Futaba receivers #define USE_SERIALRX_SPEKTRUM // SRXL, DSM2 and DSMX protocol @@ -265,6 +266,7 @@ #define USE_DSHOT_DMAR #define USE_SERIALRX_FPORT // FrSky FPort #define USE_TELEMETRY_CRSF +#define USE_TELEMETRY_GHST #define USE_TELEMETRY_SRXL #if ((TARGET_FLASH_SIZE > 256) || (FEATURE_CUT_LEVEL < 12)) diff --git a/src/main/telemetry/ghst.c b/src/main/telemetry/ghst.c new file mode 100644 index 0000000000..2169359467 --- /dev/null +++ b/src/main/telemetry/ghst.c @@ -0,0 +1,182 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#ifdef USE_TELEMETRY_GHST + +#include "build/atomic.h" +#include "build/build_config.h" +#include "build/version.h" + +#include "config/feature.h" +#include "pg/pg.h" +#include "pg/pg_ids.h" + +#include "common/crc.h" +#include "common/maths.h" +#include "common/printf.h" +#include "common/streambuf.h" +#include "common/utils.h" + +#include "cms/cms.h" + +#include "drivers/nvic.h" + +#include "config/config.h" +#include "fc/rc_modes.h" +#include "fc/runtime_config.h" + +#include "flight/imu.h" +#include "flight/position.h" + +#include "io/gps.h" +#include "io/serial.h" + +#include "rx/ghst.h" +#include "rx/ghst_protocol.h" + +#include "sensors/battery.h" +#include "sensors/sensors.h" + +#include "telemetry/telemetry.h" +#include "telemetry/msp_shared.h" + +#include "telemetry/ghst.h" + +#define GHST_CYCLETIME_US 100000 // 10x/sec +#define GHST_FRAME_PACK_PAYLOAD_SIZE 10 +#define GHST_FRAME_LENGTH_CRC 1 +#define GHST_FRAME_LENGTH_TYPE 1 + +static bool ghstTelemetryEnabled; +static uint8_t ghstFrame[GHST_FRAME_SIZE_MAX]; + +static void ghstInitializeFrame(sbuf_t *dst) +{ + dst->ptr = ghstFrame; + dst->end = ARRAYEND(ghstFrame); + + sbufWriteU8(dst, GHST_ADDR_RX); +} + +static void ghstFinalize(sbuf_t *dst) +{ + crc8_dvb_s2_sbuf_append(dst, &ghstFrame[2]); // start at byte 2, since CRC does not include device address and frame length + sbufSwitchToReader(dst, ghstFrame); + // write the telemetry frame to the receiver. + ghstRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst)); +} + +// Battery (Pack) status +void ghstFramePackTelemetry(sbuf_t *dst) +{ + // 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, 0x23); // GHST_DL_PACK_STAT + + if (telemetryConfig()->report_cell_voltage) { + sbufWriteU16(dst, getBatteryAverageCellVoltage()); // units of 10mV + } else { + sbufWriteU16(dst, getBatteryVoltage()); + } + sbufWriteU16(dst, getAmperage()); // units of 10mA + + sbufWriteU16(dst, getMAhDrawn() / 10); // units of 10mAh (range of 0-655.36Ah) + + sbufWriteU8(dst, 0x00); // Rx Voltage, units of 100mV (not passed from BF, added in Ghost Rx) + + sbufWriteU8(dst, 0x00); // tbd1 + sbufWriteU8(dst, 0x00); // tbd2 + sbufWriteU8(dst, 0x00); // tbd3 +} + +// 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_SCHEDULE_COUNT_MAX +} ghstFrameTypeIndex_e; + +static uint8_t ghstScheduleCount; +static uint8_t ghstSchedule[GHST_SCHEDULE_COUNT_MAX]; + +static void processGhst(void) +{ + static uint8_t ghstScheduleIndex = 0; + + const uint8_t currentSchedule = ghstSchedule[ghstScheduleIndex]; + + sbuf_t ghstPayloadBuf; + sbuf_t *dst = &ghstPayloadBuf; + + if (currentSchedule & BIT(GHST_FRAME_PACK_INDEX)) { + ghstInitializeFrame(dst); + ghstFramePackTelemetry(dst); + ghstFinalize(dst); + } + ghstScheduleIndex = (ghstScheduleIndex + 1) % ghstScheduleCount; +} + +void initGhstTelemetry(void) +{ + // If the GHST Rx driver is active, since tx and rx share the same pin, assume telemetry is enabled. + ghstTelemetryEnabled = ghstRxIsActive(); + + if (!ghstTelemetryEnabled) { + return; + } + + int index = 0; + if ((isBatteryVoltageConfigured() && telemetryIsSensorEnabled(SENSOR_VOLTAGE)) + || (isAmperageConfigured() && telemetryIsSensorEnabled(SENSOR_CURRENT | SENSOR_FUEL))) { + ghstSchedule[index++] = BIT(GHST_FRAME_PACK_INDEX); + } + ghstScheduleCount = (uint8_t)index; + } + +bool checkGhstTelemetryState(void) +{ + return ghstTelemetryEnabled; +} + +// Called periodically by the scheduler + void handleGhstTelemetry(timeUs_t currentTimeUs) +{ + static uint32_t ghstLastCycleTime; + + if (!ghstTelemetryEnabled) { + return; + } + + // Ready to send telemetry? + if (currentTimeUs >= ghstLastCycleTime + (GHST_CYCLETIME_US / ghstScheduleCount)) { + ghstLastCycleTime = currentTimeUs; + processGhst(); + } + + // telemetry is sent from the Rx driver, ghstProcessFrame +} + +#endif diff --git a/src/main/telemetry/ghst.h b/src/main/telemetry/ghst.h new file mode 100644 index 0000000000..8c8a338542 --- /dev/null +++ b/src/main/telemetry/ghst.h @@ -0,0 +1,33 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#include +#include + +#include "common/time.h" + +#include "rx/ghst_protocol.h" + +void initGhstTelemetry(void); +bool checkGhstTelemetryState(void); +void handleGhstTelemetry(timeUs_t currentTimeUs); + diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 7afba90a2f..7a713996fe 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -55,6 +55,7 @@ #include "telemetry/jetiexbus.h" #include "telemetry/mavlink.h" #include "telemetry/crsf.h" +#include "telemetry/ghst.h" #include "telemetry/srxl.h" #include "telemetry/ibus.h" #include "telemetry/msp_shared.h" @@ -101,6 +102,9 @@ void telemetryInit(void) #ifdef USE_TELEMETRY_MAVLINK initMAVLinkTelemetry(); #endif +#ifdef USE_TELEMETRY_GHST + initGhstTelemetry(); +#endif #ifdef USE_TELEMETRY_CRSF initCrsfTelemetry(); #if defined(USE_MSP_OVER_TELEMETRY) @@ -184,6 +188,9 @@ void telemetryCheckState(void) #ifdef USE_TELEMETRY_CRSF checkCrsfTelemetryState(); #endif +#ifdef USE_TELEMETRY_GHST + checkGhstTelemetryState(); +#endif #ifdef USE_TELEMETRY_SRXL checkSrxlTelemetryState(); #endif @@ -219,6 +226,9 @@ void telemetryProcess(uint32_t currentTime) #ifdef USE_TELEMETRY_CRSF handleCrsfTelemetry(currentTime); #endif +#ifdef USE_TELEMETRY_GHST + handleGhstTelemetry(currentTime); +#endif #ifdef USE_TELEMETRY_SRXL handleSrxlTelemetry(currentTime); #endif