From 5bf4f71a6d4e26c36d434be835d2611801b7da2d Mon Sep 17 00:00:00 2001 From: Unknown Date: Sat, 26 Sep 2020 10:43:33 +0200 Subject: [PATCH] Initial install of GHST driver First pass at GHST driver including low-latency control, and basic telemetry. Fix Ghost channel scaling Use ghstChannelData instead of rxRuntimeState->channelData Integrate PR feedback Primarily de-tab and a few cosmetic changes. Keep Travis CI happy Removed unused function Rework ghost driver to reduce time in ISR, move processing As requested in PR review. Fixed issue in telemetry driver, mAh consumed is transmitted as units of 10mAh, not 1mAh. Resolve Packet Collision Issue with GHST Send telemetry packets only within a well defined time slot after an incoming Rx packet. Remove unnecessary comment rxRefreshRate doesn't need to be dynamic. Ghost - Remove special case from scheduler No need to reschedule telemetry for the Ghost protocol. --- make/mcu/SITL.mk | 1 + make/source.mk | 2 + src/main/cli/settings.c | 1 + src/main/fc/tasks.c | 2 +- src/main/rx/ghst.c | 311 ++++++++++++++++++++++++++ src/main/rx/ghst.h | 33 +++ src/main/rx/ghst_protocol.h | 95 ++++++++ src/main/rx/rx.c | 6 + src/main/rx/rx.h | 1 + src/main/target/CRAZYBEEF3FR/target.h | 2 + src/main/target/MIDELICF3/target.h | 1 + src/main/target/SITL/target.h | 2 + src/main/target/common_post.h | 5 + src/main/target/common_pre.h | 2 + src/main/telemetry/ghst.c | 182 +++++++++++++++ src/main/telemetry/ghst.h | 33 +++ src/main/telemetry/telemetry.c | 10 + 17 files changed, 688 insertions(+), 1 deletion(-) create mode 100644 src/main/rx/ghst.c create mode 100644 src/main/rx/ghst.h create mode 100644 src/main/rx/ghst_protocol.h create mode 100644 src/main/telemetry/ghst.c create mode 100644 src/main/telemetry/ghst.h 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