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
case RX_SPI_FRSKY_D:
case RX_SPI_FRSKY_X:
frSkyBind();
frSkySpiBind();
cliPrint("Binding...");

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

@ -17,11 +17,9 @@
#pragma once
//#include <stdbool.h>
//#include <stdint.h>
#include "pg/pg.h"
//#include "rx.h"
//#include "rx_spi.h"
#include "rx/rx_spi.h"
typedef struct rxFrSkySpiConfig_s {
bool autoBind;
@ -33,4 +31,8 @@ typedef struct rxFrSkySpiConfig_s {
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 "rx/rx.h"
#include "rx/rx_spi.h"
#include "rx/cc2500_frsky_common.h"
#include "rx/cc2500_frsky_shared.h"
#include "rx/cc2500_frsky_d.h"
#include "sensors/battery.h"
#include "telemetry/frsky.h"
#define RC_CHANNEL_COUNT 8
#define MAX_MISSING_PKT 100
#include "cc2500_frsky_d.h"
#define DEBUG_DATA_ERROR_COUNT 0
#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
#if defined(USE_RX_FRSKY_SPI_TELEMETRY)
static uint8_t frame[20];
static uint8_t telemetry_id;
static uint32_t telemetryTime;
static uint8_t telemetryId;
#if defined(USE_TELEMETRY_FRSKY)
#define MAX_SERIAL_BYTES 64
static uint8_t hub_index;
static uint8_t idxx = 0;
static uint8_t idx_ok = 0;
static uint8_t telemetry_expected_id = 0;
static uint8_t srx_data[MAX_SERIAL_BYTES]; // buffer for telemetry serial data
#endif
static uint8_t hubIndex;
static uint8_t telemetryInxdex = 0;
static uint8_t serialBuffer[MAX_SERIAL_BYTES]; // buffer for telemetry serial data
#endif
#endif // USE_RX_FRSKY_SPI_TELEMETRY
#if defined(USE_RX_FRSKY_SPI_TELEMETRY)
#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)
idx_ok = idxx;
else // rx re-requests last packet
idxx = idx_ok;
static uint8_t telemetryIndexAcknowledged = 0;
static uint8_t telemetryIndexExpected = 0;
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;
for (uint8_t i = 0; i < 10; i++) {
if (idxx == hub_index) {
if (telemetryInxdex == hubIndex) {
break;
}
buf[i] = srx_data[idxx];
idxx = (idxx + 1) & (MAX_SERIAL_BYTES - 1);
buf[i] = serialBuffer[telemetryInxdex];
telemetryInxdex = (telemetryInxdex + 1) & (MAX_SERIAL_BYTES - 1);
index++;
}
return index;
@ -107,37 +88,36 @@ static uint8_t frsky_append_hub_data(uint8_t *buf)
static void frSkyTelemetryInitFrameSpi(void)
{
hub_index = 0;
idxx = 0;
hubIndex = 0;
telemetryInxdex = 0;
}
static void frSkyTelemetryWriteSpi(uint8_t ch)
{
if (hub_index < MAX_SERIAL_BYTES) {
srx_data[hub_index++] = ch;
if (hubIndex < MAX_SERIAL_BYTES) {
serialBuffer[hubIndex++] = ch;
}
}
#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 adcRssiSample = adcGetChannel(ADC_RSSI);
uint8_t bytes_used = 0;
telemetry_id = packet[4];
telemetryId = packet[4];
frame[0] = 0x11; // length
frame[1] = rxFrSkySpiConfig()->bindTxId[0];
frame[2] = rxFrSkySpiConfig()->bindTxId[1];
frame[3] = (uint8_t)((adcExternal1Sample & 0xff0) >> 4); // A1
frame[4] = (uint8_t)((adcRssiSample & 0xff0) >> 4); // A2
frame[5] = (uint8_t)RSSI_dBm;
frame[5] = (uint8_t)rssiDbm;
#if defined(USE_TELEMETRY_FRSKY)
bytes_used = frsky_append_hub_data(&frame[8]);
bytes_used = appendFrSkyHubData(&frame[8]);
#endif
frame[6] = bytes_used;
frame[7] = telemetry_id;
frame[7] = telemetryId;
}
#endif // USE_RX_FRSKY_SPI_TELEMETRY
#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)
{
uint16_t channels[RC_CHANNEL_COUNT];
static uint16_t dataErrorCount = 0;
uint16_t channels[RC_CHANNEL_COUNT_FRSKY_D];
bool dataError = false;
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 + 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)) {
dataError = true;
@ -166,7 +148,7 @@ void frSkyDSetRcData(uint16_t *rcData, const uint8_t *packet)
}
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];
}
} 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;
switch (protocolState) {
case STATE_INIT:
if ((millis() - start_time) > 10) {
initialize();
const timeUs_t currentPacketReceivedTime = micros();
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;
switch (*protocolState) {
case STATE_STARTING:
listLength = 47;
initialiseData(0);
protocolState = STATE_UPDATE;
*protocolState = STATE_UPDATE;
nextChannel(1);
cc2500Strobe(CC2500_SRX);
ret = RX_SPI_RECEIVED_BIND;
break;
case STATE_UPDATE:
lastPacketReceivedTime = currentPacketReceivedTime;
protocolState = STATE_DATA;
*protocolState = STATE_DATA;
if (checkBindRequested(false)) {
lastPacketReceivedTime = 0;
t_out = 50;
timeoutUs = 50;
missingPackets = 0;
protocolState = STATE_INIT;
*protocolState = STATE_INIT;
break;
}
@ -228,7 +198,7 @@ rx_spi_received_e frSkyDDataReceived(uint8_t *packet)
cc2500ReadFifo(packet, 20);
if (packet[19] & 0x80) {
missingPackets = 0;
t_out = 1;
timeoutUs = 1;
if (packet[0] == 0x11) {
if ((packet[1] == rxFrSkySpiConfig()->bindTxId[0]) &&
(packet[2] == rxFrSkySpiConfig()->bindTxId[1])) {
@ -236,15 +206,15 @@ rx_spi_received_e frSkyDDataReceived(uint8_t *packet)
nextChannel(1);
#if defined(USE_RX_FRSKY_SPI_TELEMETRY)
if ((packet[3] % 4) == 2) {
telemetryTime = micros();
telemetryTimeUs = micros();
setRssiDbm(packet[18]);
telemetry_build_frame(packet);
protocolState = STATE_TELEMETRY;
buildTelemetryFrame(packet);
*protocolState = STATE_TELEMETRY;
} else
#endif
{
cc2500Strobe(CC2500_SRX);
protocolState = STATE_UPDATE;
*protocolState = STATE_UPDATE;
}
ret = RX_SPI_RECEIVED_DATA;
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)
RxEnable();
#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 (missingPackets >= 2) {
if (pass & 0x01) {
IOHi(antSelPin);
} else {
IOLo(antSelPin);
}
pass++;
switchAntennae();
}
#endif
if (missingPackets > MAX_MISSING_PKT) {
t_out = 50;
timeoutUs = 50;
#if defined(USE_RX_FRSKY_SPI_TELEMETRY)
setRssiFiltered(0, RSSI_SOURCE_RX_PROTOCOL);
@ -281,11 +246,12 @@ rx_spi_received_e frSkyDDataReceived(uint8_t *packet)
missingPackets++;
nextChannel(1);
} else {
if (cnt++ & 0x01) {
if (ledIsOn) {
IOLo(frSkyLedPin);
} else {
IOHi(frSkyLedPin);
}
ledIsOn = !ledIsOn;
#if defined(USE_RX_FRSKY_SPI_TELEMETRY)
setRssiUnfiltered(0, RSSI_SOURCE_RX_PROTOCOL);
@ -294,12 +260,12 @@ rx_spi_received_e frSkyDDataReceived(uint8_t *packet)
}
cc2500Strobe(CC2500_SRX);
protocolState = STATE_UPDATE;
*protocolState = STATE_UPDATE;
}
break;
#if defined(USE_RX_FRSKY_SPI_TELEMETRY)
case STATE_TELEMETRY:
if ((micros() - telemetryTime) >= 1380) {
if (cmpTimeUs(micros(), telemetryTimeUs) >= 1380) {
cc2500Strobe(CC2500_SIDLE);
cc2500SetPower(6);
cc2500Strobe(CC2500_SFRX);
@ -308,7 +274,7 @@ rx_spi_received_e frSkyDDataReceived(uint8_t *packet)
#endif
cc2500Strobe(CC2500_SIDLE);
cc2500WriteFifo(frame, frame[0] + 1);
protocolState = STATE_DATA;
*protocolState = STATE_DATA;
ret = RX_SPI_RECEIVED_DATA;
lastPacketReceivedTime = currentPacketReceivedTime;
}
@ -317,27 +283,15 @@ rx_spi_received_e frSkyDDataReceived(uint8_t *packet)
#endif
}
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)
initFrSkyExternalTelemetry(&frSkyTelemetryInitFrameSpi,
&frSkyTelemetryWriteSpi);
#endif
frskySpiRxSetup(rxConfig->rx_spi_protocol);
start_time = millis();
}
#endif

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

@ -17,10 +17,12 @@
#pragma once
#include "rx.h"
#include "rx_spi.h"
#include "rx/rx.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);
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 "rx/rx.h"
#include "rx/rx_spi.h"
#include "rx/cc2500_frsky_common.h"
#include "rx/cc2500_frsky_d.h"
#include "rx/cc2500_frsky_x.h"
#include "cc2500_frsky_shared.h"
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 timeMs_t timeTunedMs;
uint8_t listLength;
static uint8_t bindIdx;
static int8_t bindOffset;
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;
static IO_t bindPin = DEFIO_IO(NONE);
@ -55,11 +71,11 @@ IO_t frSkyLedPin;
#if defined(USE_RX_FRSKY_SPI_PA_LNA)
static IO_t txEnPin;
static IO_t rxLnaEnPin;
IO_t antSelPin;
static IO_t antSelPin;
#endif
#ifdef USE_RX_FRSKY_SPI_TELEMETRY
int16_t RSSI_dBm;
int16_t rssiDbm;
#endif
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)
{
if (value >= 128) {
RSSI_dBm = ((((uint16_t)value) * 18) >> 5) - 82;
rssiDbm = ((((uint16_t)value) * 18) >> 5) - 82;
} 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
@ -100,12 +116,12 @@ void TxEnable(void)
}
#endif
void frSkyBind(void)
void frSkySpiBind(void)
{
bindRequested = true;
}
void initialize() {
static void initialise() {
cc2500Reset();
cc2500WriteReg(CC2500_02_IOCFG0, 0x01);
cc2500WriteReg(CC2500_17_MCSM1, 0x0C);
@ -136,7 +152,8 @@ void initialize() {
cc2500WriteReg(CC2500_03_FIFOTHR, 0x07);
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_08_PKTCTRL0, 0x05);
cc2500WriteReg(CC2500_0B_FSCTRL1, 0x08);
@ -144,7 +161,9 @@ void initialize() {
cc2500WriteReg(CC2500_11_MDMCFG3, 0x39);
cc2500WriteReg(CC2500_12_MDMCFG2, 0x11);
cc2500WriteReg(CC2500_15_DEVIATN, 0x42);
} else {
break;
case RX_SPI_FRSKY_X:
cc2500WriteReg(CC2500_06_PKTLEN, 0x1E);
cc2500WriteReg(CC2500_08_PKTCTRL0, 0x01);
cc2500WriteReg(CC2500_0B_FSCTRL1, 0x0A);
@ -152,10 +171,15 @@ void initialize() {
cc2500WriteReg(CC2500_11_MDMCFG3, 0x61);
cc2500WriteReg(CC2500_12_MDMCFG2, 0x13);
cc2500WriteReg(CC2500_15_DEVIATN, 0x51);
break;
default:
break;
}
for(uint8_t c=0;c<0xFF;c++)
{//calibrate all channels
for(unsigned c = 0;c < 0xFF; c++)
{ //calibrate all channels
cc2500Strobe(CC2500_SIDLE);
cc2500WriteReg(CC2500_0A_CHANNR, c);
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) {
case STATE_INIT:
if ((millis() - start_time) > 10) {
initialise();
protocolState = STATE_BIND;
}
break;
case STATE_BIND:
if (checkBindRequested(true) || rxFrSkySpiConfig()->autoBind) {
IOHi(frSkyLedPin);
@ -392,12 +426,22 @@ uint8_t handleBinding(uint8_t protocolState, uint8_t *packet)
}
}
ret = RX_SPI_RECEIVED_BIND;
protocolState = STATE_STARTING;
break;
default:
ret = handlePacket(packet, &protocolState);
break;
}
return protocolState;
return ret;
}
void frSkySpiSetRcData(uint16_t *rcData, const uint8_t *payload)
{
setRcData(rcData, payload);
}
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 (rssiSource == RSSI_SOURCE_NONE) {
@ -467,7 +564,10 @@ void frskySpiRxSetup(rx_spi_protocol_e protocol)
RxEnable();
#endif // USE_RX_FRSKY_SPI_PA_LNA
// if(!frSkySpiDetect())//detect spi working routine
// return;
missingPackets = 0;
timeoutUs = 50;
start_time = millis();
protocolState = STATE_INIT;
}
#endif

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

@ -23,7 +23,9 @@
#define DEBUG_DATA_ERROR_COUNT 0
#define SYNC 9000
#define SYNC_DELAY_MAX 9000
#define MAX_MISSING_PKT 100
enum {
STATE_INIT = 0,
@ -39,26 +41,23 @@ enum {
STATE_RESUME,
};
extern bool bindRequested;
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 frSkyLedPin;
extern IO_t antSelPin;
void setRssiDbm(uint8_t value);
void frskySpiRxSetup(rx_spi_protocol_e protocol);
void RxEnable(void);
void TxEnable(void);
void initialize();
void switchAntennae(void);
void initialiseData(uint8_t adr);
bool checkBindRequested(bool reset);
uint8_t handleBinding(uint8_t protocolState, uint8_t *packet);
void nextChannel(uint8_t skip);

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

@ -39,19 +39,16 @@
#include "fc/config.h"
#include "rx/rx.h"
#include "rx/rx_spi.h"
#include "rx/cc2500_frsky_common.h"
#include "rx/cc2500_frsky_shared.h"
#include "rx/cc2500_frsky_x.h"
#include "sensors/battery.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,
0x8c48,0x9dc1,0xaf5a,0xbed3,0xca6c,0xdbe5,0xe97e,0xf8f7,
0x1081,0x0108,0x3393,0x221a,0x56a5,0x472c,0x75b7,0x643e,
@ -128,74 +125,40 @@ typedef struct telemetryPayload_s {
uint8_t crc[2];
} __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 uint8_t remoteProcessedId = 0;
static uint8_t remoteAckId = 0;
static uint8_t remoteToProcessIndex = 0;
static uint8_t localPacketId;
#endif
static telemetrySequenceMarker_t responseToSend;
static uint8_t ccLen;
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
#if defined(USE_RX_FRSKY_SPI_TELEMETRY)
static uint8_t frame[20];
static uint8_t telemetryRX;
#if defined(USE_TELEMETRY_SMARTPORT)
static uint8_t telemetryOutReader = 0;
static uint8_t telemetryOutWriter;
static uint8_t telemetryOutBuffer[TELEMETRY_OUT_BUFFER_SIZE];
static bool telemetryEnabled = false;
#endif
#endif
#endif // USE_RX_FRSKY_SPI_TELEMETRY
bool frskySpiDetect(void)//debug CC2500 spi
{
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) {
static uint16_t calculateCrc(uint8_t *data, uint8_t len) {
uint16_t crc = 0;
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;
}
#if defined(USE_RX_FRSKY_SPI_TELEMETRY)
#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;
for (index = 0; index < TELEMETRY_DATA_SIZE; index++) { //max 5 bytes in a frame
if(telemetryOutReader == telemetryOutWriter){ //no new data
for (index = 0; index < TELEMETRY_DATA_SIZE; index++) { // max 5 bytes in a frame
if (telemetryOutReader == telemetryOutWriter){ // no new data
break;
}
buf[index] = telemetryOutBuffer[telemetryOutReader];
@ -206,26 +169,28 @@ static uint8_t frsky_append_sport_data(uint8_t *buf)
}
#endif
#if defined(USE_RX_FRSKY_SPI_TELEMETRY)
static void telemetry_build_frame(uint8_t *packet)
static void buildTelemetryFrame(uint8_t *packet)
{
frame[0]=0x0E;//length
frame[1]=rxFrSkySpiConfig()->bindTxId[0];
frame[2]=rxFrSkySpiConfig()->bindTxId[1];
frame[3]=packet[3];
static uint8_t localPacketId;
static bool evenRun = false;
frame[0] = 0x0E;//length
frame[1] = rxFrSkySpiConfig()->bindTxId[0];
frame[2] = rxFrSkySpiConfig()->bindTxId[1];
frame[3] = packet[3];
if (evenRun) {
frame[4]=(uint8_t)RSSI_dBm|0x80;
frame[4]=(uint8_t)rssiDbm|0x80;
} else {
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;
telemetrySequenceMarker_t *inFrameMarker = (telemetrySequenceMarker_t *)&packet[21];
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->data.initRequest = 1;
outFrameMarker->data.initResponse = 1;
@ -243,8 +208,8 @@ static void telemetry_build_frame(uint8_t *packet)
if (localPacketId != (localAckId + 1) % TELEMETRY_SEQUENCE_LENGTH) {
outFrameMarker->raw = responseToSend.raw & SEQUENCE_MARKER_REMOTE_PART;
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);
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[14]=lcrc;
}
@ -294,104 +259,112 @@ void frSkyXSetRcData(uint16_t *rcData, const uint8_t *packet)
{
uint16_t c[8];
c[0] = (uint16_t)((packet[10] <<8)& 0xF00) | packet[9];
c[1] = (uint16_t)((packet[11]<<4)&0xFF0) | (packet[10]>>4);
c[2] = (uint16_t)((packet[13] <<8)& 0xF00) | packet[12];
c[3] = (uint16_t)((packet[14]<<4)&0xFF0) | (packet[13]>>4);
c[4] = (uint16_t)((packet[16] <<8)& 0xF00) | packet[15];
c[5] = (uint16_t)((packet[17]<<4)&0xFF0) | (packet[16]>>4);
c[6] = (uint16_t)((packet[19] <<8)& 0xF00) | packet[18];
c[7] = (uint16_t)((packet[20]<<4)&0xFF0) | (packet[19]>>4);
c[0] = (uint16_t)((packet[10] << 8) & 0xF00) | packet[9];
c[1] = (uint16_t)((packet[11] << 4) & 0xFF0) | (packet[10] >> 4);
c[2] = (uint16_t)((packet[13] << 8) & 0xF00) | packet[12];
c[3] = (uint16_t)((packet[14] << 4) & 0xFF0) | (packet[13] >> 4);
c[4] = (uint16_t)((packet[16] << 8) & 0xF00) | packet[15];
c[5] = (uint16_t)((packet[17] << 4) & 0xFF0) | (packet[16] >> 4);
c[6] = (uint16_t)((packet[19] << 8) & 0xF00) | packet[18];
c[7] = (uint16_t)((packet[20] << 4) & 0xFF0) | (packet[19] >> 4);
uint8_t j;
for(uint8_t i=0;i<8;i++) {
if(c[i] > 2047) {
uint8_t j = 0;
for(uint8_t i = 0; i < 8; i++) {
if(c[i] > 2047) {
j = 8;
c[i] = c[i] - 2048;
} else {
j = 0;
}
word_temp = (((c[i]-64)<<1)/3+860);
if ((word_temp > 800) && (word_temp < 2200))
rcData[i+j] = word_temp;
int16_t temp = (((c[i] - 64) << 1) / 3 + 860);
if ((temp > 800) && (temp < 2200)) {
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 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;
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;
switch (*protocolState) {
case STATE_STARTING:
listLength = 47;
initialiseData(0);
protocolState = STATE_UPDATE;
*protocolState = STATE_UPDATE;
nextChannel(1);
cc2500Strobe(CC2500_SRX);
ret = RX_SPI_RECEIVED_BIND;
break;
case STATE_UPDATE:
packet_timer = micros();
protocolState = STATE_DATA;
frame_received=false;//again set for receive
t_received = 5300;
packetTimerUs = micros();
*protocolState = STATE_DATA;
frameReceived = false; // again set for receive
receiveDelayUs = 5300;
if (checkBindRequested(false)) {
packet_timer = 0;
t_out = 50;
packetTimerUs = 0;
timeoutUs = 50;
missingPackets = 0;
protocolState = STATE_INIT;
*protocolState = STATE_INIT;
break;
}
FALLTHROUGH; //!!TODO -check this fall through is correct
FALLTHROUGH;
// here FS code could be
case STATE_DATA:
if ((IORead(gdoPin)) &&(frame_received==false)){
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
if (ccLen > 32)
if (IORead(gdoPin) && (frameReceived == false)){
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
if (ccLen > 32) {
ccLen = 32;
}
if (ccLen) {
cc2500ReadFifo(packet, ccLen);
uint16_t lcrc= crc(&packet[3],(ccLen-7));
if((lcrc >>8)==packet[ccLen-4]&&(lcrc&0x00FF)==packet[ccLen-3]){//check crc
uint16_t lcrc= calculateCrc(&packet[3], (ccLen - 7));
if((lcrc >> 8) == packet[ccLen-4] && (lcrc&0x00FF) == packet[ccLen - 3]){ // check calculateCrc
if (packet[0] == 0x1D) {
if ((packet[1] == rxFrSkySpiConfig()->bindTxId[0]) &&
(packet[2] == rxFrSkySpiConfig()->bindTxId[1]) &&
(rxFrSkySpiConfig()->rxNum == 0 || packet[6] == 0 || packet[6] == rxFrSkySpiConfig()->rxNum)) {
missingPackets = 0;
t_out = 1;
t_received = 0;
timeoutUs = 1;
receiveDelayUs = 0;
IOHi(frSkyLedPin);
if(one_time){
chanskip=packet[5]<<2;
if(packet[4]<listLength){}
else if(packet[4]<(64+listLength))
chanskip +=1;
else if(packet[4]<(128+listLength))
chanskip +=2;
else if(packet[4]<(192+listLength))
chanskip +=3;
telemetryRX=1;//now telemetry can be sent
one_time=0;
if (skipChannels) {
channelsToSkip = packet[5] << 2;
if (packet[4] >= listLength) {
if (packet[4] < (64 + listLength)) {
channelsToSkip += 1;
} else if (packet[4] < (128 + listLength)) {
channelsToSkip += 2;
} else if (packet[4] < (192 + listLength)) {
channelsToSkip += 3;
}
}
telemetryReceived = true; // now telemetry can be sent
skipChannels = false;
}
#ifdef USE_RX_FRSKY_SPI_TELEMETRY
setRssiDbm(packet[ccLen - 2]);
@ -443,37 +416,38 @@ rx_spi_received_e frSkyXDataReceived(uint8_t *packet)
receiveTelemetryRetryCount = 0;
}
packet_timer=micros();
frame_received=true;//no need to process frame again.
packetTimerUs = micros();
frameReceived = true; // no need to process frame again.
}
}
}
}
}
if (telemetryRX) {
if(cmpTimeUs(micros(), packet_timer) > t_received) { // if received or not received in this time sent telemetry data
protocolState=STATE_TELEMETRY;
telemetry_build_frame(packet);
if (telemetryReceived) {
if(cmpTimeUs(micros(), packetTimerUs) > receiveDelayUs) { // if received or not received in this time sent telemetry data
*protocolState = STATE_TELEMETRY;
buildTelemetryFrame(packet);
}
}
if (cmpTimeUs(micros(), packet_timer) > t_out * SYNC) {
if (cnt++ & 0x01) {
if (cmpTimeUs(micros(), packetTimerUs) > timeoutUs * SYNC_DELAY_MAX) {
if (ledIsOn) {
IOLo(frSkyLedPin);
} else {
IOHi(frSkyLedPin);
}
//telemetryTime=micros();
ledIsOn = !ledIsOn;
#if defined(USE_RX_FRSKY_SPI_TELEMETRY)
setRssiFiltered(0, RSSI_SOURCE_RX_PROTOCOL);
#endif
nextChannel(1);
cc2500Strobe(CC2500_SRX);
protocolState = STATE_UPDATE;
*protocolState = STATE_UPDATE;
}
break;
#ifdef USE_RX_FRSKY_SPI_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);
cc2500SetPower(6);
cc2500Strobe(CC2500_SFRX);
@ -487,10 +461,10 @@ rx_spi_received_e frSkyXDataReceived(uint8_t *packet)
#if defined(USE_TELEMETRY_SMARTPORT)
if (telemetryEnabled) {
bool clearToSend = false;
uint32_t now = millis();
timeMs_t now = millis();
smartPortPayload_t *payload = NULL;
if ((now - polling_time) > 24) {
polling_time=now;
if ((now - pollingTimeMs) > 24) {
pollingTimeMs = now;
clearToSend = true;
} else {
@ -512,66 +486,48 @@ rx_spi_received_e frSkyXDataReceived(uint8_t *packet)
processSmartPortTelemetry(payload, &clearToSend, NULL);
}
#endif
protocolState = STATE_RESUME;
*protocolState = STATE_RESUME;
ret = RX_SPI_RECEIVED_DATA;
}
break;
#endif // USE_RX_FRSKY_SPI_TELEMETRY
case STATE_RESUME:
if (cmpTimeUs(micros(), packet_timer) > t_received + 3700) {
packet_timer = micros();
t_received=5300;
frame_received=false;//again set for receive
nextChannel(chanskip);
if (cmpTimeUs(micros(), packetTimerUs) > receiveDelayUs + 3700) {
packetTimerUs = micros();
receiveDelayUs = 5300;
frameReceived = false; // again set for receive
nextChannel(channelsToSkip);
cc2500Strobe(CC2500_SRX);
#ifdef USE_RX_FRSKY_SPI_PA_LNA
RxEnable();
#ifdef USE_RX_FRSKY_SPI_DIVERSITY // SE4311 chip
#if defined(USE_RX_FRSKY_SPI_DIVERSITY)
if (missingPackets >= 2) {
if (pass & 0x01)
{
IOHi(antSelPin);
}
else
{
IOLo(antSelPin);
}
pass++;
switchAntennae();
}
#endif
#endif // USE_RX_FRSKY_SPI_PA_LNA
if (missingPackets > MAX_MISSING_PKT)
{
t_out = 50;
one_time=1;
telemetryRX=0;
protocolState = STATE_UPDATE;
if (missingPackets > MAX_MISSING_PKT) {
timeoutUs = 50;
skipChannels = true;
telemetryReceived = false;
*protocolState = STATE_UPDATE;
break;
}
missingPackets++;
protocolState = STATE_DATA;
*protocolState = STATE_DATA;
}
break;
}
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)
telemetryEnabled = initSmartPortTelemetryExternal(frSkyXTelemetryWriteFrame);
#endif
frskySpiRxSetup(rxConfig->rx_spi_protocol);
}
#endif

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

@ -17,14 +17,12 @@
#pragma once
#include <stdbool.h>
#include <stdint.h>
#include "rx/rx.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);
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_spi.h"
#include "rx/cc2500_frsky_d.h"
#include "rx/cc2500_frsky_x.h"
#include "rx/cc2500_frsky_common.h"
#include "rx/nrf24_cx10.h"
#include "rx/nrf24_syma.h"
#include "rx/nrf24_v202.h"
@ -113,20 +112,19 @@ STATIC_UNIT_TESTED bool rxSpiSetProtocol(rx_spi_protocol_e protocol)
protocolSetRcDataFromPayload = inavNrf24SetRcDataFromPayload;
break;
#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:
protocolInit = frSkyDInit;
protocolDataReceived = frSkyDDataReceived;
protocolSetRcDataFromPayload = frSkyDSetRcData;
break;
#endif
#ifdef USE_RX_FRSKY_SPI_X
#if defined(USE_RX_FRSKY_SPI_X)
case RX_SPI_FRSKY_X:
protocolInit = frSkyXInit;
protocolDataReceived = frSkyXDataReceived;
protocolSetRcDataFromPayload = frSkyXSetRcData;
break;
#endif
protocolInit = frSkySpiInit;
protocolDataReceived = frSkySpiDataReceived;
protocolSetRcDataFromPayload = frSkySpiSetRcData;
break;
#endif // USE_RX_FRSKY_SPI
#ifdef USE_RX_FLYSKY
case RX_SPI_A7105_FLYSKY:
case RX_SPI_A7105_FLYSKY_2A: