mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-24 16:55:29 +03:00
GHST RX protocol
This commit is contained in:
Pawel Spychalski (DzikuVx)
parent
02e1fb8d13
commit
3b4bd8be52
11 changed files with 686 additions and 1 deletions
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
317
src/main/rx/ghst.c
Normal file
317
src/main/rx/ghst.c
Normal file
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#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><Len><Type><Payload><CRC>
|
||||
*
|
||||
* 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
|
||||
33
src/main/rx/ghst.h
Normal file
33
src/main/rx/ghst.h
Normal file
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#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);
|
||||
95
src/main/rx/ghst_protocol.h
Normal file
95
src/main/rx/ghst_protocol.h
Normal file
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
#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;
|
||||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
181
src/main/telemetry/ghst.c
Normal file
181
src/main/telemetry/ghst.c
Normal file
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
|
||||
#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
|
||||
33
src/main/telemetry/ghst.h
Normal file
33
src/main/telemetry/ghst.h
Normal file
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "common/time.h"
|
||||
|
||||
#include "rx/ghst_protocol.h"
|
||||
|
||||
void initGhstTelemetry(void);
|
||||
bool checkGhstTelemetryState(void);
|
||||
void handleGhstTelemetry(timeUs_t currentTimeUs);
|
||||
|
||||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue