diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 0bd201a9fb..4c12f18d72 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -375,6 +375,9 @@ main_sources(COMMON_SRC rx/fport.h rx/fport2.c rx/fport2.h + rx/ghst_protocol.h + rx/ghst.c + rx/ghst.h rx/ibus.c rx/ibus.h rx/jetiexbus.c @@ -564,6 +567,8 @@ main_sources(COMMON_SRC telemetry/frsky.h telemetry/frsky_d.c telemetry/frsky_d.h + telemetry/ghst.c + telemetry/ghst.h telemetry/hott.c telemetry/hott.h telemetry/ibus_shared.c diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 62c5496df7..91b40205d6 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -25,7 +25,7 @@ tables: values: ["NONE", "PPM", "SERIAL", "MSP", "SPI", "UIB"] enum: rxReceiverType_e - name: serial_rx - values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST", "FPORT2", "SRXL2"] + values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST", "FPORT2", "SRXL2", "GHST"] - name: rx_spi_protocol values: ["V202_250K", "V202_1M", "SYMA_X", "SYMA_X5C", "CX10", "CX10A", "H8_3D", "INAV", "ELERES"] enum: rx_spi_protocol_e diff --git a/src/main/rx/ghst.c b/src/main/rx/ghst.c new file mode 100644 index 0000000000..26bdfd27c4 --- /dev/null +++ b/src/main/rx/ghst.c @@ -0,0 +1,317 @@ +/* + * 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 "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 bool shouldSendTelemetryFrame(void) +{ + const timeUs_t now = micros(); + const timeUs_t timeSinceRxFrameEndUs = cmpTimeUs(now, ghstRxFrameEndAtUs); + return telemetryBufLen > 0 && timeSinceRxFrameEndUs > GHST_RX_TO_TELEMETRY_MIN_US && timeSinceRxFrameEndUs < GHST_RX_TO_TELEMETRY_MAX_US; +} + +static void ghstIdle(void) +{ + if (ghstTransmittingTelemetry) { + ghstTransmittingTelemetry = false; + } +} + +uint8_t ghstFrameStatus(rxRuntimeConfig_t *rxRuntimeState) +{ + UNUSED(rxRuntimeState); + + if(serialIsIdle(serialPort)) { + ghstIdle(); + } + + 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 + } + + if (shouldSendTelemetryFrame()) { + return RX_FRAME_PROCESSING_REQUIRED; + } + + return RX_FRAME_PENDING; +} + +static bool ghstProcessFrame(const rxRuntimeConfig_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); + + // do we have a telemetry buffer to send? + if (shouldSendTelemetryFrame()) { + 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: + return true; + break; + } + } + + return false; +} + +STATIC_UNIT_TESTED uint16_t ghstReadRawRC(const rxRuntimeConfig_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; +// } + +bool ghstRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeState) +{ + for (int iChan = 0; iChan < GHST_MAX_NUM_CHANNELS; ++iChan) { + ghstChannelData[iChan] = (16 * PWM_RANGE_MIDDLE) / 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) + ); + + 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..43eed4db62 --- /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 rxConfig_t *rxConfig, rxRuntimeConfig_t *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..aded09dfb0 --- /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 fa24d6ceb9..1addc6868d 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -66,6 +66,7 @@ #include "rx/sumh.h" #include "rx/uib_rx.h" #include "rx/xbus.h" +#include "rx/ghst.h" //#define DEBUG_RX_SIGNAL_LOSS @@ -250,6 +251,11 @@ bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig case SERIALRX_FPORT2: enabled = fport2RxInit(rxConfig, rxRuntimeConfig); break; +#endif +#ifdef USE_SERIALRX_GHST + case SERIALRX_GHST: + enabled = ghstRxInit(rxConfig, rxRuntimeConfig); + break; #endif default: enabled = false; diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index f16a0c0f7f..fed6db7cd4 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -83,6 +83,7 @@ typedef enum { SERIALRX_SBUS_FAST = 11, SERIALRX_FPORT2 = 12, SERIALRX_SRXL2 = 13, + SERIALRX_GHST = 14, } rxSerialReceiverType_e; #define MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT 16 diff --git a/src/main/target/common.h b/src/main/target/common.h index c403b78ecd..baedd67253 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -135,6 +135,9 @@ #define USE_VTX_COMMON +#define USE_SERIALRX_GHST +#define USE_TELEMETRY_GHST + #else // FLASH_SIZE < 256 #define LOG_LEVEL_MAXIMUM LOG_LEVEL_ERROR #endif diff --git a/src/main/telemetry/ghst.c b/src/main/telemetry/ghst.c new file mode 100644 index 0000000000..121f239446 --- /dev/null +++ b/src/main/telemetry/ghst.c @@ -0,0 +1,181 @@ +/* + * 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 "config/parameter_group_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 "fc/rc_modes.h" +#include "fc/runtime_config.h" + +#include "flight/imu.h" + +#include "io/gps.h" +#include "io/serial.h" + +#include "rx/rx.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" + +#include "build/debug.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() || isAmperageConfigured()) { + 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 5022a0192e..98531e65e3 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -50,6 +50,7 @@ #include "telemetry/crsf.h" #include "telemetry/srxl.h" #include "telemetry/sim.h" +#include "telemetry/ghst.h" PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 4); @@ -129,6 +130,10 @@ void telemetryInit(void) initSrxlTelemetry(); #endif +#ifdef USE_TELEMETRY_GHST + initGhstTelemetry(); +#endif + telemetryCheckState(); } @@ -194,6 +199,9 @@ void telemetryCheckState(void) #ifdef USE_TELEMETRY_SRXL checkSrxlTelemetryState(); #endif +#ifdef USE_TELEMETRY_GHST + checkGhstTelemetryState(); +#endif } void telemetryProcess(timeUs_t currentTimeUs) @@ -239,6 +247,9 @@ void telemetryProcess(timeUs_t currentTimeUs) #ifdef USE_TELEMETRY_SRXL handleSrxlTelemetry(currentTimeUs); #endif +#ifdef USE_TELEMETRY_GHST + handleGhstTelemetry(currentTimeUs); +#endif } #endif