Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 08:15:26 +03:00
Code Issues Wiki Activity

TBS Crossfire SerialRX protocol and telemetry (grafting from Betaflight)

Browse source 
Inject link quality to virtual channel 17 for CRSF telemetry, change to integer arithmetic for channel value conversion
Rename USE_PWM and USE_PPM to USE_RX_PWM and USE_RX_PPM
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2017-01-10 17:55:54 +10:00
parent a966135eb5
commit 4f250b96c4
22 changed files with 849 additions and 130 deletions
Download patch file Download diff file Expand all files Collapse all files

2
Makefile
View file

@ -543,6 +543,7 @@ COMMON_SRC = \
rx/pwm.c \
rx/rx.c \
rx/rx_spi.c \
rx/crsf.c \
rx/sbus.c \
rx/spektrum.c \
rx/sumd.c \
@ -595,6 +596,7 @@ HIGHEND_SRC = \
sensors/barometer.c \
sensors/pitotmeter.c \
telemetry/telemetry.c \
telemetry/crsf.c \
telemetry/frsky.c \
telemetry/hott.c \
telemetry/smartport.c \

13
src/main/common/maths.c
View file

@ -459,6 +459,19 @@ uint16_t crc16_ccitt(uint16_t crc, unsigned char a)
return crc;
}
uint8_t crc8_dvb_s2(uint8_t crc, unsigned char a)
{
crc ^= a;
for (int ii = 0; ii < 8; ++ii) {
if (crc & 0x80) {
crc = (crc << 1) ^ 0xD5;
} else {
crc = crc << 1;
}
}
return crc;
}
float bellCurve(const float x, const float curveWidth)
{
return powf(M_Ef, -sq(x) / (2.0f * sq(curveWidth)));

1
src/main/common/maths.h
View file

@ -175,5 +175,6 @@ float acos_approx(float x);
void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count);
uint16_t crc16_ccitt(uint16_t crc, unsigned char a);
uint8_t crc8_dvb_s2(uint8_t crc, unsigned char a);
float bellCurve(const float x, const float curveWidth);

6
src/main/drivers/pwm_mapping.c
View file

@ -95,7 +95,7 @@ bool CheckGPIOPinSource(ioTag_t tag, GPIO_TypeDef *gpio, uint16_t pin)
pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init)
{
#ifndef SKIP_RX_PWM_PPM
#if defined(USE_RX_PWM) || defined(USE_RX_PPM)
int channelIndex = 0;
#endif
@ -359,7 +359,7 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init)
#endif
if (type == MAP_TO_PPM_INPUT) {
#ifndef SKIP_RX_PWM_PPM
#if defined(USE_RX_PPM)
#ifdef CC3D_PPM1
if (init->useFastPwm || init->pwmProtocolType == PWM_TYPE_BRUSHED) {
ppmAvoidPWMTimerClash(timerHardwarePtr, TIM4);
@ -377,7 +377,7 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init)
addBootlogEvent6(BOOT_EVENT_TIMER_CH_MAPPED, BOOT_EVENT_FLAGS_NONE, i, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 0);
#endif
} else if (type == MAP_TO_PWM_INPUT) {
#ifndef SKIP_RX_PWM_PPM
#if defined(USE_RX_PWM)
pwmInConfig(timerHardwarePtr, channelIndex);
pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_PWM;
pwmIOConfiguration.pwmInputCount++;

2
src/main/drivers/pwm_rx.c
View file

@ -20,7 +20,7 @@
#include <platform.h>
#ifndef SKIP_RX_PWM_PPM
#if defined(USE_RX_PWM) || defined(USE_RX_PPM)
#include "build/build_config.h"
#include "build/debug.h"

2
src/main/fc/fc_init.c
View file

@ -310,7 +310,7 @@ void init(void)
pwm_params.enablePWMOutput = feature(FEATURE_PWM_OUTPUT_ENABLE);
#ifndef SKIP_RX_PWM_PPM
#if defined(USE_RX_PWM) || defined(USE_RX_PPM)
pwmRxInit(pwmRxConfig());
#endif

2
src/main/fc/fc_msp.c
View file

@ -1266,7 +1266,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break;
case MSP_SET_RAW_RC:
#ifndef SKIP_RX_MSP
#ifdef USE_RX_MSP
{
uint8_t channelCount = dataSize / sizeof(uint16_t);
if (channelCount > MAX_SUPPORTED_RC_CHANNEL_COUNT) {

2
src/main/fc/mw.c
View file

@ -715,7 +715,7 @@ bool taskUpdateRxCheck(timeUs_t currentTimeUs, uint32_t currentDeltaTime)
{
UNUSED(currentDeltaTime);
return updateRx(currentTimeUs);
return rxUpdateCheck(currentTimeUs, currentDeltaTime);
}
void taskUpdateRxMain(timeUs_t currentTimeUs)

4
src/main/fc/serial_cli.c
View file

@ -266,7 +266,9 @@ static const char * const lookupTableSerialRX[] = {
"SUMH",
"XB-B",
"XB-B-RJ01",
"IBUS"
"IBUS",
"JETIEXBUS",
"CRSF"
};
#endif

278
src/main/rx/crsf.c Executable file
View file

@ -0,0 +1,278 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it 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 is distributed in the hope that it 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 Cleanflight. 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_CRSF
#include "build/build_config.h"
#include "build/debug.h"
#include "common/maths.h"
#include "common/utils.h"
#include "drivers/system.h"
#include "drivers/serial.h"
#include "drivers/serial_uart.h"
#include "io/serial.h"
#include "rx/rx.h"
#include "rx/crsf.h"
#define CRSF_TIME_NEEDED_PER_FRAME_US 1000
#define CRSF_TIME_BETWEEN_FRAMES_US 4000 // a frame is sent by the transmitter every 4 milliseconds
#define CRSF_DIGITAL_CHANNEL_MIN 172
#define CRSF_DIGITAL_CHANNEL_MAX 1811
STATIC_UNIT_TESTED bool crsfFrameDone = false;
STATIC_UNIT_TESTED crsfFrame_t crsfFrame;
STATIC_UNIT_TESTED uint32_t crsfChannelData[CRSF_MAX_CHANNEL];
static serialPort_t *serialPort;
static uint32_t crsfFrameStartAt = 0;
static uint8_t telemetryBuf[CRSF_FRAME_SIZE_MAX];
static uint8_t telemetryBufLen = 0;
/*
* CRSF protocol
*
* CRSF protocol uses a single wire half duplex uart connection.
* The master sends one frame every 4ms and the slave replies between two frames from the master.
*
* 420000 baud
* not inverted
* 8 Bit
* 1 Stop bit
* Big endian
* 420000 bit/s = 46667 byte/s (including stop bit) = 21.43us per byte
* Assume a max payload of 32 bytes (needs confirming with TBS), so max frame size of 36 bytes
* A 36 byte frame can be transmitted in 771 microseconds.
*
* CRSF_TIME_NEEDED_PER_FRAME_US is set conservatively at 1000 microseconds
*
* Every frame has the structure:
* <Device address> <Frame length> < Type> <Payload> < CRC>
*
* Device address: (uint8_t)
* Frame length: length in bytes including Type (uint8_t)
* Type: (uint8_t)
* CRC: (uint8_t)
*
*/
struct crsfPayloadRcChannelsPacked_s {
// 176 bits of data (11 bits per channel * 16 channels) = 22 bytes.
unsigned int chan0 : 11;
unsigned int chan1 : 11;
unsigned int chan2 : 11;
unsigned int chan3 : 11;
unsigned int chan4 : 11;
unsigned int chan5 : 11;
unsigned int chan6 : 11;
unsigned int chan7 : 11;
unsigned int chan8 : 11;
unsigned int chan9 : 11;
unsigned int chan10 : 11;
unsigned int chan11 : 11;
unsigned int chan12 : 11;
unsigned int chan13 : 11;
unsigned int chan14 : 11;
unsigned int chan15 : 11;
} __attribute__ ((__packed__));
typedef struct crsfPayloadRcChannelsPacked_s crsfPayloadRcChannelsPacked_t;
struct crsfPayloadLinkStatistics_s {
uint8_t uplinkRSSIAnt1;
uint8_t uplinkRSSIAnt2;
uint8_t uplinkLQ;
int8_t uplinkSNR;
uint8_t activeAntenna;
uint8_t rfMode;
uint8_t uplinkTXPower;
uint8_t downlinkRSSI;
uint8_t downlinkLQ;
int8_t downlinkSNR;
} __attribute__ ((__packed__));
typedef struct crsfPayloadLinkStatistics_s crsfPayloadLinkStatistics_t;
// Receive ISR callback, called back from serial port
STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c)
{
static uint8_t crsfFramePosition = 0;
const uint32_t now = micros();
#ifdef DEBUG_CRSF_PACKETS
debug[2] = now - crsfFrameStartAt;
#endif
if (now > crsfFrameStartAt + CRSF_TIME_NEEDED_PER_FRAME_US) {
// We've received a character after max time needed to complete a frame,
// so this must be the start of a new frame.
crsfFramePosition = 0;
}
if (crsfFramePosition == 0) {
crsfFrameStartAt = now;
}
// 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 = crsfFramePosition < 3 ? 5 : crsfFrame.frame.frameLength + CRSF_FRAME_LENGTH_ADDRESS + CRSF_FRAME_LENGTH_FRAMELENGTH;
if (crsfFramePosition < fullFrameLength) {
crsfFrame.bytes[crsfFramePosition++] = (uint8_t)c;
crsfFrameDone = crsfFramePosition < fullFrameLength ? false : true;
}
}
STATIC_UNIT_TESTED uint8_t crsfFrameCRC(void)
{
// CRC includes type and payload
uint8_t crc = crc8_dvb_s2(0, crsfFrame.frame.type);
for (int ii = 0; ii < crsfFrame.frame.frameLength - CRSF_FRAME_LENGTH_TYPE_CRC; ++ii) {
crc = crc8_dvb_s2(crc, crsfFrame.frame.payload[ii]);
}
return crc;
}
STATIC_UNIT_TESTED uint8_t crsfFrameStatus(void)
{
if (crsfFrameDone) {
crsfFrameDone = false;
if (crsfFrame.frame.type == CRSF_FRAMETYPE_RC_CHANNELS_PACKED) {
// CRC includes type and payload of each frame
const uint8_t crc = crsfFrameCRC();
if (crc != crsfFrame.frame.payload[CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE]) {
return RX_FRAME_PENDING;
}
crsfFrame.frame.frameLength = CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC;
// unpack the RC channels
const crsfPayloadRcChannelsPacked_t* rcChannels = (crsfPayloadRcChannelsPacked_t*)&crsfFrame.frame.payload;
crsfChannelData[0] = rcChannels->chan0;
crsfChannelData[1] = rcChannels->chan1;
crsfChannelData[2] = rcChannels->chan2;
crsfChannelData[3] = rcChannels->chan3;
crsfChannelData[4] = rcChannels->chan4;
crsfChannelData[5] = rcChannels->chan5;
crsfChannelData[6] = rcChannels->chan6;
crsfChannelData[7] = rcChannels->chan7;
crsfChannelData[8] = rcChannels->chan8;
crsfChannelData[9] = rcChannels->chan9;
crsfChannelData[10] = rcChannels->chan10;
crsfChannelData[11] = rcChannels->chan11;
crsfChannelData[12] = rcChannels->chan12;
crsfChannelData[13] = rcChannels->chan13;
crsfChannelData[14] = rcChannels->chan14;
crsfChannelData[15] = rcChannels->chan15;
return RX_FRAME_COMPLETE;
}
else if (crsfFrame.frame.type == CRSF_FRAMETYPE_LINK_STATISTICS) {
// CRC includes type and payload of each frame
const uint8_t crc = crsfFrameCRC();
if (crc != crsfFrame.frame.payload[CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE]) {
return RX_FRAME_PENDING;
}
crsfFrame.frame.frameLength = CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC;
// Inject link quality into channel 17
const crsfPayloadLinkStatistics_t* linkStats = (crsfPayloadLinkStatistics_t*)&crsfFrame.frame.payload;
crsfChannelData[16] = scaleRange(constrain(linkStats->uplinkLQ, 0, 100), 0, 100, 191, 1791); // will map to [1000;2000] range
// This is not RC channels frame, update channel value but don't indicate frame completion
return RX_FRAME_PENDING;
}
}
return RX_FRAME_PENDING;
}
STATIC_UNIT_TESTED uint16_t crsfReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
{
UNUSED(rxRuntimeConfig);
/* conversion from RC value to PWM
* RC PWM
* min 172 -> 988us
* mid 992 -> 1500us
* max 1811 -> 2012us
* scale factor = (2012-988) / (1811-172) = 0.62477120195241
* offset = 988 - 172 * 0.62477120195241 = 880.53935326418548
*/
return (crsfChannelData[chan] * 1024 / 1639) + 881;
}
void crsfRxWriteTelemetryData(const void *data, int len)
{
len = MIN(len, (int)sizeof(telemetryBuf));
memcpy(telemetryBuf, data, len);
telemetryBufLen = len;
}
void crsfRxSendTelemetryData(void)
{
// if there is telemetry data to write
if (telemetryBufLen > 0) {
// check that we are not in bi dir mode or that we are not currently receiving data (ie in the middle of an RX frame)
// and that there is time to send the telemetry frame before the next RX frame arrives
if (CRSF_PORT_OPTIONS & SERIAL_BIDIR) {
const uint32_t timeSinceStartOfFrame = micros() - crsfFrameStartAt;
if ((timeSinceStartOfFrame < CRSF_TIME_NEEDED_PER_FRAME_US) ||
(timeSinceStartOfFrame > CRSF_TIME_BETWEEN_FRAMES_US - CRSF_TIME_NEEDED_PER_FRAME_US)) {
return;
}
}
serialWriteBuf(serialPort, telemetryBuf, telemetryBufLen);
telemetryBufLen = 0; // reset telemetry buffer
}
}
bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{
for (int ii = 0; ii < CRSF_MAX_CHANNEL; ++ii) {
crsfChannelData[ii] = (16 * rxConfig->midrc) / 10 - 1408;
}
rxRuntimeConfig->channelCount = CRSF_MAX_CHANNEL;
rxRuntimeConfig->rxRefreshRate = 11000; //!!TODO this needs checking
rxRuntimeConfig->rcReadRawFn = crsfReadRawRC;
rxRuntimeConfig->rcFrameStatusFn = crsfFrameStatus;
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
if (!portConfig) {
return false;
}
serialPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, crsfDataReceive, CRSF_BAUDRATE, CRSF_PORT_MODE, CRSF_PORT_OPTIONS);
return serialPort != NULL;
}
bool crsfRxIsActive(void)
{
return serialPort != NULL;
}
#endif

84
src/main/rx/crsf.h Executable file
View file

@ -0,0 +1,84 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it 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 is distributed in the hope that it 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#define CRSF_BAUDRATE 420000
#define CRSF_PORT_OPTIONS (SERIAL_STOPBITS_1 | SERIAL_PARITY_NO)
#define CRSF_PORT_MODE MODE_RXTX
#define CRSF_MAX_CHANNEL 17
typedef enum {
CRSF_FRAMETYPE_GPS = 0x02,
CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08,
CRSF_FRAMETYPE_LINK_STATISTICS = 0x14,
CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16,
CRSF_FRAMETYPE_ATTITUDE = 0x1E,
CRSF_FRAMETYPE_FLIGHT_MODE = 0x21
} crsfFrameTypes_e;
enum {
CRSF_FRAME_GPS_PAYLOAD_SIZE = 15,
CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE = 8,
CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE = 10,
CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE = 22, // 11 bits per channel * 16 channels = 22 bytes.
CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE = 6,
CRSF_FRAME_LENGTH_ADDRESS = 1, // length of ADDRESS field
CRSF_FRAME_LENGTH_FRAMELENGTH = 1, // length of FRAMELENGTH field
CRSF_FRAME_LENGTH_TYPE = 1, // length of TYPE field
CRSF_FRAME_LENGTH_CRC = 1, // length of CRC field
CRSF_FRAME_LENGTH_TYPE_CRC = 2 // length of TYPE and CRC fields combined
};
enum {
CRSF_ADDRESS_BROADCAST = 0x00,
CRSF_ADDRESS_TBS_CORE_PNP_PRO = 0x8,
CRSF_ADDRESS_RESERVED1 = 0x8A,
CRSF_ADDRESS_CURRENT_SENSOR = 0xC0,
CRSF_ADDRESS_TBS_BLACKBOX = 0xC4,
CRSF_ADDRESS_COLIBRI_RACE_FC = 0xC8,
CRSF_ADDRESS_RESERVED2 = 0xCA,
CRSF_ADDRESS_RACE_TAG = 0xCC,
CRSF_ADDRESS_RADIO_TRANSMITTER = 0xEA,
CRSF_ADDRESS_CRSF_RECEIVER = 0xEC,
CRSF_ADDRESS_CRSF_TRANSMITTER = 0xEE
};
#define CRSF_PAYLOAD_SIZE_MAX 32 // !!TODO needs checking
#define CRSF_FRAME_SIZE_MAX (CRSF_PAYLOAD_SIZE_MAX + 4)
typedef struct crsfFrameDef_s {
uint8_t deviceAddress;
uint8_t frameLength;
uint8_t type;
uint8_t payload[CRSF_PAYLOAD_SIZE_MAX + 1]; // +1 for CRC at end of payload
} crsfFrameDef_t;
typedef union crsfFrame_u {
uint8_t bytes[CRSF_FRAME_SIZE_MAX];
crsfFrameDef_t frame;
} crsfFrame_t;
void crsfRxWriteTelemetryData(const void *data, int len);
void crsfRxSendTelemetryData(void);
struct rxConfig_s;
struct rxRuntimeConfig_s;
bool crsfRxInit(const struct rxConfig_s *initialRxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig);
bool crsfRxIsActive(void);

3
src/main/rx/msp.c
View file

@ -20,7 +20,7 @@
#include "platform.h"
#ifndef SKIP_RX_MSP
#ifdef USE_RX_MSP
#include "build/build_config.h"
@ -67,6 +67,7 @@ void rxMspInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
UNUSED(rxConfig);
rxRuntimeConfig->channelCount = MAX_SUPPORTED_RC_CHANNEL_COUNT;
rxRuntimeConfig->rxRefreshRate = 20000;
rxRuntimeConfig->rcReadRawFn = rxMspReadRawRC;
rxRuntimeConfig->rcFrameStatusFn = rxMspFrameStatus;
}

