Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-14 20:10:18 +03:00
Code Issues Wiki Activity

Rework of the FrSky SPI RX code.

Browse source
This commit is contained in:
mikeller 2017-12-26 10:50:15 +13:00
parent ffe43ed8ce
commit 2d3105f375
9 changed files with 348 additions and 339 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/interface/cli.c
View file

@ -2143,7 +2143,7 @@ void cliFrSkyBind(char *cmdline){
#ifdef USE_RX_FRSKY_SPI #ifdef USE_RX_FRSKY_SPI
case RX_SPI_FRSKY_D: case RX_SPI_FRSKY_D:
case RX_SPI_FRSKY_X: case RX_SPI_FRSKY_X:
frSkyBind(); frSkySpiBind();
cliPrint("Binding..."); cliPrint("Binding...");

12
src/main/rx/cc2500_frsky_common.h
View file

@ -17,11 +17,9 @@
#pragma once #pragma once
//#include <stdbool.h> #include "pg/pg.h"
//#include <stdint.h>
//#include "rx.h" #include "rx/rx_spi.h"
//#include "rx_spi.h"
typedef struct rxFrSkySpiConfig_s { typedef struct rxFrSkySpiConfig_s {
bool autoBind; bool autoBind;
@ -33,4 +31,8 @@ typedef struct rxFrSkySpiConfig_s {
PG_DECLARE(rxFrSkySpiConfig_t, rxFrSkySpiConfig); PG_DECLARE(rxFrSkySpiConfig_t, rxFrSkySpiConfig);
void frSkyBind(void); void frSkySpiInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
rx_spi_received_e frSkySpiDataReceived(uint8_t *packet);
void frSkySpiSetRcData(uint16_t *rcData, const uint8_t *payload);
void frSkySpiBind(void);

170
src/main/rx/cc2500_frsky_d.c
View file

@ -38,68 +38,49 @@
#include "fc/config.h" #include "fc/config.h"
#include "rx/rx.h"
#include "rx/rx_spi.h"
#include "rx/cc2500_frsky_common.h" #include "rx/cc2500_frsky_common.h"
#include "rx/cc2500_frsky_shared.h" #include "rx/cc2500_frsky_shared.h"
#include "rx/cc2500_frsky_d.h"
#include "sensors/battery.h" #include "sensors/battery.h"
#include "telemetry/frsky.h" #include "telemetry/frsky.h"
#define RC_CHANNEL_COUNT 8 #include "cc2500_frsky_d.h"
#define MAX_MISSING_PKT 100
#define DEBUG_DATA_ERROR_COUNT 0 #if defined(USE_RX_FRSKY_SPI_TELEMETRY)
#define SYNC 9000
#define FS_THR 960
static uint32_t missingPackets;
static uint8_t cnt;
static int32_t t_out;
static timeUs_t lastPacketReceivedTime;
static uint8_t protocolState;
static uint32_t start_time;
static uint16_t dataErrorCount = 0;
#if defined(USE_RX_FRSKY_SPI_PA_LNA)
static uint8_t pass;
#endif
#ifdef USE_RX_FRSKY_SPI_TELEMETRY
static uint8_t frame[20]; static uint8_t frame[20];
static uint8_t telemetry_id; static uint8_t telemetryId;
static uint32_t telemetryTime;
#if defined(USE_TELEMETRY_FRSKY) #if defined(USE_TELEMETRY_FRSKY)
#define MAX_SERIAL_BYTES 64 #define MAX_SERIAL_BYTES 64
static uint8_t hub_index;
static uint8_t idxx = 0; static uint8_t hubIndex;
static uint8_t idx_ok = 0; static uint8_t telemetryInxdex = 0;
static uint8_t telemetry_expected_id = 0; static uint8_t serialBuffer[MAX_SERIAL_BYTES]; // buffer for telemetry serial data
static uint8_t srx_data[MAX_SERIAL_BYTES]; // buffer for telemetry serial data
#endif
#endif #endif
#endif // USE_RX_FRSKY_SPI_TELEMETRY
#if defined(USE_RX_FRSKY_SPI_TELEMETRY) #if defined(USE_RX_FRSKY_SPI_TELEMETRY)
#if defined(USE_TELEMETRY_FRSKY) #if defined(USE_TELEMETRY_FRSKY)
static uint8_t frsky_append_hub_data(uint8_t *buf) static uint8_t appendFrSkyHubData(uint8_t *buf)
{ {
if (telemetry_id == telemetry_expected_id) static uint8_t telemetryIndexAcknowledged = 0;
idx_ok = idxx; static uint8_t telemetryIndexExpected = 0;
else // rx re-requests last packet
idxx = idx_ok;
telemetry_expected_id = (telemetry_id + 1) & 0x1F; if (telemetryId == telemetryIndexExpected) {
telemetryIndexAcknowledged = telemetryInxdex;
} else { // rx re-requests last packet
telemetryInxdex = telemetryIndexAcknowledged;
}
telemetryIndexExpected = (telemetryId + 1) & 0x1F;
uint8_t index = 0; uint8_t index = 0;
for (uint8_t i = 0; i < 10; i++) { for (uint8_t i = 0; i < 10; i++) {
if (idxx == hub_index) { if (telemetryInxdex == hubIndex) {
break; break;
} }
buf[i] = srx_data[idxx]; buf[i] = serialBuffer[telemetryInxdex];
idxx = (idxx + 1) & (MAX_SERIAL_BYTES - 1); telemetryInxdex = (telemetryInxdex + 1) & (MAX_SERIAL_BYTES - 1);
index++; index++;
} }
return index; return index;
@ -107,37 +88,36 @@ static uint8_t frsky_append_hub_data(uint8_t *buf)
static void frSkyTelemetryInitFrameSpi(void) static void frSkyTelemetryInitFrameSpi(void)
{ {
hub_index = 0; hubIndex = 0;
idxx = 0; telemetryInxdex = 0;
} }
static void frSkyTelemetryWriteSpi(uint8_t ch) static void frSkyTelemetryWriteSpi(uint8_t ch)
{ {
if (hub_index < MAX_SERIAL_BYTES) { if (hubIndex < MAX_SERIAL_BYTES) {
srx_data[hub_index++] = ch; serialBuffer[hubIndex++] = ch;
} }
} }
#endif #endif
static void telemetry_build_frame(uint8_t *packet) static void buildTelemetryFrame(uint8_t *packet)
{ {
const uint16_t adcExternal1Sample = adcGetChannel(ADC_EXTERNAL1); const uint16_t adcExternal1Sample = adcGetChannel(ADC_EXTERNAL1);
const uint16_t adcRssiSample = adcGetChannel(ADC_RSSI); const uint16_t adcRssiSample = adcGetChannel(ADC_RSSI);
uint8_t bytes_used = 0; uint8_t bytes_used = 0;
telemetry_id = packet[4]; telemetryId = packet[4];
frame[0] = 0x11; // length frame[0] = 0x11; // length
frame[1] = rxFrSkySpiConfig()->bindTxId[0]; frame[1] = rxFrSkySpiConfig()->bindTxId[0];
frame[2] = rxFrSkySpiConfig()->bindTxId[1]; frame[2] = rxFrSkySpiConfig()->bindTxId[1];
frame[3] = (uint8_t)((adcExternal1Sample & 0xff0) >> 4); // A1 frame[3] = (uint8_t)((adcExternal1Sample & 0xff0) >> 4); // A1
frame[4] = (uint8_t)((adcRssiSample & 0xff0) >> 4); // A2 frame[4] = (uint8_t)((adcRssiSample & 0xff0) >> 4); // A2
frame[5] = (uint8_t)RSSI_dBm; frame[5] = (uint8_t)rssiDbm;
#if defined(USE_TELEMETRY_FRSKY) #if defined(USE_TELEMETRY_FRSKY)
bytes_used = frsky_append_hub_data(&frame[8]); bytes_used = appendFrSkyHubData(&frame[8]);
#endif #endif
frame[6] = bytes_used; frame[6] = bytes_used;
frame[7] = telemetry_id; frame[7] = telemetryId;
} }
#endif // USE_RX_FRSKY_SPI_TELEMETRY #endif // USE_RX_FRSKY_SPI_TELEMETRY
#define FRSKY_D_CHANNEL_SCALING (2.0f / 3) #define FRSKY_D_CHANNEL_SCALING (2.0f / 3)
@ -149,7 +129,9 @@ static void decodeChannelPair(uint16_t *channels, const uint8_t *packet, const u
void frSkyDSetRcData(uint16_t *rcData, const uint8_t *packet) void frSkyDSetRcData(uint16_t *rcData, const uint8_t *packet)
{ {
uint16_t channels[RC_CHANNEL_COUNT]; static uint16_t dataErrorCount = 0;
uint16_t channels[RC_CHANNEL_COUNT_FRSKY_D];
bool dataError = false; bool dataError = false;
decodeChannelPair(channels, packet + 6, 4); decodeChannelPair(channels, packet + 6, 4);
@ -157,7 +139,7 @@ void frSkyDSetRcData(uint16_t *rcData, const uint8_t *packet)
decodeChannelPair(channels + 4, packet + 12, 4); decodeChannelPair(channels + 4, packet + 12, 4);
decodeChannelPair(channels + 6, packet + 14, 3); decodeChannelPair(channels + 6, packet + 14, 3);
for (int i = 0; i < RC_CHANNEL_COUNT; i++) { for (int i = 0; i < RC_CHANNEL_COUNT_FRSKY_D; i++) {
if ((channels[i] < 800) || (channels[i] > 2200)) { if ((channels[i] < 800) || (channels[i] > 2200)) {
dataError = true; dataError = true;
@ -166,7 +148,7 @@ void frSkyDSetRcData(uint16_t *rcData, const uint8_t *packet)
} }
if (!dataError) { if (!dataError) {
for (int i = 0; i < RC_CHANNEL_COUNT; i++) { for (int i = 0; i < RC_CHANNEL_COUNT_FRSKY_D; i++) {
rcData[i] = channels[i]; rcData[i] = channels[i];
} }
} else { } else {
@ -174,48 +156,36 @@ void frSkyDSetRcData(uint16_t *rcData, const uint8_t *packet)
} }
} }
rx_spi_received_e frSkyDDataReceived(uint8_t *packet) rx_spi_received_e frSkyDHandlePacket(uint8_t * const packet, uint8_t * const protocolState)
{ {
const timeUs_t currentPacketReceivedTime = micros(); static timeUs_t lastPacketReceivedTime = 0;
static timeUs_t telemetryTimeUs;
static bool ledIsOn;
rx_spi_received_e ret = RX_SPI_RECEIVED_NONE; rx_spi_received_e ret = RX_SPI_RECEIVED_NONE;
switch (protocolState) { const timeUs_t currentPacketReceivedTime = micros();
case STATE_INIT:
if ((millis() - start_time) > 10) {
initialize();
protocolState = STATE_BIND; switch (*protocolState) {
}
break;
case STATE_BIND:
case STATE_BIND_TUNING:
case STATE_BIND_BINDING1:
case STATE_BIND_BINDING2:
case STATE_BIND_COMPLETE:
protocolState = handleBinding(protocolState, packet);
break;
case STATE_STARTING: case STATE_STARTING:
listLength = 47; listLength = 47;
initialiseData(0); initialiseData(0);
protocolState = STATE_UPDATE; *protocolState = STATE_UPDATE;
nextChannel(1); nextChannel(1);
cc2500Strobe(CC2500_SRX); cc2500Strobe(CC2500_SRX);
ret = RX_SPI_RECEIVED_BIND;
break; break;
case STATE_UPDATE: case STATE_UPDATE:
lastPacketReceivedTime = currentPacketReceivedTime; lastPacketReceivedTime = currentPacketReceivedTime;
protocolState = STATE_DATA; *protocolState = STATE_DATA;
if (checkBindRequested(false)) { if (checkBindRequested(false)) {
lastPacketReceivedTime = 0; lastPacketReceivedTime = 0;
t_out = 50; timeoutUs = 50;
missingPackets = 0; missingPackets = 0;
protocolState = STATE_INIT; *protocolState = STATE_INIT;
break; break;
} }
@ -228,7 +198,7 @@ rx_spi_received_e frSkyDDataReceived(uint8_t *packet)
cc2500ReadFifo(packet, 20); cc2500ReadFifo(packet, 20);
if (packet[19] & 0x80) { if (packet[19] & 0x80) {
missingPackets = 0; missingPackets = 0;
t_out = 1; timeoutUs = 1;
if (packet[0] == 0x11) { if (packet[0] == 0x11) {
if ((packet[1] == rxFrSkySpiConfig()->bindTxId[0]) && if ((packet[1] == rxFrSkySpiConfig()->bindTxId[0]) &&
(packet[2] == rxFrSkySpiConfig()->bindTxId[1])) { (packet[2] == rxFrSkySpiConfig()->bindTxId[1])) {
@ -236,15 +206,15 @@ rx_spi_received_e frSkyDDataReceived(uint8_t *packet)
nextChannel(1); nextChannel(1);
#if defined(USE_RX_FRSKY_SPI_TELEMETRY) #if defined(USE_RX_FRSKY_SPI_TELEMETRY)
if ((packet[3] % 4) == 2) { if ((packet[3] % 4) == 2) {
telemetryTime = micros(); telemetryTimeUs = micros();
setRssiDbm(packet[18]); setRssiDbm(packet[18]);
telemetry_build_frame(packet); buildTelemetryFrame(packet);
protocolState = STATE_TELEMETRY; *protocolState = STATE_TELEMETRY;
} else } else
#endif #endif
{ {
cc2500Strobe(CC2500_SRX); cc2500Strobe(CC2500_SRX);
protocolState = STATE_UPDATE; *protocolState = STATE_UPDATE;
} }
ret = RX_SPI_RECEIVED_DATA; ret = RX_SPI_RECEIVED_DATA;
lastPacketReceivedTime = currentPacketReceivedTime; lastPacketReceivedTime = currentPacketReceivedTime;
@ -254,24 +224,19 @@ rx_spi_received_e frSkyDDataReceived(uint8_t *packet)
} }
} }
if (cmpTimeUs(currentPacketReceivedTime, lastPacketReceivedTime) > (t_out * SYNC)) { if (cmpTimeUs(currentPacketReceivedTime, lastPacketReceivedTime) > (timeoutUs * SYNC_DELAY_MAX)) {
#if defined(USE_RX_FRSKY_SPI_PA_LNA) #if defined(USE_RX_FRSKY_SPI_PA_LNA)
RxEnable(); RxEnable();
#endif #endif
if (t_out == 1) { if (timeoutUs == 1) {
#if defined(USE_RX_FRSKY_SPI_PA_LNA) && defined(USE_RX_FRSKY_SPI_DIVERSITY) // SE4311 chip #if defined(USE_RX_FRSKY_SPI_PA_LNA) && defined(USE_RX_FRSKY_SPI_DIVERSITY) // SE4311 chip
if (missingPackets >= 2) { if (missingPackets >= 2) {
if (pass & 0x01) { switchAntennae();
IOHi(antSelPin);
} else {
IOLo(antSelPin);
}
pass++;
} }
#endif #endif
if (missingPackets > MAX_MISSING_PKT) { if (missingPackets > MAX_MISSING_PKT) {
t_out = 50; timeoutUs = 50;
#if defined(USE_RX_FRSKY_SPI_TELEMETRY) #if defined(USE_RX_FRSKY_SPI_TELEMETRY)
setRssiFiltered(0, RSSI_SOURCE_RX_PROTOCOL); setRssiFiltered(0, RSSI_SOURCE_RX_PROTOCOL);
@ -281,11 +246,12 @@ rx_spi_received_e frSkyDDataReceived(uint8_t *packet)
missingPackets++; missingPackets++;
nextChannel(1); nextChannel(1);
} else { } else {
if (cnt++ & 0x01) { if (ledIsOn) {
IOLo(frSkyLedPin); IOLo(frSkyLedPin);
} else { } else {
IOHi(frSkyLedPin); IOHi(frSkyLedPin);
} }
ledIsOn = !ledIsOn;
#if defined(USE_RX_FRSKY_SPI_TELEMETRY) #if defined(USE_RX_FRSKY_SPI_TELEMETRY)
setRssiUnfiltered(0, RSSI_SOURCE_RX_PROTOCOL); setRssiUnfiltered(0, RSSI_SOURCE_RX_PROTOCOL);
@ -294,12 +260,12 @@ rx_spi_received_e frSkyDDataReceived(uint8_t *packet)
} }
cc2500Strobe(CC2500_SRX); cc2500Strobe(CC2500_SRX);
protocolState = STATE_UPDATE; *protocolState = STATE_UPDATE;
} }
break; break;
#if defined(USE_RX_FRSKY_SPI_TELEMETRY) #if defined(USE_RX_FRSKY_SPI_TELEMETRY)
case STATE_TELEMETRY: case STATE_TELEMETRY:
if ((micros() - telemetryTime) >= 1380) { if (cmpTimeUs(micros(), telemetryTimeUs) >= 1380) {
cc2500Strobe(CC2500_SIDLE); cc2500Strobe(CC2500_SIDLE);
cc2500SetPower(6); cc2500SetPower(6);
cc2500Strobe(CC2500_SFRX); cc2500Strobe(CC2500_SFRX);
@ -308,7 +274,7 @@ rx_spi_received_e frSkyDDataReceived(uint8_t *packet)
#endif #endif
cc2500Strobe(CC2500_SIDLE); cc2500Strobe(CC2500_SIDLE);
cc2500WriteFifo(frame, frame[0] + 1); cc2500WriteFifo(frame, frame[0] + 1);
protocolState = STATE_DATA; *protocolState = STATE_DATA;
ret = RX_SPI_RECEIVED_DATA; ret = RX_SPI_RECEIVED_DATA;
lastPacketReceivedTime = currentPacketReceivedTime; lastPacketReceivedTime = currentPacketReceivedTime;
} }
@ -317,27 +283,15 @@ rx_spi_received_e frSkyDDataReceived(uint8_t *packet)
#endif #endif
} }
return ret; return ret;
} }
void frSkyDInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) void frSkyDInit(void)
{ {
UNUSED(rxConfig);
rxRuntimeConfig->channelCount = RC_CHANNEL_COUNT;
protocolState = STATE_INIT;
lastPacketReceivedTime = 0;
missingPackets = 0;
t_out = 50;
#if defined(USE_RX_FRSKY_SPI_TELEMETRY) && defined(USE_TELEMETRY_FRSKY) #if defined(USE_RX_FRSKY_SPI_TELEMETRY) && defined(USE_TELEMETRY_FRSKY)
initFrSkyExternalTelemetry(&frSkyTelemetryInitFrameSpi, initFrSkyExternalTelemetry(&frSkyTelemetryInitFrameSpi,
&frSkyTelemetryWriteSpi); &frSkyTelemetryWriteSpi);
#endif #endif
frskySpiRxSetup(rxConfig->rx_spi_protocol);
start_time = millis();
} }
#endif #endif

12
src/main/rx/cc2500_frsky_d.h
View file

@ -17,10 +17,12 @@
#pragma once #pragma once
#include "rx.h" #include "rx/rx.h"
#include "rx_spi.h" #include "rx/rx_spi.h"
#define RC_CHANNEL_COUNT_FRSKY_D 8
void frSkyDInit(const struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig);
void frSkyDSetRcData(uint16_t *rcData, const uint8_t *payload); void frSkyDSetRcData(uint16_t *rcData, const uint8_t *payload);
rx_spi_received_e frSkyDDataReceived(uint8_t *payload);
void frSkyBind(void); void frSkyDInit(void);
rx_spi_received_e frSkyDHandlePacket(uint8_t * const packet, uint8_t * const protocolState);

136
src/main/rx/cc2500_frsky_shared.c
View file

@ -33,20 +33,36 @@
#include "pg/pg_ids.h" #include "pg/pg_ids.h"
#include "rx/rx.h" #include "rx/rx.h"
#include "rx/rx_spi.h"
#include "rx/cc2500_frsky_common.h" #include "rx/cc2500_frsky_common.h"
#include "rx/cc2500_frsky_d.h"
#include "rx/cc2500_frsky_x.h"
#include "cc2500_frsky_shared.h" #include "cc2500_frsky_shared.h"
static rx_spi_protocol_e spiProtocol; static rx_spi_protocol_e spiProtocol;
static timeMs_t start_time;
static uint8_t protocolState;
uint32_t missingPackets;
timeDelta_t timeoutUs;
static uint8_t calData[255][3]; static uint8_t calData[255][3];
static timeMs_t timeTunedMs; static timeMs_t timeTunedMs;
uint8_t listLength; uint8_t listLength;
static uint8_t bindIdx; static uint8_t bindIdx;
static int8_t bindOffset; static int8_t bindOffset;
static bool lastBindPinStatus; static bool lastBindPinStatus;
bool bindRequested = false;
static bool bindRequested;
typedef uint8_t handlePacketFn(uint8_t * const packet, uint8_t * const protocolState);
typedef void setRcDataFn(uint16_t *rcData, const uint8_t *payload);
static handlePacketFn *handlePacket;
static setRcDataFn *setRcData;
IO_t gdoPin; IO_t gdoPin;
static IO_t bindPin = DEFIO_IO(NONE); static IO_t bindPin = DEFIO_IO(NONE);
@ -55,11 +71,11 @@ IO_t frSkyLedPin;
#if defined(USE_RX_FRSKY_SPI_PA_LNA) #if defined(USE_RX_FRSKY_SPI_PA_LNA)
static IO_t txEnPin; static IO_t txEnPin;
static IO_t rxLnaEnPin; static IO_t rxLnaEnPin;
IO_t antSelPin; static IO_t antSelPin;
#endif #endif
#ifdef USE_RX_FRSKY_SPI_TELEMETRY #ifdef USE_RX_FRSKY_SPI_TELEMETRY
int16_t RSSI_dBm; int16_t rssiDbm;
#endif #endif
PG_REGISTER_WITH_RESET_TEMPLATE(rxFrSkySpiConfig_t, rxFrSkySpiConfig, PG_RX_FRSKY_SPI_CONFIG, 0); PG_REGISTER_WITH_RESET_TEMPLATE(rxFrSkySpiConfig_t, rxFrSkySpiConfig, PG_RX_FRSKY_SPI_CONFIG, 0);
@ -79,12 +95,12 @@ PG_RESET_TEMPLATE(rxFrSkySpiConfig_t, rxFrSkySpiConfig,
void setRssiDbm(uint8_t value) void setRssiDbm(uint8_t value)
{ {
if (value >= 128) { if (value >= 128) {
RSSI_dBm = ((((uint16_t)value) * 18) >> 5) - 82; rssiDbm = ((((uint16_t)value) * 18) >> 5) - 82;
} else { } else {
RSSI_dBm = ((((uint16_t)value) * 18) >> 5) + 65; rssiDbm = ((((uint16_t)value) * 18) >> 5) + 65;
} }
setRssiUnfiltered(constrain(RSSI_dBm << 3, 0, 1023), RSSI_SOURCE_RX_PROTOCOL); setRssiUnfiltered(constrain(rssiDbm << 3, 0, 1023), RSSI_SOURCE_RX_PROTOCOL);
} }
#endif // USE_RX_FRSKY_SPI_TELEMETRY #endif // USE_RX_FRSKY_SPI_TELEMETRY
@ -100,12 +116,12 @@ void TxEnable(void)
} }
#endif #endif
void frSkyBind(void) void frSkySpiBind(void)
{ {
bindRequested = true; bindRequested = true;
} }
void initialize() { static void initialise() {
cc2500Reset(); cc2500Reset();
cc2500WriteReg(CC2500_02_IOCFG0, 0x01); cc2500WriteReg(CC2500_02_IOCFG0, 0x01);
cc2500WriteReg(CC2500_17_MCSM1, 0x0C); cc2500WriteReg(CC2500_17_MCSM1, 0x0C);
@ -136,7 +152,8 @@ void initialize() {
cc2500WriteReg(CC2500_03_FIFOTHR, 0x07); cc2500WriteReg(CC2500_03_FIFOTHR, 0x07);
cc2500WriteReg(CC2500_09_ADDR, 0x00); cc2500WriteReg(CC2500_09_ADDR, 0x00);
if (spiProtocol == RX_SPI_FRSKY_D) { switch (spiProtocol) {
case RX_SPI_FRSKY_D:
cc2500WriteReg(CC2500_06_PKTLEN, 0x19); cc2500WriteReg(CC2500_06_PKTLEN, 0x19);
cc2500WriteReg(CC2500_08_PKTCTRL0, 0x05); cc2500WriteReg(CC2500_08_PKTCTRL0, 0x05);
cc2500WriteReg(CC2500_0B_FSCTRL1, 0x08); cc2500WriteReg(CC2500_0B_FSCTRL1, 0x08);
@ -144,7 +161,9 @@ void initialize() {
cc2500WriteReg(CC2500_11_MDMCFG3, 0x39); cc2500WriteReg(CC2500_11_MDMCFG3, 0x39);
cc2500WriteReg(CC2500_12_MDMCFG2, 0x11); cc2500WriteReg(CC2500_12_MDMCFG2, 0x11);
cc2500WriteReg(CC2500_15_DEVIATN, 0x42); cc2500WriteReg(CC2500_15_DEVIATN, 0x42);
} else {
break;
case RX_SPI_FRSKY_X:
cc2500WriteReg(CC2500_06_PKTLEN, 0x1E); cc2500WriteReg(CC2500_06_PKTLEN, 0x1E);
cc2500WriteReg(CC2500_08_PKTCTRL0, 0x01); cc2500WriteReg(CC2500_08_PKTCTRL0, 0x01);
cc2500WriteReg(CC2500_0B_FSCTRL1, 0x0A); cc2500WriteReg(CC2500_0B_FSCTRL1, 0x0A);
@ -152,10 +171,15 @@ void initialize() {
cc2500WriteReg(CC2500_11_MDMCFG3, 0x61); cc2500WriteReg(CC2500_11_MDMCFG3, 0x61);
cc2500WriteReg(CC2500_12_MDMCFG2, 0x13); cc2500WriteReg(CC2500_12_MDMCFG2, 0x13);
cc2500WriteReg(CC2500_15_DEVIATN, 0x51); cc2500WriteReg(CC2500_15_DEVIATN, 0x51);
break;
default:
break;
} }
for(uint8_t c=0;c<0xFF;c++) for(unsigned c = 0;c < 0xFF; c++)
{//calibrate all channels { //calibrate all channels
cc2500Strobe(CC2500_SIDLE); cc2500Strobe(CC2500_SIDLE);
cc2500WriteReg(CC2500_0A_CHANNR, c); cc2500WriteReg(CC2500_0A_CHANNR, c);
cc2500Strobe(CC2500_SCAL); cc2500Strobe(CC2500_SCAL);
@ -342,9 +366,19 @@ bool checkBindRequested(bool reset)
} }
} }
uint8_t handleBinding(uint8_t protocolState, uint8_t *packet) rx_spi_received_e frSkySpiDataReceived(uint8_t *packet)
{ {
rx_spi_received_e ret = RX_SPI_RECEIVED_NONE;
switch (protocolState) { switch (protocolState) {
case STATE_INIT:
if ((millis() - start_time) > 10) {
initialise();
protocolState = STATE_BIND;
}
break;
case STATE_BIND: case STATE_BIND:
if (checkBindRequested(true) || rxFrSkySpiConfig()->autoBind) { if (checkBindRequested(true) || rxFrSkySpiConfig()->autoBind) {
IOHi(frSkyLedPin); IOHi(frSkyLedPin);
@ -392,12 +426,22 @@ uint8_t handleBinding(uint8_t protocolState, uint8_t *packet)
} }
} }
ret = RX_SPI_RECEIVED_BIND;
protocolState = STATE_STARTING; protocolState = STATE_STARTING;
break;
default:
ret = handlePacket(packet, &protocolState);
break; break;
} }
return protocolState; return ret;
}
void frSkySpiSetRcData(uint16_t *rcData, const uint8_t *payload)
{
setRcData(rcData, payload);
} }
void nextChannel(uint8_t skip) void nextChannel(uint8_t skip)
@ -421,9 +465,62 @@ void nextChannel(uint8_t skip)
} }
} }
void frskySpiRxSetup(rx_spi_protocol_e protocol) #if defined(USE_RX_FRSKY_SPI_PA_LNA) && defined(USE_RX_FRSKY_SPI_DIVERSITY)
void switchAntennae(void)
{ {
spiProtocol = protocol; static bool alternativeAntennaSelected = true;
if (alternativeAntennaSelected) {
IOLo(antSelPin);
} else {
IOHi(antSelPin);
}
alternativeAntennaSelected = !alternativeAntennaSelected;
}
#endif
static bool frSkySpiDetect(void)
{
uint8_t tmp[2];
tmp[0] = cc2500ReadReg(CC2500_30_PARTNUM | CC2500_READ_BURST); //CC2500 read registers chip part num
tmp[1] = cc2500ReadReg(CC2500_31_VERSION | CC2500_READ_BURST); //CC2500 read registers chip version
if (tmp[0] == 0x80 && tmp[1]==0x03){
return true;
}
return false;
}
void frSkySpiInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{
if (!frSkySpiDetect()) {
rxRuntimeConfig->channelCount = 0;
return;
}
spiProtocol = rxConfig->rx_spi_protocol;
switch (spiProtocol) {
case RX_SPI_FRSKY_D:
rxRuntimeConfig->channelCount = RC_CHANNEL_COUNT_FRSKY_D;
handlePacket = frSkyDHandlePacket;
setRcData = frSkyDSetRcData;
frSkyDInit();
break;
case RX_SPI_FRSKY_X:
rxRuntimeConfig->channelCount = RC_CHANNEL_COUNT_FRSKY_X;
handlePacket = frSkyXHandlePacket;
setRcData = frSkyXSetRcData;
frSkyXInit();
break;
default:
break;
}
#if defined(USE_RX_FRSKY_SPI_TELEMETRY) #if defined(USE_RX_FRSKY_SPI_TELEMETRY)
if (rssiSource == RSSI_SOURCE_NONE) { if (rssiSource == RSSI_SOURCE_NONE) {
@ -467,7 +564,10 @@ void frskySpiRxSetup(rx_spi_protocol_e protocol)
RxEnable(); RxEnable();
#endif // USE_RX_FRSKY_SPI_PA_LNA #endif // USE_RX_FRSKY_SPI_PA_LNA
// if(!frSkySpiDetect())//detect spi working routine missingPackets = 0;
// return; timeoutUs = 50;
start_time = millis();
protocolState = STATE_INIT;
} }
#endif #endif

17
src/main/rx/cc2500_frsky_shared.h
View file

@ -23,7 +23,9 @@
#define DEBUG_DATA_ERROR_COUNT 0 #define DEBUG_DATA_ERROR_COUNT 0
#define SYNC 9000 #define SYNC_DELAY_MAX 9000
#define MAX_MISSING_PKT 100
enum { enum {
STATE_INIT = 0, STATE_INIT = 0,
@ -39,26 +41,23 @@ enum {
STATE_RESUME, STATE_RESUME,
}; };
extern bool bindRequested;
extern uint8_t listLength; extern uint8_t listLength;
extern int16_t RSSI_dBm; extern uint32_t missingPackets;
extern timeDelta_t timeoutUs;
extern int16_t rssiDbm;
extern IO_t gdoPin; extern IO_t gdoPin;
extern IO_t frSkyLedPin; extern IO_t frSkyLedPin;
extern IO_t antSelPin;
void setRssiDbm(uint8_t value); void setRssiDbm(uint8_t value);
void frskySpiRxSetup(rx_spi_protocol_e protocol);
void RxEnable(void); void RxEnable(void);
void TxEnable(void); void TxEnable(void);
void initialize(); void switchAntennae(void);
void initialiseData(uint8_t adr); void initialiseData(uint8_t adr);
bool checkBindRequested(bool reset); bool checkBindRequested(bool reset);
uint8_t handleBinding(uint8_t protocolState, uint8_t *packet);
void nextChannel(uint8_t skip); void nextChannel(uint8_t skip);

302
src/main/rx/cc2500_frsky_x.c
View file

@ -39,19 +39,16 @@
#include "fc/config.h" #include "fc/config.h"
#include "rx/rx.h"
#include "rx/rx_spi.h"
#include "rx/cc2500_frsky_common.h" #include "rx/cc2500_frsky_common.h"
#include "rx/cc2500_frsky_shared.h" #include "rx/cc2500_frsky_shared.h"
#include "rx/cc2500_frsky_x.h"
#include "sensors/battery.h" #include "sensors/battery.h"
#include "telemetry/smartport.h" #include "telemetry/smartport.h"
#define RC_CHANNEL_COUNT 16 #include "cc2500_frsky_x.h"
const uint16_t CRCTable[] = { const uint16_t crcTable[] = {
0x0000,0x1189,0x2312,0x329b,0x4624,0x57ad,0x6536,0x74bf, 0x0000,0x1189,0x2312,0x329b,0x4624,0x57ad,0x6536,0x74bf,
0x8c48,0x9dc1,0xaf5a,0xbed3,0xca6c,0xdbe5,0xe97e,0xf8f7, 0x8c48,0x9dc1,0xaf5a,0xbed3,0xca6c,0xdbe5,0xe97e,0xf8f7,
0x1081,0x0108,0x3393,0x221a,0x56a5,0x472c,0x75b7,0x643e, 0x1081,0x0108,0x3393,0x221a,0x56a5,0x472c,0x75b7,0x643e,
@ -128,74 +125,40 @@ typedef struct telemetryPayload_s {
uint8_t crc[2]; uint8_t crc[2];
} __attribute__ ((__packed__)) telemetryPayload_t; } __attribute__ ((__packed__)) telemetryPayload_t;
static telemetryBuffer_t telemetryRxBuffer[TELEMETRY_SEQUENCE_LENGTH]; #if defined(USE_RX_FRSKY_SPI_TELEMETRY)
static telemetryData_t telemetryTxBuffer[TELEMETRY_SEQUENCE_LENGTH]; static telemetryData_t telemetryTxBuffer[TELEMETRY_SEQUENCE_LENGTH];
#endif
static uint8_t remoteProcessedId = 0;
static uint8_t remoteAckId = 0;
static uint8_t remoteToProcessIndex = 0;
static uint8_t localPacketId;
static telemetrySequenceMarker_t responseToSend; static telemetrySequenceMarker_t responseToSend;
static uint8_t ccLen; #if defined(USE_RX_FRSKY_SPI_TELEMETRY)
static uint32_t missingPackets;
static uint8_t cnt;
static timeDelta_t t_out;
static timeUs_t packet_timer;
static uint8_t protocolState;
static int16_t word_temp;
static uint32_t start_time;
static bool frame_received;
static uint8_t one_time=1;
static uint8_t chanskip=1;
#if defined(USE_RX_FRSKY_SPI_PA_LNA)
static uint8_t pass;
#endif
static timeDelta_t t_received;
#ifdef USE_RX_FRSKY_SPI_TELEMETRY
static uint8_t frame[20]; static uint8_t frame[20];
static uint8_t telemetryRX;
#if defined(USE_TELEMETRY_SMARTPORT) #if defined(USE_TELEMETRY_SMARTPORT)
static uint8_t telemetryOutReader = 0;
static uint8_t telemetryOutWriter; static uint8_t telemetryOutWriter;
static uint8_t telemetryOutBuffer[TELEMETRY_OUT_BUFFER_SIZE]; static uint8_t telemetryOutBuffer[TELEMETRY_OUT_BUFFER_SIZE];
static bool telemetryEnabled = false; static bool telemetryEnabled = false;
#endif #endif
#endif #endif // USE_RX_FRSKY_SPI_TELEMETRY
bool frskySpiDetect(void)//debug CC2500 spi static uint16_t calculateCrc(uint8_t *data, uint8_t len) {
{
uint8_t tmp[2];
tmp[0] = cc2500ReadReg(CC2500_30_PARTNUM | CC2500_READ_BURST);//Cc2500 read registers chip part num
tmp[1] = cc2500ReadReg(CC2500_31_VERSION | CC2500_READ_BURST);//Cc2500 read registers chip version
if (tmp[0] == 0x80 && tmp[1]==0x03){
return true;
}
return false;
}
static uint16_t crc(uint8_t *data, uint8_t len) {
uint16_t crc = 0; uint16_t crc = 0;
for(uint8_t i=0; i < len; i++) for(uint8_t i=0; i < len; i++)
crc = (crc<<8) ^ (CRCTable[((uint8_t)(crc>>8) ^ *data++) & 0xFF]); crc = (crc<<8) ^ (crcTable[((uint8_t)(crc>>8) ^ *data++) & 0xFF]);
return crc; return crc;
} }
#if defined(USE_RX_FRSKY_SPI_TELEMETRY)
#if defined(USE_TELEMETRY_SMARTPORT) #if defined(USE_TELEMETRY_SMARTPORT)
static uint8_t frsky_append_sport_data(uint8_t *buf) static uint8_t appendSmartPortData(uint8_t *buf)
{ {
static uint8_t telemetryOutReader = 0;
uint8_t index; uint8_t index;
for (index = 0; index < TELEMETRY_DATA_SIZE; index++) { //max 5 bytes in a frame for (index = 0; index < TELEMETRY_DATA_SIZE; index++) { // max 5 bytes in a frame
if(telemetryOutReader == telemetryOutWriter){ //no new data if (telemetryOutReader == telemetryOutWriter){ // no new data
break; break;
} }
buf[index] = telemetryOutBuffer[telemetryOutReader]; buf[index] = telemetryOutBuffer[telemetryOutReader];
@ -206,26 +169,28 @@ static uint8_t frsky_append_sport_data(uint8_t *buf)
} }
#endif #endif
#if defined(USE_RX_FRSKY_SPI_TELEMETRY) static void buildTelemetryFrame(uint8_t *packet)
static void telemetry_build_frame(uint8_t *packet)
{ {
frame[0]=0x0E;//length static uint8_t localPacketId;
frame[1]=rxFrSkySpiConfig()->bindTxId[0];
frame[2]=rxFrSkySpiConfig()->bindTxId[1];
frame[3]=packet[3];
static bool evenRun = false; static bool evenRun = false;
frame[0] = 0x0E;//length
frame[1] = rxFrSkySpiConfig()->bindTxId[0];
frame[2] = rxFrSkySpiConfig()->bindTxId[1];
frame[3] = packet[3];
if (evenRun) { if (evenRun) {
frame[4]=(uint8_t)RSSI_dBm|0x80; frame[4]=(uint8_t)rssiDbm|0x80;
} else { } else {
const uint16_t adcExternal1Sample = adcGetChannel(ADC_EXTERNAL1); const uint16_t adcExternal1Sample = adcGetChannel(ADC_EXTERNAL1);
frame[4]=(uint8_t)((adcExternal1Sample & 0xfe0) >> 5); // A1; frame[4] = (uint8_t)((adcExternal1Sample & 0xfe0) >> 5); // A1;
} }
evenRun = !evenRun; evenRun = !evenRun;
telemetrySequenceMarker_t *inFrameMarker = (telemetrySequenceMarker_t *)&packet[21]; telemetrySequenceMarker_t *inFrameMarker = (telemetrySequenceMarker_t *)&packet[21];
telemetrySequenceMarker_t *outFrameMarker = (telemetrySequenceMarker_t *)&frame[5]; telemetrySequenceMarker_t *outFrameMarker = (telemetrySequenceMarker_t *)&frame[5];
if (inFrameMarker->data.initRequest) {//check syncronization at startup ok if not no sport telemetry if (inFrameMarker->data.initRequest) { // check syncronization at startup ok if not no sport telemetry
outFrameMarker-> raw = 0; outFrameMarker-> raw = 0;
outFrameMarker->data.initRequest = 1; outFrameMarker->data.initRequest = 1;
outFrameMarker->data.initResponse = 1; outFrameMarker->data.initResponse = 1;
@ -243,8 +208,8 @@ static void telemetry_build_frame(uint8_t *packet)
if (localPacketId != (localAckId + 1) % TELEMETRY_SEQUENCE_LENGTH) { if (localPacketId != (localAckId + 1) % TELEMETRY_SEQUENCE_LENGTH) {
outFrameMarker->raw = responseToSend.raw & SEQUENCE_MARKER_REMOTE_PART; outFrameMarker->raw = responseToSend.raw & SEQUENCE_MARKER_REMOTE_PART;
outFrameMarker->data.packetSequenceId = localPacketId; outFrameMarker->data.packetSequenceId = localPacketId;
\
frame[6] = frsky_append_sport_data(&frame[7]); frame[6] = appendSmartPortData(&frame[7]);
memcpy(&telemetryTxBuffer[localPacketId], &frame[6], TELEMETRY_FRAME_SIZE); memcpy(&telemetryTxBuffer[localPacketId], &frame[6], TELEMETRY_FRAME_SIZE);
localPacketId = (localPacketId + 1) % TELEMETRY_SEQUENCE_LENGTH; localPacketId = (localPacketId + 1) % TELEMETRY_SEQUENCE_LENGTH;
@ -252,7 +217,7 @@ static void telemetry_build_frame(uint8_t *packet)
} }
} }
uint16_t lcrc = crc(&frame[3], 10); uint16_t lcrc = calculateCrc(&frame[3], 10);
frame[13]=lcrc>>8; frame[13]=lcrc>>8;
frame[14]=lcrc; frame[14]=lcrc;
} }
@ -294,104 +259,112 @@ void frSkyXSetRcData(uint16_t *rcData, const uint8_t *packet)
{ {
uint16_t c[8]; uint16_t c[8];
c[0] = (uint16_t)((packet[10] <<8)& 0xF00) | packet[9]; c[0] = (uint16_t)((packet[10] << 8) & 0xF00) | packet[9];
c[1] = (uint16_t)((packet[11]<<4)&0xFF0) | (packet[10]>>4); c[1] = (uint16_t)((packet[11] << 4) & 0xFF0) | (packet[10] >> 4);
c[2] = (uint16_t)((packet[13] <<8)& 0xF00) | packet[12]; c[2] = (uint16_t)((packet[13] << 8) & 0xF00) | packet[12];
c[3] = (uint16_t)((packet[14]<<4)&0xFF0) | (packet[13]>>4); c[3] = (uint16_t)((packet[14] << 4) & 0xFF0) | (packet[13] >> 4);
c[4] = (uint16_t)((packet[16] <<8)& 0xF00) | packet[15]; c[4] = (uint16_t)((packet[16] << 8) & 0xF00) | packet[15];
c[5] = (uint16_t)((packet[17]<<4)&0xFF0) | (packet[16]>>4); c[5] = (uint16_t)((packet[17] << 4) & 0xFF0) | (packet[16] >> 4);
c[6] = (uint16_t)((packet[19] <<8)& 0xF00) | packet[18]; c[6] = (uint16_t)((packet[19] << 8) & 0xF00) | packet[18];
c[7] = (uint16_t)((packet[20]<<4)&0xFF0) | (packet[19]>>4); c[7] = (uint16_t)((packet[20] << 4) & 0xFF0) | (packet[19] >> 4);
uint8_t j; uint8_t j = 0;
for(uint8_t i=0;i<8;i++) { for(uint8_t i = 0; i < 8; i++) {
if(c[i] > 2047) { if(c[i] > 2047) {
j = 8; j = 8;
c[i] = c[i] - 2048; c[i] = c[i] - 2048;
} else { } else {
j = 0; j = 0;
} }
word_temp = (((c[i]-64)<<1)/3+860); int16_t temp = (((c[i] - 64) << 1) / 3 + 860);
if ((word_temp > 800) && (word_temp < 2200)) if ((temp > 800) && (temp < 2200)) {
rcData[i+j] = word_temp; rcData[i+j] = temp;
}
} }
} }
rx_spi_received_e frSkyXDataReceived(uint8_t *packet) rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const protocolState)
{ {
static unsigned receiveTelemetryRetryCount = 0; static unsigned receiveTelemetryRetryCount = 0;
static uint32_t polling_time=0; static timeMs_t pollingTimeMs = 0;
static bool skipChannels = true;
static bool ledIsOn;
static uint8_t remoteProcessedId = 0;
static uint8_t remoteAckId = 0;
static uint8_t remoteToProcessIndex = 0;
static timeUs_t packetTimerUs;
static bool frameReceived;
static timeDelta_t receiveDelayUs;
static uint8_t channelsToSkip = 1;
static telemetryBuffer_t telemetryRxBuffer[TELEMETRY_SEQUENCE_LENGTH];
#if defined(USE_RX_FRSKY_SPI_TELEMETRY)
static bool telemetryReceived = false;
#endif
rx_spi_received_e ret = RX_SPI_RECEIVED_NONE; rx_spi_received_e ret = RX_SPI_RECEIVED_NONE;
switch (protocolState) { switch (*protocolState) {
case STATE_INIT:
if ((millis() - start_time) > 10) {
initialize();
protocolState = STATE_BIND;
}
break;
case STATE_BIND:
case STATE_BIND_TUNING:
case STATE_BIND_BINDING1:
case STATE_BIND_BINDING2:
case STATE_BIND_COMPLETE:
protocolState = handleBinding(protocolState, packet);
break;
case STATE_STARTING: case STATE_STARTING:
listLength = 47; listLength = 47;
initialiseData(0); initialiseData(0);
protocolState = STATE_UPDATE; *protocolState = STATE_UPDATE;
nextChannel(1); nextChannel(1);
cc2500Strobe(CC2500_SRX); cc2500Strobe(CC2500_SRX);
ret = RX_SPI_RECEIVED_BIND;
break; break;
case STATE_UPDATE: case STATE_UPDATE:
packet_timer = micros(); packetTimerUs = micros();
protocolState = STATE_DATA; *protocolState = STATE_DATA;
frame_received=false;//again set for receive frameReceived = false; // again set for receive
t_received = 5300; receiveDelayUs = 5300;
if (checkBindRequested(false)) { if (checkBindRequested(false)) {
packet_timer = 0; packetTimerUs = 0;
t_out = 50; timeoutUs = 50;
missingPackets = 0; missingPackets = 0;
protocolState = STATE_INIT; *protocolState = STATE_INIT;
break; break;
} }
FALLTHROUGH; //!!TODO -check this fall through is correct
FALLTHROUGH;
// here FS code could be // here FS code could be
case STATE_DATA: case STATE_DATA:
if ((IORead(gdoPin)) &&(frame_received==false)){ if (IORead(gdoPin) && (frameReceived == false)){
ccLen =cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
ccLen =cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;//read 2 times to avoid reading errors ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F; // read 2 times to avoid reading errors
if (ccLen > 32) if (ccLen > 32) {
ccLen = 32; ccLen = 32;
}
if (ccLen) { if (ccLen) {
cc2500ReadFifo(packet, ccLen); cc2500ReadFifo(packet, ccLen);
uint16_t lcrc= crc(&packet[3],(ccLen-7)); uint16_t lcrc= calculateCrc(&packet[3], (ccLen - 7));
if((lcrc >>8)==packet[ccLen-4]&&(lcrc&0x00FF)==packet[ccLen-3]){//check crc if((lcrc >> 8) == packet[ccLen-4] && (lcrc&0x00FF) == packet[ccLen - 3]){ // check calculateCrc
if (packet[0] == 0x1D) { if (packet[0] == 0x1D) {
if ((packet[1] == rxFrSkySpiConfig()->bindTxId[0]) && if ((packet[1] == rxFrSkySpiConfig()->bindTxId[0]) &&
(packet[2] == rxFrSkySpiConfig()->bindTxId[1]) && (packet[2] == rxFrSkySpiConfig()->bindTxId[1]) &&
(rxFrSkySpiConfig()->rxNum == 0 || packet[6] == 0 || packet[6] == rxFrSkySpiConfig()->rxNum)) { (rxFrSkySpiConfig()->rxNum == 0 || packet[6] == 0 || packet[6] == rxFrSkySpiConfig()->rxNum)) {
missingPackets = 0; missingPackets = 0;
t_out = 1; timeoutUs = 1;
t_received = 0; receiveDelayUs = 0;
IOHi(frSkyLedPin); IOHi(frSkyLedPin);
if(one_time){ if (skipChannels) {
chanskip=packet[5]<<2; channelsToSkip = packet[5] << 2;
if(packet[4]<listLength){} if (packet[4] >= listLength) {
else if(packet[4]<(64+listLength)) if (packet[4] < (64 + listLength)) {
chanskip +=1; channelsToSkip += 1;
else if(packet[4]<(128+listLength)) } else if (packet[4] < (128 + listLength)) {
chanskip +=2; channelsToSkip += 2;
else if(packet[4]<(192+listLength)) } else if (packet[4] < (192 + listLength)) {
chanskip +=3; channelsToSkip += 3;
telemetryRX=1;//now telemetry can be sent }
one_time=0; }
telemetryReceived = true; // now telemetry can be sent
skipChannels = false;
} }
#ifdef USE_RX_FRSKY_SPI_TELEMETRY #ifdef USE_RX_FRSKY_SPI_TELEMETRY
setRssiDbm(packet[ccLen - 2]); setRssiDbm(packet[ccLen - 2]);
@ -443,37 +416,38 @@ rx_spi_received_e frSkyXDataReceived(uint8_t *packet)
receiveTelemetryRetryCount = 0; receiveTelemetryRetryCount = 0;
} }
packet_timer=micros(); packetTimerUs = micros();
frame_received=true;//no need to process frame again. frameReceived = true; // no need to process frame again.
} }
} }
} }
} }
} }
if (telemetryRX) { if (telemetryReceived) {
if(cmpTimeUs(micros(), packet_timer) > t_received) { // if received or not received in this time sent telemetry data if(cmpTimeUs(micros(), packetTimerUs) > receiveDelayUs) { // if received or not received in this time sent telemetry data
protocolState=STATE_TELEMETRY; *protocolState = STATE_TELEMETRY;
telemetry_build_frame(packet); buildTelemetryFrame(packet);
} }
} }
if (cmpTimeUs(micros(), packet_timer) > t_out * SYNC) { if (cmpTimeUs(micros(), packetTimerUs) > timeoutUs * SYNC_DELAY_MAX) {
if (cnt++ & 0x01) { if (ledIsOn) {
IOLo(frSkyLedPin); IOLo(frSkyLedPin);
} else { } else {
IOHi(frSkyLedPin); IOHi(frSkyLedPin);
} }
//telemetryTime=micros(); ledIsOn = !ledIsOn;
#if defined(USE_RX_FRSKY_SPI_TELEMETRY) #if defined(USE_RX_FRSKY_SPI_TELEMETRY)
setRssiFiltered(0, RSSI_SOURCE_RX_PROTOCOL); setRssiFiltered(0, RSSI_SOURCE_RX_PROTOCOL);
#endif #endif
nextChannel(1); nextChannel(1);
cc2500Strobe(CC2500_SRX); cc2500Strobe(CC2500_SRX);
protocolState = STATE_UPDATE; *protocolState = STATE_UPDATE;
} }
break; break;
#ifdef USE_RX_FRSKY_SPI_TELEMETRY #ifdef USE_RX_FRSKY_SPI_TELEMETRY
case STATE_TELEMETRY: case STATE_TELEMETRY:
if(cmpTimeUs(micros(), packet_timer) >= t_received + 400) { // if received or not received in this time sent telemetry data if(cmpTimeUs(micros(), packetTimerUs) >= receiveDelayUs + 400) { // if received or not received in this time sent telemetry data
cc2500Strobe(CC2500_SIDLE); cc2500Strobe(CC2500_SIDLE);
cc2500SetPower(6); cc2500SetPower(6);
cc2500Strobe(CC2500_SFRX); cc2500Strobe(CC2500_SFRX);
@ -487,10 +461,10 @@ rx_spi_received_e frSkyXDataReceived(uint8_t *packet)
#if defined(USE_TELEMETRY_SMARTPORT) #if defined(USE_TELEMETRY_SMARTPORT)
if (telemetryEnabled) { if (telemetryEnabled) {
bool clearToSend = false; bool clearToSend = false;
uint32_t now = millis(); timeMs_t now = millis();
smartPortPayload_t *payload = NULL; smartPortPayload_t *payload = NULL;
if ((now - polling_time) > 24) { if ((now - pollingTimeMs) > 24) {
polling_time=now; pollingTimeMs = now;
clearToSend = true; clearToSend = true;
} else { } else {
@ -512,66 +486,48 @@ rx_spi_received_e frSkyXDataReceived(uint8_t *packet)
processSmartPortTelemetry(payload, &clearToSend, NULL); processSmartPortTelemetry(payload, &clearToSend, NULL);
} }
#endif #endif
protocolState = STATE_RESUME; *protocolState = STATE_RESUME;
ret = RX_SPI_RECEIVED_DATA; ret = RX_SPI_RECEIVED_DATA;
} }
break; break;
#endif // USE_RX_FRSKY_SPI_TELEMETRY #endif // USE_RX_FRSKY_SPI_TELEMETRY
case STATE_RESUME: case STATE_RESUME:
if (cmpTimeUs(micros(), packet_timer) > t_received + 3700) { if (cmpTimeUs(micros(), packetTimerUs) > receiveDelayUs + 3700) {
packet_timer = micros(); packetTimerUs = micros();
t_received=5300; receiveDelayUs = 5300;
frame_received=false;//again set for receive frameReceived = false; // again set for receive
nextChannel(chanskip); nextChannel(channelsToSkip);
cc2500Strobe(CC2500_SRX); cc2500Strobe(CC2500_SRX);
#ifdef USE_RX_FRSKY_SPI_PA_LNA #ifdef USE_RX_FRSKY_SPI_PA_LNA
RxEnable(); RxEnable();
#ifdef USE_RX_FRSKY_SPI_DIVERSITY // SE4311 chip #if defined(USE_RX_FRSKY_SPI_DIVERSITY)
if (missingPackets >= 2) { if (missingPackets >= 2) {
if (pass & 0x01) switchAntennae();
{
IOHi(antSelPin);
}
else
{
IOLo(antSelPin);
}
pass++;
} }
#endif #endif
#endif // USE_RX_FRSKY_SPI_PA_LNA #endif // USE_RX_FRSKY_SPI_PA_LNA
if (missingPackets > MAX_MISSING_PKT) if (missingPackets > MAX_MISSING_PKT) {
{ timeoutUs = 50;
t_out = 50; skipChannels = true;
one_time=1; telemetryReceived = false;
telemetryRX=0; *protocolState = STATE_UPDATE;
protocolState = STATE_UPDATE;
break; break;
} }
missingPackets++; missingPackets++;
protocolState = STATE_DATA; *protocolState = STATE_DATA;
} }
break; break;
} }
return ret; return ret;
} }
void frSkyXInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) void frSkyXInit(void)
{ {
UNUSED(rxConfig);
rxRuntimeConfig->channelCount = RC_CHANNEL_COUNT;
protocolState = STATE_INIT;
missingPackets = 0;
t_out = 50;
#if defined(USE_TELEMETRY_SMARTPORT) #if defined(USE_TELEMETRY_SMARTPORT)
telemetryEnabled = initSmartPortTelemetryExternal(frSkyXTelemetryWriteFrame); telemetryEnabled = initSmartPortTelemetryExternal(frSkyXTelemetryWriteFrame);
#endif #endif
frskySpiRxSetup(rxConfig->rx_spi_protocol);
} }
#endif #endif

14
src/main/rx/cc2500_frsky_x.h
View file

@ -17,14 +17,12 @@
#pragma once #pragma once
#include <stdbool.h> #include "rx/rx.h"
#include <stdint.h> #include "rx/rx_spi.h"
#include "rx_spi.h" #define RC_CHANNEL_COUNT_FRSKY_X 16
struct rxConfig_s;
struct rxRuntimeConfig_s;
void frSkyXInit(const struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig);
void frSkyXSetRcData(uint16_t *rcData, const uint8_t *payload); void frSkyXSetRcData(uint16_t *rcData, const uint8_t *payload);
rx_spi_received_e frSkyXDataReceived(uint8_t *payload);
void frSkyXBind(); void frSkyXInit(void);
rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const protocolState);

22
src/main/rx/rx_spi.c
View file

@ -35,8 +35,7 @@
#include "rx/rx.h" #include "rx/rx.h"
#include "rx/rx_spi.h" #include "rx/rx_spi.h"
#include "rx/cc2500_frsky_d.h" #include "rx/cc2500_frsky_common.h"
#include "rx/cc2500_frsky_x.h"
#include "rx/nrf24_cx10.h" #include "rx/nrf24_cx10.h"
#include "rx/nrf24_syma.h" #include "rx/nrf24_syma.h"
#include "rx/nrf24_v202.h" #include "rx/nrf24_v202.h"
@ -113,20 +112,19 @@ STATIC_UNIT_TESTED bool rxSpiSetProtocol(rx_spi_protocol_e protocol)
protocolSetRcDataFromPayload = inavNrf24SetRcDataFromPayload; protocolSetRcDataFromPayload = inavNrf24SetRcDataFromPayload;
break; break;
#endif #endif
#ifdef USE_RX_FRSKY_SPI_D #if defined(USE_RX_FRSKY_SPI)
#if defined(USE_RX_FRSKY_SPI_D)
case RX_SPI_FRSKY_D: case RX_SPI_FRSKY_D:
protocolInit = frSkyDInit;
protocolDataReceived = frSkyDDataReceived;
protocolSetRcDataFromPayload = frSkyDSetRcData;
break;
#endif #endif
#ifdef USE_RX_FRSKY_SPI_X #if defined(USE_RX_FRSKY_SPI_X)
case RX_SPI_FRSKY_X: case RX_SPI_FRSKY_X:
protocolInit = frSkyXInit;
protocolDataReceived = frSkyXDataReceived;
protocolSetRcDataFromPayload = frSkyXSetRcData;
break;
#endif #endif
protocolInit = frSkySpiInit;
protocolDataReceived = frSkySpiDataReceived;
protocolSetRcDataFromPayload = frSkySpiSetRcData;
break;
#endif // USE_RX_FRSKY_SPI
#ifdef USE_RX_FLYSKY #ifdef USE_RX_FLYSKY
case RX_SPI_A7105_FLYSKY: case RX_SPI_A7105_FLYSKY:
case RX_SPI_A7105_FLYSKY_2A: case RX_SPI_A7105_FLYSKY_2A: