1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 22:35:23 +03:00

Changed function order to avoid function forward declarations

This commit is contained in:
Martin Budden 2016-10-02 17:30:27 +01:00
parent 98c5e0298a
commit fd20ece914
8 changed files with 292 additions and 314 deletions

View file

@ -53,41 +53,6 @@
static bool ibusFrameDone = false;
static uint32_t ibusChannelData[IBUS_MAX_CHANNEL];
static void ibusDataReceive(uint16_t c);
static uint16_t ibusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan);
bool ibusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{
UNUSED(rxConfig);
rxRuntimeConfig->channelCount = IBUS_MAX_CHANNEL;
rxRuntimeConfig->rxRefreshRate = 20000; // TODO - Verify speed
rxRuntimeConfig->rcReadRawFunc = ibusReadRawRC;
rxRuntimeConfig->rcFrameStatusFunc = ibusFrameStatus;
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
if (!portConfig) {
return false;
}
#ifdef TELEMETRY
bool portShared = telemetryCheckRxPortShared(portConfig);
#else
bool portShared = false;
#endif
serialPort_t *ibusPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, ibusDataReceive, IBUS_BAUDRATE, portShared ? MODE_RXTX : MODE_RX, SERIAL_NOT_INVERTED);
#ifdef TELEMETRY
if (portShared) {
telemetrySharedPort = ibusPort;
}
#endif
return ibusPort != NULL;
}
static uint8_t ibus[IBUS_BUFFSIZE] = { 0, };
// Receive ISR callback
@ -149,4 +114,36 @@ static uint16_t ibusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t
UNUSED(rxRuntimeConfig);
return ibusChannelData[chan];
}
bool ibusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{
UNUSED(rxConfig);
rxRuntimeConfig->channelCount = IBUS_MAX_CHANNEL;
rxRuntimeConfig->rxRefreshRate = 20000; // TODO - Verify speed
rxRuntimeConfig->rcReadRawFunc = ibusReadRawRC;
rxRuntimeConfig->rcFrameStatusFunc = ibusFrameStatus;
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
if (!portConfig) {
return false;
}
#ifdef TELEMETRY
bool portShared = telemetryCheckRxPortShared(portConfig);
#else
bool portShared = false;
#endif
serialPort_t *ibusPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, ibusDataReceive, IBUS_BAUDRATE, portShared ? MODE_RXTX : MODE_RX, SERIAL_NOT_INVERTED);
#ifdef TELEMETRY
if (portShared) {
telemetrySharedPort = ibusPort;
}
#endif
return ibusPort != NULL;
}
#endif

View file

@ -210,10 +210,6 @@ static uint8_t jetiExBusChannelFrame[EXBUS_MAX_CHANNEL_FRAME_SIZE];
static uint8_t jetiExBusRequestFrame[EXBUS_MAX_REQUEST_FRAME_SIZE];
static uint16_t jetiExBusChannelData[JETIEXBUS_CHANNEL_COUNT];
static void jetiExBusDataReceive(uint16_t c);
static uint16_t jetiExBusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan);
static void jetiExBusFrameReset();
#ifdef TELEMETRY
@ -228,30 +224,6 @@ uint8_t calcCRC8(uint8_t *pt, uint8_t msgLen);
uint16_t calcCRC16(uint8_t *pt, uint8_t msgLen);
bool jetiExBusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{
UNUSED(rxConfig);
rxRuntimeConfig->channelCount = JETIEXBUS_CHANNEL_COUNT;
rxRuntimeConfig->rxRefreshRate = 5500;
rxRuntimeConfig->rcReadRawFunc = jetiExBusReadRawRC;
rxRuntimeConfig->rcFrameStatusFunc = jetiExBusFrameStatus;
jetiExBusFrameReset();
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
if (!portConfig) {
return false;
}
jetiExBusPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, jetiExBusDataReceive, JETIEXBUS_BAUDRATE, MODE_RXTX, JETIEXBUS_OPTIONS );
serialSetMode(jetiExBusPort, MODE_RX);
return jetiExBusPort != NULL;
}
// Jeti Ex Bus CRC calculations for a frame
uint16_t calcCRC16(uint8_t *pt, uint8_t msgLen)
{
@ -612,6 +584,28 @@ void sendJetiExBusTelemetry(uint8_t packetID)
jetiExBusTransceiveState = EXBUS_TRANS_IS_TX_COMPLETED;
requestLoop++;
}
#endif // TELEMETRY
bool jetiExBusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{
UNUSED(rxConfig);
rxRuntimeConfig->channelCount = JETIEXBUS_CHANNEL_COUNT;
rxRuntimeConfig->rxRefreshRate = 5500;
rxRuntimeConfig->rcReadRawFunc = jetiExBusReadRawRC;
rxRuntimeConfig->rcFrameStatusFunc = jetiExBusFrameStatus;
jetiExBusFrameReset();
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
if (!portConfig) {
return false;
}
jetiExBusPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, jetiExBusDataReceive, JETIEXBUS_BAUDRATE, MODE_RXTX, JETIEXBUS_OPTIONS );
serialSetMode(jetiExBusPort, MODE_RX);
return jetiExBusPort != NULL;
}
#endif // SERIAL_RX