5
src/main/rx/pwm.c
View file

@ -23,7 +23,7 @@
#include "platform.h"
#ifndef SKIP_RX_PWM_PPM
#if defined(USE_RX_PWM) || defined(USE_RX_PPM)
#include "common/utils.h"
@ -51,6 +51,9 @@ static uint16_t ppmReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t c
void rxPwmInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{
UNUSED(rxConfig);
rxRuntimeConfig->rxRefreshRate = 20000;
// configure PWM/CPPM read function and max number of channels. serial rx below will override both of these, if enabled
if (feature(FEATURE_RX_PARALLEL_PWM)) {
rxRuntimeConfig->channelCount = MAX_SUPPORTED_RC_PARALLEL_PWM_CHANNEL_COUNT;

149
src/main/rx/rx.c
View file

@ -58,6 +58,7 @@
#include "rx/ibus.h"
#include "rx/jetiexbus.h"
#include "rx/rx_spi.h"
#include "rx/crsf.h"
//#define DEBUG_RX_SIGNAL_LOSS
@ -75,6 +76,7 @@ static bool rxIsInFailsafeModeNotDataDriven = true;
static timeUs_t rxUpdateAt = 0;
static timeUs_t needRxSignalBefore = 0;
static uint32_t needRxSignalMaxDelayUs = 0;
static timeUs_t suspendRxSignalUntil = 0;
static uint8_t skipRxSamples = 0;
@ -235,6 +237,11 @@ bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
case SERIALRX_JETIEXBUS:
enabled = jetiExBusInit(rxConfig, rxRuntimeConfig);
break;
#endif
#ifdef USE_SERIALRX_CRSF
case SERIALRX_CRSF:
enabled = crsfRxInit(rxConfig, rxRuntimeConfig);
break;
#endif
default:
enabled = false;
@ -242,54 +249,6 @@ bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
}
return enabled;
}
static uint8_t serialRxFrameStatus(const rxConfig_t *rxConfig)
{
/**
* FIXME: Each of the xxxxFrameStatus() methods MUST be able to survive being called without the
* corresponding xxxInit() method having been called first.
*
* This situation arises when the cli or the msp changes the value of rxConfig->serialrx_provider
*
* A solution is for the ___Init() to configure the serialRxFrameStatus function pointer which
* should be used instead of the switch statement below.
*/
switch (rxConfig->serialrx_provider) {
#ifdef USE_SERIALRX_SPEKTRUM
case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048:
return spektrumFrameStatus();
#endif
#ifdef USE_SERIALRX_SBUS
case SERIALRX_SBUS:
return sbusFrameStatus();
#endif
#ifdef USE_SERIALRX_SUMD
case SERIALRX_SUMD:
return sumdFrameStatus();
#endif
#ifdef USE_SERIALRX_SUMH
case SERIALRX_SUMH:
return sumhFrameStatus();
#endif
#ifdef USE_SERIALRX_XBUS
case SERIALRX_XBUS_MODE_B:
case SERIALRX_XBUS_MODE_B_RJ01:
return xBusFrameStatus();
#endif
#ifdef USE_SERIALRX_IBUS
case SERIALRX_IBUS:
return ibusFrameStatus();
#endif
#ifdef USE_SERIALRX_JETIEXBUS
case SERIALRX_JETIEXBUS:
return jetiExBusFrameStatus();
#endif
default:
return RX_FRAME_PENDING;
}
return RX_FRAME_PENDING;
}
#endif
void rxInit(const modeActivationCondition_t *modeActivationConditions)
@ -297,6 +256,7 @@ void rxInit(const modeActivationCondition_t *modeActivationConditions)
rxRuntimeConfig.rcReadRawFn = nullReadRawRC;
rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus;
rcSampleIndex = 0;
needRxSignalMaxDelayUs = DELAY_10_HZ;
for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) {
rcData[i] = rxConfig()->midrc;
@ -332,9 +292,10 @@ void rxInit(const modeActivationCondition_t *modeActivationConditions)
}
#endif
#ifndef SKIP_RX_MSP
#ifdef USE_RX_MSP
if (feature(FEATURE_RX_MSP)) {
rxMspInit(rxConfig(), &rxRuntimeConfig);
needRxSignalMaxDelayUs = DELAY_5_HZ;
}
#endif
@ -344,13 +305,13 @@ void rxInit(const modeActivationCondition_t *modeActivationConditions)
if (!enabled) {
featureClear(FEATURE_RX_SPI);
rxRuntimeConfig.rcReadRawFn = nullReadRawRC;
rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus;
}
}
#endif
#ifndef SKIP_RX_PWM_PPM
#if defined(USE_RX_PWM) || defined(USE_RX_PPM)
if (feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM)) {
rxRuntimeConfig.rxRefreshRate = 20000;
rxPwmInit(rxConfig(), &rxRuntimeConfig);
}
#endif
@ -373,23 +334,12 @@ bool rxAreFlightChannelsValid(void)
{
return rxFlightChannelsValid;
}
static bool isRxDataDriven(void)
{
return !(feature(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM));
}
static void resetRxSignalReceivedFlagIfNeeded(timeUs_t currentTimeUs)
{
if (!rxSignalReceived) {
return;
}
if (((int32_t)(currentTimeUs - needRxSignalBefore) >= 0)) {
rxSignalReceived = false;
rxSignalReceivedNotDataDriven = false;
}
}
void suspendRxSignal(void)
{
suspendRxSignalUntil = micros() + SKIP_RC_ON_SUSPEND_PERIOD;
@ -404,69 +354,44 @@ void resumeRxSignal(void)
failsafeOnRxResume();
}
bool updateRx(timeUs_t currentTimeUs)
bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime)
{
resetRxSignalReceivedFlagIfNeeded(currentTimeUs);
UNUSED(currentDeltaTime);
if (isRxDataDriven()) {
rxDataReceived = false;
}
#ifdef SERIAL_RX
if (feature(FEATURE_RX_SERIAL)) {
const uint8_t frameStatus = serialRxFrameStatus(rxConfig());
if (frameStatus & RX_FRAME_COMPLETE) {
rxDataReceived = true;
rxIsInFailsafeMode = (frameStatus & RX_FRAME_FAILSAFE) != 0;
rxSignalReceived = !rxIsInFailsafeMode;
needRxSignalBefore = currentTimeUs + DELAY_10_HZ;
if (rxSignalReceived) {
if (((int32_t)(currentTimeUs - needRxSignalBefore) >= 0)) {
rxSignalReceived = false;
rxSignalReceivedNotDataDriven = false;
}
}
#endif
#ifdef USE_RX_SPI
if (feature(FEATURE_RX_SPI)) {
const uint8_t frameStatus = rxRuntimeConfig.rcFrameStatusFn();
if (frameStatus & RX_FRAME_COMPLETE) {
rxDataReceived = true;
rxIsInFailsafeMode = false;
rxSignalReceived = !rxIsInFailsafeMode;
needRxSignalBefore = currentTimeUs + DELAY_5_HZ;
}
}
#endif
#ifndef SKIP_RX_MSP
if (feature(FEATURE_RX_MSP)) {
const uint8_t frameStatus = rxMspFrameStatus();
if (frameStatus & RX_FRAME_COMPLETE) {
rxDataReceived = true;
rxIsInFailsafeMode = false;
rxSignalReceived = !rxIsInFailsafeMode;
needRxSignalBefore = currentTimeUs + DELAY_5_HZ;
}
}
#endif
#ifndef SKIP_RX_PWM_PPM
#if defined(USE_RX_PWM) || defined(USE_RX_PPM)
if (feature(FEATURE_RX_PPM)) {
if (isPPMDataBeingReceived()) {
rxSignalReceivedNotDataDriven = true;
rxIsInFailsafeModeNotDataDriven = false;
needRxSignalBefore = currentTimeUs + DELAY_10_HZ;
needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
resetPPMDataReceivedState();
}
}
if (feature(FEATURE_RX_PARALLEL_PWM)) {
} else if (feature(FEATURE_RX_PARALLEL_PWM)) {
if (isPWMDataBeingReceived()) {
rxSignalReceivedNotDataDriven = true;
rxIsInFailsafeModeNotDataDriven = false;
needRxSignalBefore = currentTimeUs + DELAY_10_HZ;
needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
}
} else
#endif
{
rxDataReceived = false;
const uint8_t frameStatus = rxRuntimeConfig.rcFrameStatusFn();
if (frameStatus & RX_FRAME_COMPLETE) {
rxDataReceived = true;
rxIsInFailsafeMode = (frameStatus & RX_FRAME_FAILSAFE) != 0;
rxSignalReceived = !rxIsInFailsafeMode;
needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
}
}
#endif
return rxDataReceived || ((int32_t)(currentTimeUs - rxUpdateAt) >= 0); // data driven or 50Hz
}
@ -557,8 +482,8 @@ static void readRxChannelsApplyRanges(void)
static void detectAndApplySignalLossBehaviour(void)
{
bool useValueFromRx = true;
bool rxIsDataDriven = isRxDataDriven();
uint32_t currentMilliTime = millis();
const bool rxIsDataDriven = isRxDataDriven();
const uint32_t currentMilliTime = millis();
if (!rxIsDataDriven) {
rxSignalReceived = rxSignalReceivedNotDataDriven;

4
src/main/rx/rx.h
View file

@ -59,7 +59,7 @@ typedef enum {
SERIALRX_XBUS_MODE_B_RJ01 = 6,
SERIALRX_IBUS = 7,
SERIALRX_JETIEXBUS = 8,
SERIALRX_PROVIDER_MAX = SERIALRX_JETIEXBUS
SERIALRX_CRSF = 9
} SerialRXType;
#define MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT 12
@ -153,7 +153,7 @@ extern rxRuntimeConfig_t rxRuntimeConfig; //!!TODO remove this extern, only need
struct modeActivationCondition_s;
void rxInit(const struct modeActivationCondition_s *modeActivationConditions);
bool updateRx(timeUs_t currentTimeUs);
bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime);
bool rxIsReceivingSignal(void);
bool rxAreFlightChannelsValid(void);
void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs);

3
src/main/target/CC3D/target.h
View file

@ -188,7 +188,8 @@
#ifdef USE_RX_NRF24
#define SKIP_RX_PWM_PPM
#undef USE_RX_PWM
#undef USE_RX_PPM
#undef SERIAL_RX
#undef SPEKTRUM_BIND
#endif

5
src/main/target/CJMCU/target.h
View file

@ -84,14 +84,15 @@
//#define TELEMETRY
//#define TELEMETRY_LTM
//#define TELEMETRY_NRF24_LTM
#define SKIP_RX_PWM_PPM
#undef USE_RX_PWM
#undef USE_RX_PPM
#undef SERIAL_RX
#undef SKIP_TASK_STATISTICS
#else
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
#undef SKIP_RX_MSP
#define USE_RX_MSP
#define SPEKTRUM_BIND
#define BIND_PIN PA3 // UART2, PA3

5
src/main/target/CRAZEPONYMINI/target.h
View file

@ -67,9 +67,10 @@
#define RX_IRQ_PIN PA15
#define RX_IRQ_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA
#define SKIP_RX_MSP
#undef USE_RX_MSP
#define SKIP_INFLIGHT_ADJUSTMENTS
#define SKIP_RX_PWM_PPM
#undef USE_RX_PWM
#undef USE_RX_PPM
#undef SERIAL_RX
#undef BLACKBOX

6
src/main/target/common.h
View file

@ -24,6 +24,8 @@
#define USE_SERVOS
#define USE_CLI
#define USE_RX_PWM
#define USE_RX_PPM
#define SERIAL_RX
#define USE_SERIALRX_SPEKTRUM // Cheap and fairly common protocol
#define USE_SERIALRX_SBUS // Very common protocol
@ -66,11 +68,14 @@
#define TELEMETRY_IBUS
#define TELEMETRY_MAVLINK
#define TELEMETRY_SMARTPORT
#define TELEMETRY_CRSF
// These are rather exotic serial protocols
#define USE_RX_MSP
#define USE_SERIALRX_SUMD
#define USE_SERIALRX_SUMH
#define USE_SERIALRX_XBUS
#define USE_SERIALRX_JETIEXBUS
#define USE_SERIALRX_CRSF
#define USE_PMW_SERVO_DRIVER
#define PWM_DRIVER_PCA9685
#define NAV_MAX_WAYPOINTS 60
@ -79,7 +84,6 @@
#define CLI_MINIMAL_VERBOSITY
#define SKIP_CLI_COMMAND_HELP
#define SKIP_CLI_RESOURCES
#define SKIP_RX_MSP
#define DISABLE_UNCOMMON_MIXERS
#define NAV_MAX_WAYPOINTS 30
#define MAX_BOOTLOG_ENTRIES 32

357
src/main/telemetry/crsf.c Executable file
View file

@ -0,0 +1,357 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it 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 is distributed in the hope that it 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include "platform.h"
#if defined(TELEMETRY) && defined(TELEMETRY_CRSF)
#include "build/build_config.h"
#include "build/version.h"
#include "common/streambuf.h"
#include "common/maths.h"
#include "common/axis.h"
#include "common/utils.h"
#include "common/time.h"
#include "drivers/system.h"
#include "drivers/serial.h"
#include "sensors/battery.h"
#include "io/gps.h"
#include "io/serial.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "flight/imu.h"
#include "rx/rx.h"
#include "rx/crsf.h"
#include "telemetry/telemetry.h"
#include "telemetry/crsf.h"
#include "config/feature.h"
//#include "config/config.h"
#include "config/config_master.h"
#define CRSF_CYCLETIME_US 100000 // 100ms, 10 Hz
static bool crsfTelemetryEnabled;
static uint8_t crsfCrc;
static uint8_t crsfFrame[CRSF_FRAME_SIZE_MAX];
static void crsfInitializeFrame(sbuf_t *dst)
{
crsfCrc = 0;
dst->ptr = crsfFrame;
dst->end = ARRAYEND(crsfFrame);
sbufWriteU8(dst, CRSF_ADDRESS_BROADCAST);
}
static void crsfSerialize8(sbuf_t *dst, uint8_t v)
{
sbufWriteU8(dst, v);
crsfCrc = crc8_dvb_s2(crsfCrc, v);
}
static void crsfSerialize16(sbuf_t *dst, uint16_t v)
{
// Use BigEndian format
crsfSerialize8(dst, (v >> 8));
crsfSerialize8(dst, (uint8_t)v);
}
static void crsfSerialize32(sbuf_t *dst, uint32_t v)
{
// Use BigEndian format
crsfSerialize8(dst, (v >> 24));
crsfSerialize8(dst, (v >> 16));
crsfSerialize8(dst, (v >> 8));
crsfSerialize8(dst, (uint8_t)v);
}
static void crsfSerializeData(sbuf_t *dst, const uint8_t *data, int len)
{
for (int ii = 0; ii< len; ++ii) {
crsfSerialize8(dst, data[ii]);
}
}
static void crsfFinalize(sbuf_t *dst)
{
sbufWriteU8(dst, crsfCrc);
sbufSwitchToReader(dst, crsfFrame);
// write the telemetry frame to the receiver.
crsfRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst));
}
static int crsfFinalizeBuf(sbuf_t *dst, uint8_t *frame)
{
sbufWriteU8(dst, crsfCrc);
sbufSwitchToReader(dst, crsfFrame);
const int frameSize = sbufBytesRemaining(dst);
for (int ii = 0; sbufBytesRemaining(dst); ++ii) {
frame[ii] = sbufReadU8(dst);
}
return frameSize;
}
/*
CRSF frame has the structure:
<Device address> <Frame length> <Type> <Payload> <CRC>
Device address: (uint8_t)
Frame length: length in bytes including Type (uint8_t)
Type: (uint8_t)
CRC: (uint8_t)
*/
/*
0x02 GPS
Payload:
int32_t Latitude ( degree / 10`000`000 )
int32_t Longitude (degree / 10`000`000 )
uint16_t Groundspeed ( km/h / 10 )
uint16_t GPS heading ( degree / 100 )
uint16 Altitude ( meter ­ 1000m offset )
uint8_t Satellites in use ( counter )
*/
void crsfFrameGps(sbuf_t *dst)
{
// use sbufWrite since CRC does not include frame length
sbufWriteU8(dst, CRSF_FRAME_GPS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
crsfSerialize8(dst, CRSF_FRAMETYPE_GPS);
crsfSerialize32(dst, gpsSol.llh.lat); // CRSF and betaflight use same units for degrees
crsfSerialize32(dst, gpsSol.llh.lon);
crsfSerialize16(dst, (gpsSol.groundSpeed * 36 + 50) / 100); // gpsSol.groundSpeed is in cm/s
crsfSerialize16(dst, DECIDEGREES_TO_CENTIDEGREES(gpsSol.groundCourse)); // gpsSol.groundCourse is 0.1 degrees, need 0.01 deg
//Send real GPS altitude only if it's reliable (there's a GPS fix)
const uint16_t altitude = (STATE(GPS_FIX) ? gpsSol.llh.alt : 0) + 1000;
crsfSerialize16(dst, altitude);
crsfSerialize8(dst, gpsSol.numSat);
}
/*
0x08 Battery sensor
Payload:
uint16_t Voltage ( mV * 100 )
uint16_t Current ( mA * 100 )
uint24_t Capacity ( mAh )
uint8_t Battery remaining ( percent )
*/
void crsfFrameBatterySensor(sbuf_t *dst)
{
// use sbufWrite since CRC does not include frame length
sbufWriteU8(dst, CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
crsfSerialize8(dst, CRSF_FRAMETYPE_BATTERY_SENSOR);
crsfSerialize16(dst, vbat); // vbat is in units of 0.1V
crsfSerialize16(dst, amperage / 10);
const uint32_t batteryCapacity = batteryConfig()->batteryCapacity;
const uint8_t batteryRemainingPercentage = calculateBatteryPercentage();
crsfSerialize8(dst, (batteryCapacity >> 16));
crsfSerialize8(dst, (batteryCapacity >> 8));
crsfSerialize8(dst, (uint8_t)batteryCapacity);
crsfSerialize8(dst, batteryRemainingPercentage);
}
typedef enum {
CRSF_ACTIVE_ANTENNA1 = 0,
CRSF_ACTIVE_ANTENNA2 = 1
} crsfActiveAntenna_e;
typedef enum {
CRSF_RF_MODE_4_HZ = 0,
CRSF_RF_MODE_50_HZ = 1,
CRSF_RF_MODE_150_HZ = 2
} crsrRfMode_e;
typedef enum {
CRSF_RF_POWER_0_mW = 0,
CRSF_RF_POWER_10_mW = 1,
CRSF_RF_POWER_25_mW = 2,
CRSF_RF_POWER_100_mW = 3,
CRSF_RF_POWER_500_mW = 4,
CRSF_RF_POWER_1000_mW = 5,
CRSF_RF_POWER_2000_mW = 6
} crsrRfPower_e;
/*
0x1E Attitude
Payload:
int16_t Pitch angle ( rad / 10000 )
int16_t Roll angle ( rad / 10000 )
int16_t Yaw angle ( rad / 10000 )
*/
#define DECIDEGREES_TO_RADIANS10000(angle) ((int16_t)(1000.0f * (angle) * RAD))
void crsfFrameAttitude(sbuf_t *dst)
{
sbufWriteU8(dst, CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
crsfSerialize8(dst, CRSF_FRAMETYPE_ATTITUDE);
crsfSerialize16(dst, DECIDEGREES_TO_RADIANS10000(attitude.values.pitch));
crsfSerialize16(dst, DECIDEGREES_TO_RADIANS10000(attitude.values.roll));
crsfSerialize16(dst, DECIDEGREES_TO_RADIANS10000(attitude.values.yaw));
}
/*
0x21 Flight mode text based
Payload:
char[] Flight mode ( Null­terminated string )
*/
void crsfFrameFlightMode(sbuf_t *dst)
{
// just do Angle for the moment as a placeholder
// write zero for frame length, since we don't know it yet
uint8_t *lengthPtr = sbufPtr(dst);
sbufWriteU8(dst, 0);
crsfSerialize8(dst, CRSF_FRAMETYPE_FLIGHT_MODE);
// use same logic as OSD, so telemetry displays same flight text as OSD
const char *flightMode = "ACRO";
if (FLIGHT_MODE(FAILSAFE_MODE)) {
flightMode = "!FS";
} else if (FLIGHT_MODE(ANGLE_MODE)) {
flightMode = "STAB";
} else if (FLIGHT_MODE(HORIZON_MODE)) {
flightMode = "HOR";
}
crsfSerializeData(dst, (const uint8_t*)flightMode, strlen(flightMode));
crsfSerialize8(dst, 0); // zero terminator for string
// write in the length
*lengthPtr = sbufPtr(dst) - lengthPtr;
}
#define BV(x) (1 << (x)) // bit value
// schedule array to decide how often each type of frame is sent
#define CRSF_SCHEDULE_COUNT_MAX 5
static uint8_t crsfScheduleCount;
static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX];
static void processCrsf(void)
{
static uint8_t crsfScheduleIndex = 0;
const uint8_t currentSchedule = crsfSchedule[crsfScheduleIndex];
sbuf_t crsfPayloadBuf;
sbuf_t *dst = &crsfPayloadBuf;
if (currentSchedule & BV(CRSF_FRAME_ATTITUDE)) {
crsfInitializeFrame(dst);
crsfFrameAttitude(dst);
crsfFinalize(dst);
}
if (currentSchedule & BV(CRSF_FRAME_BATTERY_SENSOR)) {
crsfInitializeFrame(dst);
crsfFrameBatterySensor(dst);
crsfFinalize(dst);
}
if (currentSchedule & BV(CRSF_FRAME_FLIGHT_MODE)) {
crsfInitializeFrame(dst);
crsfFrameFlightMode(dst);
crsfFinalize(dst);
}
#ifdef GPS
if (currentSchedule & BV(CRSF_FRAME_GPS)) {
crsfInitializeFrame(dst);
crsfFrameGps(dst);
crsfFinalize(dst);
}
#endif
crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount;
}
void initCrsfTelemetry(void)
{
// check if there is a serial port open for CRSF telemetry (ie opened by the CRSF RX)
// and feature is enabled, if so, set CRSF telemetry enabled
crsfTelemetryEnabled = crsfRxIsActive();
int index = 0;
crsfSchedule[index++] = BV(CRSF_FRAME_ATTITUDE);
crsfSchedule[index++] = BV(CRSF_FRAME_BATTERY_SENSOR);
crsfSchedule[index++] = BV(CRSF_FRAME_FLIGHT_MODE);
if (feature(FEATURE_GPS)) {
crsfSchedule[index++] = BV(CRSF_FRAME_GPS);
}
crsfScheduleCount = (uint8_t)index;
}
bool checkCrsfTelemetryState(void)
{
return crsfTelemetryEnabled;
}
/*
* Called periodically by the scheduler
*/
void handleCrsfTelemetry(timeUs_t currentTimeUs)
{
static uint32_t crsfLastCycleTime;
if (!crsfTelemetryEnabled) {
return;
}
// Give the receiver a chance to send any outstanding telemetry data.
// This needs to be done at high frequency, to enable the RX to send the telemetry frame
// in between the RX frames.
crsfRxSendTelemetryData();
// Actual telemetry data only needs to be sent at a low frequency, ie 10Hz
if (currentTimeUs >= crsfLastCycleTime + CRSF_CYCLETIME_US) {
crsfLastCycleTime = currentTimeUs;
processCrsf();
}
}
int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType)
{
sbuf_t crsfFrameBuf;
sbuf_t *sbuf = &crsfFrameBuf;
crsfInitializeFrame(sbuf);
switch (frameType) {
default:
case CRSF_FRAME_ATTITUDE:
crsfFrameAttitude(sbuf);
break;
case CRSF_FRAME_BATTERY_SENSOR:
crsfFrameBatterySensor(sbuf);
break;
case CRSF_FRAME_FLIGHT_MODE:
crsfFrameFlightMode(sbuf);
break;
#if defined(GPS)
case CRSF_FRAME_GPS:
crsfFrameGps(sbuf);
break;
#endif
}
const int frameSize = crsfFinalizeBuf(sbuf, frame);
return frameSize;
}
#endif

35
src/main/telemetry/crsf.h Executable file
View file

@ -0,0 +1,35 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it 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 is distributed in the hope that it 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "common/time.h"
typedef enum {
CRSF_FRAME_START = 0,
CRSF_FRAME_ATTITUDE = CRSF_FRAME_START,
CRSF_FRAME_BATTERY_SENSOR,
CRSF_FRAME_FLIGHT_MODE,
CRSF_FRAME_GPS
} crsfFrameType_e;
void initCrsfTelemetry(void);
bool checkCrsfTelemetryState(void);
void handleCrsfTelemetry(timeUs_t currentTimeUs);
int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType);

11
src/main/telemetry/telemetry.c
View file

@ -43,6 +43,7 @@
#include "telemetry/mavlink.h"
#include "telemetry/jetiexbus.h"
#include "telemetry/ibus.h"
#include "telemetry/crsf.h"
static telemetryConfig_t *telemetryConfig;
@ -81,6 +82,10 @@ void telemetryInit(void)
initIbusTelemetry(telemetryConfig);
#endif
#if defined(TELEMETRY_CRSF)
initCrsfTelemetry();
#endif
telemetryCheckState();
}
@ -135,6 +140,9 @@ void telemetryCheckState(void)
checkIbusTelemetryState();
#endif
#if defined(TELEMETRY_CRSF)
checkCrsfTelemetryState();
#endif
}
void telemetryProcess(timeUs_t currentTimeUs, uint16_t deadband3d_throttle)
@ -174,6 +182,9 @@ void telemetryProcess(timeUs_t currentTimeUs, uint16_t deadband3d_throttle)
handleIbusTelemetry();
#endif
#if defined(TELEMETRY_CRSF)
handleCrsfTelemetry(currentTimeUs);
#endif
}
#endif