View file

@ -107,8 +107,6 @@ static uint8_t nullFrameStatus(void)
return RX_FRAME_PENDING;
}
bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
void useRxConfig(const rxConfig_t *rxConfigToUse)
{
rxConfig = rxConfigToUse;
@ -151,6 +149,39 @@ void resetAllRxChannelRangeConfigurations(rxChannelRangeConfiguration_t *rxChann
}
}
#ifdef SERIAL_RX
bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{
bool enabled = false;
switch (rxConfig->serialrx_provider) {
case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048:
enabled = spektrumInit(rxConfig, rxRuntimeConfig);
break;
case SERIALRX_SBUS:
enabled = sbusInit(rxConfig, rxRuntimeConfig);
break;
case SERIALRX_SUMD:
enabled = sumdInit(rxConfig, rxRuntimeConfig);
break;
case SERIALRX_SUMH:
enabled = sumhInit(rxConfig, rxRuntimeConfig);
break;
case SERIALRX_XBUS_MODE_B:
case SERIALRX_XBUS_MODE_B_RJ01:
enabled = xBusInit(rxConfig, rxRuntimeConfig);
break;
case SERIALRX_IBUS:
enabled = ibusInit(rxConfig, rxRuntimeConfig);
break;
case SERIALRX_JETIEXBUS:
enabled = jetiExBusInit(rxConfig, rxRuntimeConfig);
break;
}
return enabled;
}
#endif
void rxInit(const rxConfig_t *rxConfig, const modeActivationCondition_t *modeActivationConditions)
{
useRxConfig(rxConfig);
@ -217,37 +248,6 @@ void rxInit(const rxConfig_t *rxConfig, const modeActivationCondition_t *modeAct
}
#ifdef SERIAL_RX
bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{
bool enabled = false;
switch (rxConfig->serialrx_provider) {
case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048:
enabled = spektrumInit(rxConfig, rxRuntimeConfig);
break;
case SERIALRX_SBUS:
enabled = sbusInit(rxConfig, rxRuntimeConfig);
break;
case SERIALRX_SUMD:
enabled = sumdInit(rxConfig, rxRuntimeConfig);
break;
case SERIALRX_SUMH:
enabled = sumhInit(rxConfig, rxRuntimeConfig);
break;
case SERIALRX_XBUS_MODE_B:
case SERIALRX_XBUS_MODE_B_RJ01:
enabled = xBusInit(rxConfig, rxRuntimeConfig);
break;
case SERIALRX_IBUS:
enabled = ibusInit(rxConfig, rxRuntimeConfig);
break;
case SERIALRX_JETIEXBUS:
enabled = jetiExBusInit(rxConfig, rxRuntimeConfig);
break;
}
return enabled;
}
static uint8_t serialRxFrameStatus(const rxConfig_t *rxConfig)
{
/**

View file

@ -81,46 +81,9 @@ static uint16_t sbusStateFlags = 0;
#define SBUS_DIGITAL_CHANNEL_MAX 1812
static bool sbusFrameDone = false;
static void sbusDataReceive(uint16_t c);
static uint16_t sbusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan);
static uint32_t sbusChannelData[SBUS_MAX_CHANNEL];
bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{
for (int b = 0; b < SBUS_MAX_CHANNEL; b++) {
sbusChannelData[b] = (16 * rxConfig->midrc) / 10 - 1408;
}
rxRuntimeConfig->channelCount = SBUS_MAX_CHANNEL;
rxRuntimeConfig->rxRefreshRate = 11000;
rxRuntimeConfig->rcReadRawFunc = sbusReadRawRC;
rxRuntimeConfig->rcFrameStatusFunc = sbusFrameStatus;
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
if (!portConfig) {
return false;
}
#ifdef TELEMETRY
bool portShared = telemetryCheckRxPortShared(portConfig);
#else
bool portShared = false;
#endif
portOptions_t options = (rxConfig->sbus_inversion) ? (SBUS_PORT_OPTIONS | SERIAL_INVERTED) : SBUS_PORT_OPTIONS;
serialPort_t *sBusPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, sbusDataReceive, SBUS_BAUDRATE, portShared ? MODE_RXTX : MODE_RX, options);
#ifdef TELEMETRY
if (portShared) {
telemetrySharedPort = sBusPort;
}
#endif
return sBusPort != NULL;
}
#define SBUS_FLAG_CHANNEL_17 (1 << 0)
#define SBUS_FLAG_CHANNEL_18 (1 << 1)
#define SBUS_FLAG_SIGNAL_LOSS (1 << 2)
@ -266,4 +229,39 @@ static uint16_t sbusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t
// http://www.wolframalpha.com/input/?i=linear+fit+%7B173%2C+988%7D%2C+%7B1812%2C+2012%7D%2C+%7B993%2C+1500%7D
return (0.625f * sbusChannelData[chan]) + 880;
}
bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{
for (int b = 0; b < SBUS_MAX_CHANNEL; b++) {
sbusChannelData[b] = (16 * rxConfig->midrc) / 10 - 1408;
}
rxRuntimeConfig->channelCount = SBUS_MAX_CHANNEL;
rxRuntimeConfig->rxRefreshRate = 11000;
rxRuntimeConfig->rcReadRawFunc = sbusReadRawRC;
rxRuntimeConfig->rcFrameStatusFunc = sbusFrameStatus;
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
if (!portConfig) {
return false;
}
#ifdef TELEMETRY
bool portShared = telemetryCheckRxPortShared(portConfig);
#else
bool portShared = false;
#endif
portOptions_t options = (rxConfig->sbus_inversion) ? (SBUS_PORT_OPTIONS | SERIAL_INVERTED) : SBUS_PORT_OPTIONS;
serialPort_t *sBusPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, sbusDataReceive, SBUS_BAUDRATE, portShared ? MODE_RXTX : MODE_RX, options);
#ifdef TELEMETRY
if (portShared) {
telemetrySharedPort = sBusPort;
}
#endif
return sBusPort != NULL;
}
#endif

View file

@ -62,9 +62,6 @@ static bool spekHiRes = false;
static volatile uint8_t spekFrame[SPEK_FRAME_SIZE];
static void spektrumDataReceive(uint16_t c);
static uint16_t spektrumReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan);
static rxRuntimeConfig_t *rxRuntimeConfigPtr;
#ifdef SPEKTRUM_BIND
@ -74,53 +71,6 @@ static IO_t BindPin = DEFIO_IO(NONE);
static IO_t BindPlug = DEFIO_IO(NONE);
#endif
bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{
rxRuntimeConfigPtr = rxRuntimeConfig;
switch (rxConfig->serialrx_provider) {
case SERIALRX_SPEKTRUM2048:
// 11 bit frames
spek_chan_shift = 3;
spek_chan_mask = 0x07;
spekHiRes = true;
rxRuntimeConfig->channelCount = SPEKTRUM_2048_CHANNEL_COUNT;
rxRuntimeConfig->rxRefreshRate = 11000;
break;
case SERIALRX_SPEKTRUM1024:
// 10 bit frames
spek_chan_shift = 2;
spek_chan_mask = 0x03;
spekHiRes = false;
rxRuntimeConfig->channelCount = SPEKTRUM_1024_CHANNEL_COUNT;
rxRuntimeConfig->rxRefreshRate = 22000;
break;
}
rxRuntimeConfig->rcReadRawFunc = spektrumReadRawRC;
rxRuntimeConfig->rcFrameStatusFunc = spektrumFrameStatus;
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
if (!portConfig) {
return false;
}
#ifdef TELEMETRY
bool portShared = telemetryCheckRxPortShared(portConfig);
#else
bool portShared = false;
#endif
serialPort_t *spektrumPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, spektrumDataReceive, SPEKTRUM_BAUDRATE, portShared ? MODE_RXTX : MODE_RX, SERIAL_NOT_INVERTED);
#ifdef TELEMETRY
if (portShared) {
telemetrySharedPort = spektrumPort;
}
#endif
return spektrumPort != NULL;
}
// Receive ISR callback
static void spektrumDataReceive(uint16_t c)
@ -261,5 +211,53 @@ void spektrumBind(rxConfig_t *rxConfig)
}
#endif // SPEKTRUM_BIND
bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{
rxRuntimeConfigPtr = rxRuntimeConfig;
switch (rxConfig->serialrx_provider) {
case SERIALRX_SPEKTRUM2048:
// 11 bit frames
spek_chan_shift = 3;
spek_chan_mask = 0x07;
spekHiRes = true;
rxRuntimeConfig->channelCount = SPEKTRUM_2048_CHANNEL_COUNT;
rxRuntimeConfig->rxRefreshRate = 11000;
break;
case SERIALRX_SPEKTRUM1024:
// 10 bit frames
spek_chan_shift = 2;
spek_chan_mask = 0x03;
spekHiRes = false;
rxRuntimeConfig->channelCount = SPEKTRUM_1024_CHANNEL_COUNT;
rxRuntimeConfig->rxRefreshRate = 22000;
break;
}
rxRuntimeConfig->rcReadRawFunc = spektrumReadRawRC;
rxRuntimeConfig->rcFrameStatusFunc = spektrumFrameStatus;
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
if (!portConfig) {
return false;
}
#ifdef TELEMETRY
bool portShared = telemetryCheckRxPortShared(portConfig);
#else
bool portShared = false;
#endif
serialPort_t *spektrumPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, spektrumDataReceive, SPEKTRUM_BAUDRATE, portShared ? MODE_RXTX : MODE_RX, SERIAL_NOT_INVERTED);
#ifdef TELEMETRY
if (portShared) {
telemetrySharedPort = spektrumPort;
}
#endif
return spektrumPort != NULL;
}
#endif // SERIAL_RX

View file

@ -52,41 +52,6 @@ static bool sumdFrameDone = false;
static uint16_t sumdChannels[SUMD_MAX_CHANNEL];
static uint16_t crc;
static void sumdDataReceive(uint16_t c);
static uint16_t sumdReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan);
bool sumdInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{
UNUSED(rxConfig);
rxRuntimeConfig->channelCount = SUMD_MAX_CHANNEL;
rxRuntimeConfig->rxRefreshRate = 11000;
rxRuntimeConfig->rcReadRawFunc = sumdReadRawRC;
rxRuntimeConfig->rcFrameStatusFunc = sumdFrameStatus;
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
if (!portConfig) {
return false;
}
#ifdef TELEMETRY
bool portShared = telemetryCheckRxPortShared(portConfig);
#else
bool portShared = false;
#endif
serialPort_t *sumdPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, sumdDataReceive, SUMD_BAUDRATE, portShared ? MODE_RXTX : MODE_RX, SERIAL_NOT_INVERTED);
#ifdef TELEMETRY
if (portShared) {
telemetrySharedPort = sumdPort;
}
#endif
return sumdPort != NULL;
}
#define CRC_POLYNOME 0x1021
// CRC calculation, adds a 8 bit unsigned to 16 bit crc
@ -194,4 +159,36 @@ static uint16_t sumdReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t
UNUSED(rxRuntimeConfig);
return sumdChannels[chan] / 8;
}
bool sumdInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{
UNUSED(rxConfig);
rxRuntimeConfig->channelCount = SUMD_MAX_CHANNEL;
rxRuntimeConfig->rxRefreshRate = 11000;
rxRuntimeConfig->rcReadRawFunc = sumdReadRawRC;
rxRuntimeConfig->rcFrameStatusFunc = sumdFrameStatus;
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
if (!portConfig) {
return false;
}
#ifdef TELEMETRY
bool portShared = telemetryCheckRxPortShared(portConfig);
#else
bool portShared = false;
#endif
serialPort_t *sumdPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, sumdDataReceive, SUMD_BAUDRATE, portShared ? MODE_RXTX : MODE_RX, SERIAL_NOT_INVERTED);
#ifdef TELEMETRY
if (portShared) {
telemetrySharedPort = sumdPort;
}
#endif
return sumdPort != NULL;
}
#endif

View file

@ -58,41 +58,6 @@ static uint32_t sumhChannels[SUMH_MAX_CHANNEL_COUNT];
static serialPort_t *sumhPort;
static void sumhDataReceive(uint16_t c);
static uint16_t sumhReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan);
bool sumhInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{
UNUSED(rxConfig);
rxRuntimeConfig->channelCount = SUMH_MAX_CHANNEL_COUNT;
rxRuntimeConfig->rxRefreshRate = 11000;
rxRuntimeConfig->rcReadRawFunc = sumhReadRawRC;
rxRuntimeConfig->rcFrameStatusFunc = sumhFrameStatus;
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
if (!portConfig) {
return false;
}
#ifdef TELEMETRY
bool portShared = telemetryCheckRxPortShared(portConfig);
#else
bool portShared = false;
#endif
sumhPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, sumhDataReceive, SUMH_BAUDRATE, portShared ? MODE_RXTX : MODE_RX, SERIAL_NOT_INVERTED);
#ifdef TELEMETRY
if (portShared) {
telemetrySharedPort = sumhPort;
}
#endif
return sumhPort != NULL;
}
// Receive ISR callback
static void sumhDataReceive(uint16_t c)
@ -148,4 +113,36 @@ static uint16_t sumhReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t
return sumhChannels[chan];
}
bool sumhInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{
UNUSED(rxConfig);
rxRuntimeConfig->channelCount = SUMH_MAX_CHANNEL_COUNT;
rxRuntimeConfig->rxRefreshRate = 11000;
rxRuntimeConfig->rcReadRawFunc = sumhReadRawRC;
rxRuntimeConfig->rcFrameStatusFunc = sumhFrameStatus;
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
if (!portConfig) {
return false;
}
#ifdef TELEMETRY
bool portShared = telemetryCheckRxPortShared(portConfig);
#else
bool portShared = false;
#endif
sumhPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, sumhDataReceive, SUMH_BAUDRATE, portShared ? MODE_RXTX : MODE_RX, SERIAL_NOT_INVERTED);
#ifdef TELEMETRY
if (portShared) {
telemetrySharedPort = sumhPort;
}
#endif
return sumhPort != NULL;
}
#endif

View file

@ -87,66 +87,6 @@ static uint8_t xBusProvider;
static volatile uint8_t xBusFrame[XBUS_RJ01_FRAME_SIZE];
static uint16_t xBusChannelData[XBUS_RJ01_CHANNEL_COUNT];
static void xBusDataReceive(uint16_t c);
static uint16_t xBusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan);
bool xBusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{
uint32_t baudRate;
switch (rxConfig->serialrx_provider) {
case SERIALRX_XBUS_MODE_B:
rxRuntimeConfig->channelCount = XBUS_CHANNEL_COUNT;
xBusFrameReceived = false;
xBusDataIncoming = false;
xBusFramePosition = 0;
baudRate = XBUS_BAUDRATE;
xBusFrameLength = XBUS_FRAME_SIZE;
xBusChannelCount = XBUS_CHANNEL_COUNT;
xBusProvider = SERIALRX_XBUS_MODE_B;
break;
case SERIALRX_XBUS_MODE_B_RJ01:
rxRuntimeConfig->channelCount = XBUS_RJ01_CHANNEL_COUNT;
xBusFrameReceived = false;
xBusDataIncoming = false;
xBusFramePosition = 0;
baudRate = XBUS_RJ01_BAUDRATE;
xBusFrameLength = XBUS_RJ01_FRAME_SIZE;
xBusChannelCount = XBUS_RJ01_CHANNEL_COUNT;
xBusProvider = SERIALRX_XBUS_MODE_B_RJ01;
break;
default:
return false;
break;
}
rxRuntimeConfig->rxRefreshRate = 11000;
rxRuntimeConfig->rcReadRawFunc = xBusReadRawRC;
rxRuntimeConfig->rcFrameStatusFunc = xBusFrameStatus;
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
if (!portConfig) {
return false;
}
#ifdef TELEMETRY
bool portShared = telemetryCheckRxPortShared(portConfig);
#else
bool portShared = false;
#endif
serialPort_t *xBusPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, xBusDataReceive, baudRate, portShared ? MODE_RXTX : MODE_RX, SERIAL_NOT_INVERTED);
#ifdef TELEMETRY
if (portShared) {
telemetrySharedPort = xBusPort;
}
#endif
return xBusPort != NULL;
}
// The xbus mode B CRC calculations
static uint16_t xBusCRC16(uint16_t crc, uint8_t value)
{
@ -335,4 +275,61 @@ static uint16_t xBusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t
return data;
}
bool xBusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{
uint32_t baudRate;
switch (rxConfig->serialrx_provider) {
case SERIALRX_XBUS_MODE_B:
rxRuntimeConfig->channelCount = XBUS_CHANNEL_COUNT;
xBusFrameReceived = false;
xBusDataIncoming = false;
xBusFramePosition = 0;
baudRate = XBUS_BAUDRATE;
xBusFrameLength = XBUS_FRAME_SIZE;
xBusChannelCount = XBUS_CHANNEL_COUNT;
xBusProvider = SERIALRX_XBUS_MODE_B;
break;
case SERIALRX_XBUS_MODE_B_RJ01:
rxRuntimeConfig->channelCount = XBUS_RJ01_CHANNEL_COUNT;
xBusFrameReceived = false;
xBusDataIncoming = false;
xBusFramePosition = 0;
baudRate = XBUS_RJ01_BAUDRATE;
xBusFrameLength = XBUS_RJ01_FRAME_SIZE;
xBusChannelCount = XBUS_RJ01_CHANNEL_COUNT;
xBusProvider = SERIALRX_XBUS_MODE_B_RJ01;
break;
default:
return false;
break;
}
rxRuntimeConfig->rxRefreshRate = 11000;
rxRuntimeConfig->rcReadRawFunc = xBusReadRawRC;
rxRuntimeConfig->rcFrameStatusFunc = xBusFrameStatus;
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
if (!portConfig) {
return false;
}
#ifdef TELEMETRY
bool portShared = telemetryCheckRxPortShared(portConfig);
#else
bool portShared = false;
#endif
serialPort_t *xBusPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, xBusDataReceive, baudRate, portShared ? MODE_RXTX : MODE_RX, SERIAL_NOT_INVERTED);
#ifdef TELEMETRY
if (portShared) {
telemetrySharedPort = xBusPort;
}
#endif
return xBusPort != NULL;
}
#endif