rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Cleanup of the FrSky SPI RX code.

This commit is contained in:
mikeller 2017-08-20 23:22:00 +12:00
parent bd84749e92
commit a4ba8b3db8
9 changed files with 298 additions and 329 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/main/build/debug.c
View File

@ -49,5 +49,6 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"ALTITUDE",
"FFT",
"FFT_TIME",
"FFT_FREQ"
"FFT_FREQ",
"FRSKY_D_RX"
};

1
src/main/build/debug.h
View File

@ -68,6 +68,7 @@ typedef enum {
DEBUG_FFT,
DEBUG_FFT_TIME,
DEBUG_FFT_FREQ,
DEBUG_FRSKY_D_RX,
DEBUG_COUNT
} debugType_e;

43
src/main/drivers/cc2500.c
View File

@ -7,56 +7,56 @@
#include "platform.h"
#ifdef USE_RX_CC2500
#include "build/build_config.h"
#include "cc2500.h"
#include "io.h"
#include "rx_spi.h"
#include "system.h"
#include "time.h"
#include "drivers/cc2500.h"
#include "drivers/io.h"
#include "drivers/rx_spi.h"
#include "drivers/system.h"
#include "drivers/time.h"
#define NOP 0xFF
uint8_t cc2500_readFifo(uint8_t *dpbuffer, uint8_t len)
uint8_t cc2500ReadFifo(uint8_t *dpbuffer, uint8_t len)
{
return rxSpiReadCommandMulti(CC2500_3F_RXFIFO | CC2500_READ_BURST, NOP, dpbuffer, len);
}
uint8_t cc2500_writeFifo(uint8_t *dpbuffer, uint8_t len)
uint8_t cc2500WriteFifo(uint8_t *dpbuffer, uint8_t len)
{
uint8_t ret;
cc2500_strobe(CC2500_SFTX); // 0x3B SFTX
cc2500Strobe(CC2500_SFTX); // 0x3B SFTX
ret = rxSpiWriteCommandMulti(CC2500_3F_TXFIFO | CC2500_WRITE_BURST,
dpbuffer, len);
cc2500_strobe(CC2500_STX); // 0x35
cc2500Strobe(CC2500_STX); // 0x35
return ret;
}
uint8_t cc2500_ReadRegisterMulti(uint8_t address, uint8_t *data, uint8_t length)
uint8_t cc2500ReadRegisterMulti(uint8_t address, uint8_t *data, uint8_t length)
{
return rxSpiReadCommandMulti(address, NOP, data, length);
}
uint8_t cc2500_WriteRegisterMulti(uint8_t address, uint8_t *data,
uint8_t cc2500WriteRegisterMulti(uint8_t address, uint8_t *data,
uint8_t length)
{
return rxSpiWriteCommandMulti(address, data, length);
}
uint8_t cc2500_readReg(uint8_t reg)
uint8_t cc2500ReadReg(uint8_t reg)
{
return rxSpiReadCommand(reg | 0x80, NOP);
}
void cc2500_strobe(uint8_t address) { rxSpiWriteByte(address); }
void cc2500Strobe(uint8_t address) { rxSpiWriteByte(address); }
uint8_t cc2500_writeReg(uint8_t address, uint8_t data)
uint8_t cc2500WriteReg(uint8_t address, uint8_t data)
{
return rxSpiWriteCommand(address, data);
}
void CC2500_SetPower(uint8_t power)
void cc2500SetPower(uint8_t power)
{
const uint8_t patable[8] = {
0xC5, // -12dbm
@ -70,16 +70,15 @@ void CC2500_SetPower(uint8_t power)
};
if (power > 7)
power = 7;
cc2500_writeReg(CC2500_3E_PATABLE, patable[power]);
cc2500WriteReg(CC2500_3E_PATABLE, patable[power]);
}
uint8_t CC2500_Reset()
uint8_t cc2500Reset()
{
cc2500_strobe(CC2500_SRES);
cc2500Strobe(CC2500_SRES);
delayMicroseconds(1000); // 1000us
// CC2500_SetTxRxMode(TXRX_OFF);
// RX_EN_off;//off tx
// TX_EN_off;//off rx
return cc2500_readReg(CC2500_0E_FREQ1) == 0xC4; // check if reset
return cc2500ReadReg(CC2500_0E_FREQ1) == 0xC4; // check if reset
}
#endif

20
src/main/drivers/cc2500.h
View File

@ -138,16 +138,16 @@ enum {
#define CC2500_LQI_CRC_OK_BM 0x80
#define CC2500_LQI_EST_BM 0x7F
// void FrskyRXspiInit();
uint8_t cc2500_readFifo(uint8_t *dpbuffer, uint8_t len);
uint8_t cc2500_writeFifo(uint8_t *dpbuffer, uint8_t len);
uint8_t cc2500_ReadRegisterMulti(uint8_t address, uint8_t *data,
uint8_t cc2500ReadFifo(uint8_t *dpbuffer, uint8_t len);
uint8_t cc2500WriteFifo(uint8_t *dpbuffer, uint8_t len);
uint8_t cc2500ReadRegisterMulti(uint8_t address, uint8_t *data,
uint8_t length);
uint8_t CC2500_WriteRegisterMulti(uint8_t address, uint8_t *data,
uint8_t cc2500WriteRegisterMulti(uint8_t address, uint8_t *data,
uint8_t length);
uint8_t cc2500_readReg(uint8_t reg);
void cc2500_strobe(uint8_t address);
uint8_t cc2500_writeReg(uint8_t address, uint8_t data);
void CC2500_SetPower(uint8_t power);
uint8_t CC2500_Reset();
uint8_t cc2500ReadReg(uint8_t reg);
void cc2500Strobe(uint8_t address);
uint8_t cc2500WriteReg(uint8_t address, uint8_t data);
void cc2500SetPower(uint8_t power);
uint8_t cc2500Reset();

4
src/main/fc/cli.c
View File

@ -2110,7 +2110,7 @@ static void cliBeeper(char *cmdline)
}
#endif
#ifdef FRSKY_BIND
#ifdef USE_RX_FRSKY_D
void cliFrSkyBind(char *cmdline){
UNUSED(cmdline);
frSkyDBind();
@ -3491,7 +3491,7 @@ const clicmd_t cmdTable[] = {
CLI_COMMAND_DEF("beeper", "turn on/off beeper", "list\r\n"
"\t<+|->[name]", cliBeeper),
#endif
#ifdef FRSKY_BIND
#ifdef USE_RX_FRSKY_D
CLI_COMMAND_DEF("frsky_bind", NULL, NULL, cliFrSkyBind),
#endif
#ifdef LED_STRIP

521
src/main/rx/frsky_d.c
View File

@ -25,113 +25,70 @@
#ifdef USE_RX_FRSKY_D
#include "build/build_config.h"
#include "build/debug.h"
#include "common/utils.h"
#include "common/maths.h"
#include "common/utils.h"
#include "drivers/cc2500.h"
#include "drivers/io.h"
#include "drivers/system.h"
#include "drivers/time.h"
#include "fc/config.h"
#include "config/feature.h"
#include "config/parameter_group_ids.h"
#include "rx/rx.h"
#include "rx/rx_spi.h"
#include "rx/frsky_d.h"
#include "sensors/battery.h"
#include "fc/config.h"
#include "config/feature.h"
#include "config/parameter_group_ids.h"
#include "telemetry/frsky.h"
#define RC_CHANNEL_COUNT 8
#define MAX_MISSING_PKT 100
#define DEBUG_DATA_ERROR_COUNT 0
#define SYNC 9000
#define FS_THR 960
#define FLED_on \
{ \
IOHi(IOGetByTag(IO_TAG(FRSKY_LED_PIN))); \
}
#define FLED_off \
{ \
IOLo(IOGetByTag(IO_TAG(FRSKY_LED_PIN))); \
}
#define GDO_1 IORead(IOGetByTag(IO_TAG(GDO_0_PIN)))
#if defined(PA_LNA)
#define TX_EN_on \
{ \
IOHi(IOGetByTag(IO_TAG(TX_EN_PIN))); \
}
#define TX_EN_off \
{ \
IOLo(IOGetByTag(IO_TAG(TX_EN_PIN))); \
}
#define RX_EN_on \
{ \
IOHi(IOGetByTag(IO_TAG(RX_EN_PIN))); \
}
#define RX_EN_off \
{ \
IOLo(IOGetByTag(IO_TAG(RX_EN_PIN))); \
}
#if defined(DIVERSITY)
#define ANT_SEL_on \
{ \
IOHi(IOGetByTag(IO_TAG(ANT_SEL_PIN))); \
}
#define ANT_SEL_off \
{ \
IOLo(IOGetByTag(IO_TAG(ANT_SEL_PIN))); \
}
#endif
#endif
static uint8_t ccLen;
static uint8_t channr;
static uint32_t missingPackets;
static uint8_t calData[255][3];
static uint32_t time_tune;
static uint8_t listLength;
static uint8_t bindIdx;
static uint8_t cnt;
static uint16_t Servo_data[RC_CHANNEL_COUNT];
static uint16_t c[8];
static uint32_t t_out;
static int32_t t_out;
static uint8_t Lqi;
static uint32_t packet_timer;
static uint8_t protocol_state;
static int16_t word_temp;
static timeUs_t lastPacketReceivedTime;
static uint8_t protocolState;
static uint32_t start_time;
static int8_t bindOffset;
static uint16_t dataErrorCount = 0;
static IO_t GdoPin;
static IO_t BindPin = DEFIO_IO(NONE);
static IO_t gdoPin;
static IO_t bindPin = DEFIO_IO(NONE);
static bool lastBindPinStatus;
static IO_t FrskyLedPin;
#if defined(PA_LNA)
static IO_t TxEnPin;
static IO_t RxEnPin;
static IO_t AntSelPin;
static IO_t frSkyLedPin;
#if defined(USE_FRSKY_RX_PA_LNA)
static IO_t txEnPin;
static IO_t rxEnPin;
static IO_t antSelPin;
static uint8_t pass;
#endif
bool bindRequested = false;
#ifdef FRSKY_TELEMETRY
#ifdef USE_FRSKY_D_TELEMETRY
static uint8_t frame[20];
static int16_t RSSI_dBm;
static uint8_t telemetry_id;
static uint8_t telemetryRX;
static uint32_t telemetryTime;
#if defined(HUB)
#if defined(TELEMETRY_FRSKY)
#define MAX_SERIAL_BYTES 64
static uint8_t hub_index;
static uint8_t idxx = 0;
@ -154,8 +111,6 @@ PG_RESET_TEMPLATE(frSkyDConfig_t, frSkyDConfig,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
);
rx_spi_received_e ret;
enum {
STATE_INIT = 0,
STATE_BIND,
@ -166,22 +121,22 @@ enum {
STATE_STARTING,
STATE_UPDATE,
STATE_DATA,
STATE_TELEM
STATE_TELEMETRY
};
#if defined(FRSKY_TELEMETRY)
#if defined(USE_FRSKY_D_TELEMETRY)
static void compute_RSSIdbm(uint8_t *packet)
{
if (packet[ccLen - 2] >= 128) {
RSSI_dBm = ((((uint16_t)packet[ccLen - 2]) * 18) >> 5) - 82;
if (packet[18] >= 128) {
RSSI_dBm = ((((uint16_t)packet[18]) * 18) >> 5) - 82;
} else {
RSSI_dBm = ((((uint16_t)packet[ccLen - 2]) * 18) >> 5) + 65;
RSSI_dBm = ((((uint16_t)packet[18]) * 18) >> 5) + 65;
}
processRssi(constrain((RSSI_dBm << 3) / 10, 0, 100));
}
#if defined(HUB)
#if defined(TELEMETRY_FRSKY)
static uint8_t frsky_append_hub_data(uint8_t *buf)
{
if (telemetry_id == telemetry_expected_id)
@ -228,7 +183,7 @@ static void telemetry_build_frame(uint8_t *packet)
frame[3] = (uint8_t)((adcExternal1Sample & 0xff0) >> 4); // A1
frame[4] = (uint8_t)((adcRssiSample & 0xff0) >> 4); // A2
frame[5] = (uint8_t)RSSI_dBm;
#if defined(HUB)
#if defined(TELEMETRY_FRSKY)
bytes_used = frsky_append_hub_data(&frame[8]);
#endif
frame[6] = bytes_used;
@ -237,17 +192,17 @@ static void telemetry_build_frame(uint8_t *packet)
#endif
#if defined(PA_LNA)
#if defined(USE_FRSKY_RX_PA_LNA)
static void RX_enable()
{
TX_EN_off;
RX_EN_on;
IOLo(txEnPin);
IOHi(rxEnPin);
}
static void TX_enable()
{
RX_EN_off;
TX_EN_on;
IOLo(rxEnPin);
IOHi(txEnPin);
}
#endif
@ -258,82 +213,82 @@ void frSkyDBind()
static void initialize()
{
CC2500_Reset();
cc2500_writeReg(CC2500_02_IOCFG0, 0x01);
cc2500_writeReg(CC2500_17_MCSM1, 0x0C);
cc2500_writeReg(CC2500_18_MCSM0, 0x18);
cc2500_writeReg(CC2500_06_PKTLEN, 0x19);
cc2500_writeReg(CC2500_08_PKTCTRL0, 0x05);
cc2500_writeReg(CC2500_3E_PATABLE, 0xFF);
cc2500_writeReg(CC2500_0B_FSCTRL1, 0x08);
cc2500_writeReg(CC2500_0C_FSCTRL0, 0x00);
cc2500_writeReg(CC2500_0D_FREQ2, 0x5C);
cc2500_writeReg(CC2500_0E_FREQ1, 0x76);
cc2500_writeReg(CC2500_0F_FREQ0, 0x27);
cc2500_writeReg(CC2500_10_MDMCFG4, 0xAA);
cc2500_writeReg(CC2500_11_MDMCFG3, 0x39);
cc2500_writeReg(CC2500_12_MDMCFG2, 0x11);
cc2500_writeReg(CC2500_13_MDMCFG1, 0x23);
cc2500_writeReg(CC2500_14_MDMCFG0, 0x7A);
cc2500_writeReg(CC2500_15_DEVIATN, 0x42);
cc2500_writeReg(CC2500_19_FOCCFG, 0x16);
cc2500_writeReg(CC2500_1A_BSCFG, 0x6C);
cc2500_writeReg(CC2500_1B_AGCCTRL2, 0x03);
cc2500_writeReg(CC2500_1C_AGCCTRL1, 0x40);
cc2500_writeReg(CC2500_1D_AGCCTRL0, 0x91);
cc2500_writeReg(CC2500_21_FREND1, 0x56);
cc2500_writeReg(CC2500_22_FREND0, 0x10);
cc2500_writeReg(CC2500_23_FSCAL3, 0xA9);
cc2500_writeReg(CC2500_24_FSCAL2, 0x0A);
cc2500_writeReg(CC2500_25_FSCAL1, 0x00);
cc2500_writeReg(CC2500_26_FSCAL0, 0x11);
cc2500_writeReg(CC2500_29_FSTEST, 0x59);
cc2500_writeReg(CC2500_2C_TEST2, 0x88);
cc2500_writeReg(CC2500_2D_TEST1, 0x31);
cc2500_writeReg(CC2500_2E_TEST0, 0x0B);
cc2500_writeReg(CC2500_03_FIFOTHR, 0x07);
cc2500_writeReg(CC2500_09_ADDR, 0x00);
cc2500_strobe(CC2500_SIDLE);
cc2500Reset();
cc2500WriteReg(CC2500_02_IOCFG0, 0x01);
cc2500WriteReg(CC2500_17_MCSM1, 0x0C);
cc2500WriteReg(CC2500_18_MCSM0, 0x18);
cc2500WriteReg(CC2500_06_PKTLEN, 0x19);
cc2500WriteReg(CC2500_08_PKTCTRL0, 0x05);
cc2500WriteReg(CC2500_3E_PATABLE, 0xFF);
cc2500WriteReg(CC2500_0B_FSCTRL1, 0x08);
cc2500WriteReg(CC2500_0C_FSCTRL0, 0x00);
cc2500WriteReg(CC2500_0D_FREQ2, 0x5C);
cc2500WriteReg(CC2500_0E_FREQ1, 0x76);
cc2500WriteReg(CC2500_0F_FREQ0, 0x27);
cc2500WriteReg(CC2500_10_MDMCFG4, 0xAA);
cc2500WriteReg(CC2500_11_MDMCFG3, 0x39);
cc2500WriteReg(CC2500_12_MDMCFG2, 0x11);
cc2500WriteReg(CC2500_13_MDMCFG1, 0x23);
cc2500WriteReg(CC2500_14_MDMCFG0, 0x7A);
cc2500WriteReg(CC2500_15_DEVIATN, 0x42);
cc2500WriteReg(CC2500_19_FOCCFG, 0x16);
cc2500WriteReg(CC2500_1A_BSCFG, 0x6C);
cc2500WriteReg(CC2500_1B_AGCCTRL2, 0x03);
cc2500WriteReg(CC2500_1C_AGCCTRL1, 0x40);
cc2500WriteReg(CC2500_1D_AGCCTRL0, 0x91);
cc2500WriteReg(CC2500_21_FREND1, 0x56);
cc2500WriteReg(CC2500_22_FREND0, 0x10);
cc2500WriteReg(CC2500_23_FSCAL3, 0xA9);
cc2500WriteReg(CC2500_24_FSCAL2, 0x0A);
cc2500WriteReg(CC2500_25_FSCAL1, 0x00);
cc2500WriteReg(CC2500_26_FSCAL0, 0x11);
cc2500WriteReg(CC2500_29_FSTEST, 0x59);
cc2500WriteReg(CC2500_2C_TEST2, 0x88);
cc2500WriteReg(CC2500_2D_TEST1, 0x31);
cc2500WriteReg(CC2500_2E_TEST0, 0x0B);
cc2500WriteReg(CC2500_03_FIFOTHR, 0x07);
cc2500WriteReg(CC2500_09_ADDR, 0x00);
cc2500Strobe(CC2500_SIDLE);
cc2500_writeReg(CC2500_07_PKTCTRL1, 0x04);
cc2500_writeReg(CC2500_0C_FSCTRL0, 0);
cc2500WriteReg(CC2500_07_PKTCTRL1, 0x04);
cc2500WriteReg(CC2500_0C_FSCTRL0, 0);
for (uint8_t c = 0; c < 0xFF; c++) {
cc2500_strobe(CC2500_SIDLE);
cc2500_writeReg(CC2500_0A_CHANNR, c);
cc2500_strobe(CC2500_SCAL);
cc2500Strobe(CC2500_SIDLE);
cc2500WriteReg(CC2500_0A_CHANNR, c);
cc2500Strobe(CC2500_SCAL);
delayMicroseconds(900);
calData[c][0] = cc2500_readReg(CC2500_23_FSCAL3);
calData[c][1] = cc2500_readReg(CC2500_24_FSCAL2);
calData[c][2] = cc2500_readReg(CC2500_25_FSCAL1);
calData[c][0] = cc2500ReadReg(CC2500_23_FSCAL3);
calData[c][1] = cc2500ReadReg(CC2500_24_FSCAL2);
calData[c][2] = cc2500ReadReg(CC2500_25_FSCAL1);
}
}
static void initialize_data(uint8_t adr)
{
cc2500_writeReg(CC2500_0C_FSCTRL0, (uint8_t)frSkyDConfig()->bindOffset);
cc2500_writeReg(CC2500_18_MCSM0, 0x8);
cc2500_writeReg(CC2500_09_ADDR, adr ? 0x03 : frSkyDConfig()->bindTxId[0]);
cc2500_writeReg(CC2500_07_PKTCTRL1, 0x0D);
cc2500_writeReg(CC2500_19_FOCCFG, 0x16);
cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)frSkyDConfig()->bindOffset);
cc2500WriteReg(CC2500_18_MCSM0, 0x8);
cc2500WriteReg(CC2500_09_ADDR, adr ? 0x03 : frSkyDConfig()->bindTxId[0]);
cc2500WriteReg(CC2500_07_PKTCTRL1, 0x0D);
cc2500WriteReg(CC2500_19_FOCCFG, 0x16);
delay(10);
}
static void initTuneRx(void)
{
cc2500_writeReg(CC2500_19_FOCCFG, 0x14);
cc2500WriteReg(CC2500_19_FOCCFG, 0x14);
time_tune = millis();
bindOffset = -126;
cc2500_writeReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset);
cc2500_writeReg(CC2500_07_PKTCTRL1, 0x0C);
cc2500_writeReg(CC2500_18_MCSM0, 0x8);
cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset);
cc2500WriteReg(CC2500_07_PKTCTRL1, 0x0C);
cc2500WriteReg(CC2500_18_MCSM0, 0x8);
cc2500_strobe(CC2500_SIDLE);
cc2500_writeReg(CC2500_23_FSCAL3, calData[0][0]);
cc2500_writeReg(CC2500_24_FSCAL2, calData[0][1]);
cc2500_writeReg(CC2500_25_FSCAL1, calData[0][2]);
cc2500_writeReg(CC2500_0A_CHANNR, 0);
cc2500_strobe(CC2500_SFRX);
cc2500_strobe(CC2500_SRX);
cc2500Strobe(CC2500_SIDLE);
cc2500WriteReg(CC2500_23_FSCAL3, calData[0][0]);
cc2500WriteReg(CC2500_24_FSCAL2, calData[0][1]);
cc2500WriteReg(CC2500_25_FSCAL1, calData[0][2]);
cc2500WriteReg(CC2500_0A_CHANNR, 0);
cc2500Strobe(CC2500_SFRX);
cc2500Strobe(CC2500_SRX);
}
static bool tuneRx(uint8_t *packet)
@ -344,12 +299,12 @@ static bool tuneRx(uint8_t *packet)
if ((millis() - time_tune) > 50) {
time_tune = millis();
bindOffset += 5;
cc2500_writeReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset);
cc2500WriteReg(CC2500_0C_FSCTRL0, (uint8_t)bindOffset);
}
if (GDO_1) {
ccLen = cc2500_readReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
if (IORead(gdoPin)) {
uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
if (ccLen) {
cc2500_readFifo(packet, ccLen);
cc2500ReadFifo(packet, ccLen);
if (packet[ccLen - 1] & 0x80) {
if (packet[2] == 0x01) {
Lqi = packet[ccLen - 1] & 0x7F;
@ -368,15 +323,15 @@ static bool tuneRx(uint8_t *packet)
static void initGetBind(void)
{
cc2500_strobe(CC2500_SIDLE);
cc2500_writeReg(CC2500_23_FSCAL3, calData[0][0]);
cc2500_writeReg(CC2500_24_FSCAL2, calData[0][1]);
cc2500_writeReg(CC2500_25_FSCAL1, calData[0][2]);
cc2500_writeReg(CC2500_0A_CHANNR, 0);
cc2500_strobe(CC2500_SFRX);
cc2500Strobe(CC2500_SIDLE);
cc2500WriteReg(CC2500_23_FSCAL3, calData[0][0]);
cc2500WriteReg(CC2500_24_FSCAL2, calData[0][1]);
cc2500WriteReg(CC2500_25_FSCAL1, calData[0][2]);
cc2500WriteReg(CC2500_0A_CHANNR, 0);
cc2500Strobe(CC2500_SFRX);
delayMicroseconds(20); // waiting flush FIFO
cc2500_strobe(CC2500_SRX);
cc2500Strobe(CC2500_SRX);
listLength = 0;
bindIdx = 0x05;
}
@ -386,10 +341,10 @@ static bool getBind1(uint8_t *packet)
// len|bind |tx
// id|03|01|idx|h0|h1|h2|h3|h4|00|00|00|00|00|00|00|00|00|00|00|00|00|00|00|CHK1|CHK2|RSSI|LQI/CRC|
// Start by getting bind packet 0 and the txid
if (GDO_1) {
ccLen = cc2500_readReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
if (IORead(gdoPin)) {
uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
if (ccLen) {
cc2500_readFifo(packet, ccLen);
cc2500ReadFifo(packet, ccLen);
if (packet[ccLen - 1] & 0x80) {
if (packet[2] == 0x01) {
if (packet[5] == 0x00) {
@ -413,11 +368,10 @@ static bool getBind1(uint8_t *packet)
static bool getBind2(uint8_t *packet)
{
if (bindIdx <= 120) {
if (GDO_1) {
ccLen =
cc2500_readReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
if (IORead(gdoPin)) {
uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
if (ccLen) {
cc2500_readFifo(packet, ccLen);
cc2500ReadFifo(packet, ccLen);
if (packet[ccLen - 1] & 0x80) {
if (packet[2] == 0x01) {
if ((packet[3] == frSkyDConfig()->bindTxId[0]) &&
@ -467,27 +421,32 @@ static bool getBind2(uint8_t *packet)
static void nextChannel(uint8_t skip)
{
static uint8_t channr = 0;
channr += skip;
if (channr >= listLength)
while (channr >= listLength) {
channr -= listLength;
cc2500_strobe(CC2500_SIDLE);
cc2500_writeReg(CC2500_23_FSCAL3,
}
cc2500Strobe(CC2500_SIDLE);
cc2500WriteReg(CC2500_23_FSCAL3,
calData[frSkyDConfig()->bindHopData[channr]][0]);
cc2500_writeReg(CC2500_24_FSCAL2,
cc2500WriteReg(CC2500_24_FSCAL2,
calData[frSkyDConfig()->bindHopData[channr]][1]);
cc2500_writeReg(CC2500_25_FSCAL1,
cc2500WriteReg(CC2500_25_FSCAL1,
calData[frSkyDConfig()->bindHopData[channr]][2]);
cc2500_writeReg(CC2500_0A_CHANNR, frSkyDConfig()->bindHopData[channr]);
cc2500_strobe(CC2500_SFRX);
cc2500WriteReg(CC2500_0A_CHANNR, frSkyDConfig()->bindHopData[channr]);
cc2500Strobe(CC2500_SFRX);
}
static bool checkBindRequested(bool reset)
{
bool bindPinStatus = IORead(BindPin);
if (lastBindPinStatus && !bindPinStatus) {
bindRequested = true;
if (bindPin) {
bool bindPinStatus = IORead(bindPin);
if (lastBindPinStatus && !bindPinStatus) {
bindRequested = true;
}
lastBindPinStatus = bindPinStatus;
}
lastBindPinStatus = bindPinStatus;
if (!bindRequested) {
return false;
@ -500,46 +459,63 @@ static bool checkBindRequested(bool reset)
}
}
void frskyD_Rx_SetRCdata(uint16_t *rcData, const uint8_t *packet)
{
c[0] = (uint16_t)(((packet[10] & 0x0F) << 8 | packet[6]));
c[1] = (uint16_t)(((packet[10] & 0xF0) << 4 | packet[7]));
c[2] = (uint16_t)(((packet[11] & 0x0F) << 8 | packet[8]));
c[3] = (uint16_t)(((packet[11] & 0xF0) << 4 | packet[9]));
c[4] = (uint16_t)(((packet[16] & 0x0F) << 8 | packet[12]));
c[5] = (uint16_t)(((packet[16] & 0xF0) << 4 | packet[13]));
c[6] = (uint16_t)(((packet[17] & 0x0F) << 8 | packet[14]));
c[7] = (uint16_t)(((packet[17] & 0xF0) << 4 | packet[15]));
#define FRSKY_D_CHANNEL_SCALING (2.0f / 3)
for (uint8_t i = 0; i < 8; i++) {
word_temp = 0.667 * c[i];
if ((word_temp > 800) && (word_temp < 2200))
Servo_data[i] = word_temp;
rcData[i] = Servo_data[i];
static void decodeChannelPair(uint16_t *channels, const uint8_t *packet, const uint8_t highNibbleOffset) {
channels[0] = FRSKY_D_CHANNEL_SCALING * (uint16_t)((packet[highNibbleOffset] & 0xf) << 8 | packet[0]);
channels[1] = FRSKY_D_CHANNEL_SCALING * (uint16_t)((packet[highNibbleOffset] & 0xf0) << 4 | packet[1]);
}
void frSkyDSetRcData(uint16_t *rcData, const uint8_t *packet)
{
uint16_t channels[RC_CHANNEL_COUNT];
bool dataError = false;
decodeChannelPair(channels, packet + 6, 4);
decodeChannelPair(channels + 2, packet + 8, 3);
decodeChannelPair(channels + 4, packet + 12, 4);
decodeChannelPair(channels + 6, packet + 14, 3);
for (int i = 0; i < RC_CHANNEL_COUNT; i++) {
if ((channels[i] < 800) || (channels[i] > 2200)) {
dataError = true;
break;
}
}
if (!dataError) {
for (int i = 0; i < RC_CHANNEL_COUNT; i++) {
rcData[i] = channels[i];
}
} else {
DEBUG_SET(DEBUG_FRSKY_D_RX, DEBUG_DATA_ERROR_COUNT, ++dataErrorCount);
}
}
rx_spi_received_e frskyD_Rx_DataReceived(uint8_t *packet)
rx_spi_received_e frSkyDDataReceived(uint8_t *packet)
{
ret = RX_SPI_RECEIVED_NONE;
const timeUs_t currentPacketReceivedTime = micros();
switch (protocol_state) {
rx_spi_received_e ret = RX_SPI_RECEIVED_NONE;
switch (protocolState) {
case STATE_INIT:
if ((millis() - start_time) > 10) {
initialize();
protocol_state = STATE_BIND;
protocolState = STATE_BIND;
}
break;
case STATE_BIND:
if (checkBindRequested(true) || frSkyDConfig()->autoBind) {
FLED_on;
IOHi(frSkyLedPin);
initTuneRx();
protocol_state = STATE_BIND_TUNING;
protocolState = STATE_BIND_TUNING;
} else {
protocol_state = STATE_STARTING;
protocolState = STATE_STARTING;
}
break;
@ -548,21 +524,21 @@ rx_spi_received_e frskyD_Rx_DataReceived(uint8_t *packet)
initGetBind();
initialize_data(1);
protocol_state = STATE_BIND_BINDING1;
protocolState = STATE_BIND_BINDING1;
}
break;
case STATE_BIND_BINDING1:
if (getBind1(packet)) {
protocol_state = STATE_BIND_BINDING2;
protocolState = STATE_BIND_BINDING2;
}
break;
case STATE_BIND_BINDING2:
if (getBind2(packet)) {
cc2500_strobe(CC2500_SIDLE);
cc2500Strobe(CC2500_SIDLE);
protocol_state = STATE_BIND_COMPLETE;
protocolState = STATE_BIND_COMPLETE;
}
break;
@ -572,86 +548,83 @@ rx_spi_received_e frskyD_Rx_DataReceived(uint8_t *packet)
} else {
uint8_t ctr = 40;
while (ctr--) {
FLED_on;
IOHi(frSkyLedPin);
delay(50);
FLED_off;
IOLo(frSkyLedPin);
delay(50);
}
}
protocol_state = STATE_STARTING;
protocolState = STATE_STARTING;
break;
case STATE_STARTING:
listLength = 47;
initialize_data(0);
protocol_state = STATE_UPDATE;
protocolState = STATE_UPDATE;
nextChannel(1); //
cc2500_strobe(CC2500_SRX);
cc2500Strobe(CC2500_SRX);
ret = RX_SPI_RECEIVED_BIND;
break;
case STATE_UPDATE:
packet_timer = micros();
protocol_state = STATE_DATA;
lastPacketReceivedTime = currentPacketReceivedTime;
protocolState = STATE_DATA;
if (checkBindRequested(false)) {
packet_timer = 0;
lastPacketReceivedTime = 0;
t_out = 50;
missingPackets = 0;
protocol_state = STATE_INIT;
protocolState = STATE_INIT;
break;
}
// here FS code could be
case STATE_DATA:
if (GDO_1) {
ccLen =
cc2500_readReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
if (ccLen > 20)
ccLen = 20;
if (ccLen == 20) {
cc2500_readFifo(packet, ccLen);
if (packet[ccLen - 1] & 0x80) {
if (IORead(gdoPin)) {
uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
if (ccLen >= 20) {
cc2500ReadFifo(packet, 20);
if (packet[19] & 0x80) {
missingPackets = 0;
t_out = 1;
if (packet[0] == 0x11) {
if ((packet[1] == frSkyDConfig()->bindTxId[0]) &&
(packet[2] == frSkyDConfig()->bindTxId[1])) {
FLED_on;
IOHi(frSkyLedPin);
nextChannel(1);
#ifdef FRSKY_TELEMETRY
#ifdef USE_FRSKY_D_TELEMETRY
if ((packet[3] % 4) == 2) {
telemetryRX = 1;
telemetryTime = micros();
compute_RSSIdbm(packet);
telemetry_build_frame(packet);
protocol_state = STATE_TELEM;
protocolState = STATE_TELEMETRY;
} else
#endif
{
cc2500_strobe(CC2500_SRX);
protocol_state = STATE_UPDATE;
cc2500Strobe(CC2500_SRX);
protocolState = STATE_UPDATE;
}
ret = RX_SPI_RECEIVED_DATA;
packet_timer = micros();
lastPacketReceivedTime = currentPacketReceivedTime;
}
}
}
}
}
if ((micros() - packet_timer) > (t_out * SYNC)) {
#ifdef PA_LNA
if (cmp32(currentPacketReceivedTime, lastPacketReceivedTime) > (t_out * SYNC)) {
#ifdef USE_FRSKY_RX_PA_LNA
RX_enable();
#endif
if (t_out == 1) {
#ifdef DIVERSITY // SE4311 chip
#ifdef USE_FRSKY_RX_DIVERSITY // SE4311 chip
if (missingPackets >= 2) {
if (pass & 0x01) {
ANT_SEL_on;
IOHi(antSelPin);
} else {
ANT_SEL_off;
IOLo(antSelPin);
}
pass++;
}
@ -664,34 +637,31 @@ rx_spi_received_e frskyD_Rx_DataReceived(uint8_t *packet)
nextChannel(1);
} else {
if (cnt++ & 0x01) {
FLED_off;
IOLo(frSkyLedPin);
} else
FLED_on;
IOHi(frSkyLedPin);
nextChannel(13);
}
cc2500_strobe(CC2500_SRX);
protocol_state = STATE_UPDATE;
cc2500Strobe(CC2500_SRX);
protocolState = STATE_UPDATE;
}
break;
#ifdef FRSKY_TELEMETRY
case STATE_TELEM:
if (telemetryRX) {
if ((micros() - telemetryTime) >= 1380) {
cc2500_strobe(CC2500_SIDLE);
CC2500_SetPower(6);
cc2500_strobe(CC2500_SFRX);
#if defined(PA_LNA)
TX_enable();
#ifdef USE_FRSKY_D_TELEMETRY
case STATE_TELEMETRY:
if ((micros() - telemetryTime) >= 1380) {
cc2500Strobe(CC2500_SIDLE);
cc2500SetPower(6);
cc2500Strobe(CC2500_SFRX);
#if defined(USE_FRSKY_RX_PA_LNA)
TX_enable();
#endif
cc2500_strobe(CC2500_SIDLE);
cc2500_writeFifo(frame, frame[0] + 1);
protocol_state = STATE_DATA;
ret = RX_SPI_RECEIVED_DATA;
packet_timer = micros();
telemetryRX = 0;
}
cc2500Strobe(CC2500_SIDLE);
cc2500WriteFifo(frame, frame[0] + 1);
protocolState = STATE_DATA;
ret = RX_SPI_RECEIVED_DATA;
lastPacketReceivedTime = currentPacketReceivedTime;
}
break;
@ -701,58 +671,59 @@ rx_spi_received_e frskyD_Rx_DataReceived(uint8_t *packet)
return ret;
}
void frskyD_Rx_Setup(rx_spi_protocol_e protocol)
static void frskyD_Rx_Setup(rx_spi_protocol_e protocol)
{
UNUSED(protocol);
// gpio init here
GdoPin = IOGetByTag(IO_TAG(GDO_0_PIN));
BindPin = IOGetByTag(IO_TAG(BINDPLUG_PIN));
FrskyLedPin = IOGetByTag(IO_TAG(FRSKY_LED_PIN));
#if defined(PA_LNA)
RxEnPin = IOGetByTag(IO_TAG(RX_EN_PIN));
TxEnPin = IOGetByTag(IO_TAG(TX_EN_PIN));
IOInit(RxEnPin, OWNER_RX_BIND, 0);
IOInit(TxEnPin, OWNER_RX_BIND, 0);
IOConfigGPIO(RxEnPin, IOCFG_OUT_PP);
IOConfigGPIO(TxEnPin, IOCFG_OUT_PP);
gdoPin = IOGetByTag(IO_TAG(FRSKY_RX_GDO_0_PIN));
IOInit(gdoPin, OWNER_RX_BIND, 0);
IOConfigGPIO(gdoPin, IOCFG_IN_FLOATING);
frSkyLedPin = IOGetByTag(IO_TAG(FRSKY_RX_LED_PIN));
IOInit(frSkyLedPin, OWNER_LED, 0);
IOConfigGPIO(frSkyLedPin, IOCFG_OUT_PP);
#if defined(USE_FRSKY_RX_PA_LNA)
rxEnPin = IOGetByTag(IO_TAG(FRSKY_RX_RX_EN_PIN));
IOInit(rxEnPin, OWNER_RX_BIND, 0);
IOConfigGPIO(rxEnPin, IOCFG_OUT_PP);
txEnPin = IOGetByTag(IO_TAG(FRSKY_RX_TX_EN_PIN));
IOInit(txEnPin, OWNER_RX_BIND, 0);
IOConfigGPIO(txEnPin, IOCFG_OUT_PP);
#endif
#if defined(DIVERSITY)
AntSelPin = IOGetByTag(IO_TAG(ANT_SEL_PIN));
IOInit(AntSelPin, OWNER_RX_BIND, 0);
IOConfigGPIO(AntSelPin, IOCFG_OUT_PP);
#if defined(USE_FRSKY_RX_DIVERSITY)
antSelPin = IOGetByTag(IO_TAG(FRSKY_RX_ANT_SEL_PIN));
IOInit(antSelPin, OWNER_RX_BIND, 0);
IOConfigGPIO(antSelPin, IOCFG_OUT_PP);
#endif
#if defined(BINDPLUG_PIN)
bindPin = IOGetByTag(IO_TAG(BINDPLUG_PIN));
IOInit(bindPin, OWNER_RX_BIND, 0);
IOConfigGPIO(bindPin, IOCFG_IPU);
lastBindPinStatus = IORead(bindPin);
#endif
//
IOInit(GdoPin, OWNER_RX_BIND, 0);
IOInit(BindPin, OWNER_RX_BIND, 0);
IOInit(FrskyLedPin, OWNER_LED, 0);
//
IOConfigGPIO(GdoPin, IOCFG_IN_FLOATING);
IOConfigGPIO(BindPin, IOCFG_IPU);
IOConfigGPIO(FrskyLedPin, IOCFG_OUT_PP);
lastBindPinStatus = IORead(BindPin);
start_time = millis();
packet_timer = 0;
lastPacketReceivedTime = 0;
t_out = 50;
missingPackets = 0;
protocol_state = STATE_INIT;
#if defined(DIVERSITY)
ANT_SEL_on;
protocolState = STATE_INIT;
#if defined(USE_FRSKY_RX_DIVERSITY)
IOHi(antSelPin);
#endif
#if defined(PA_LNA)
#if defined(USE_FRSKY_RX_PA_LNA)
RX_enable();
#endif
#if defined(HUB)
#if defined(TELEMETRY_FRSKY)
initFrSkyExternalTelemetry(&frSkyTelemetryInitFrameSpi,
&frSkyTelemetryWriteSpi);
#endif
// if(!frskySpiDetect())//detect spi working routine
// if(!frSkySpiDetect())//detect spi working routine
// return;
}
void frskyD_Rx_Init(const rxConfig_t *rxConfig,
void frSkyDInit(const rxConfig_t *rxConfig,
rxRuntimeConfig_t *rxRuntimeConfig)
{
rxRuntimeConfig->channelCount = RC_CHANNEL_COUNT;

6
src/main/rx/frsky_d.h
View File

@ -33,7 +33,7 @@ PG_DECLARE(frSkyDConfig_t, frSkyDConfig);
struct rxConfig_s;
struct rxRuntimeConfig_s;
void frskyD_Rx_Init(const struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig);
void frskyD_Rx_SetRCdata(uint16_t *rcData, const uint8_t *payload);
rx_spi_received_e frskyD_Rx_DataReceived(uint8_t *payload);
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 frSkyDBind();

6
src/main/rx/rx_spi.c
View File

@ -111,9 +111,9 @@ STATIC_UNIT_TESTED bool rxSpiSetProtocol(rx_spi_protocol_e protocol)
#endif
#ifdef USE_RX_FRSKY_D
case RX_SPI_FRSKY_D:
protocolInit = frskyD_Rx_Init;
protocolDataReceived = frskyD_Rx_DataReceived;
protocolSetRcDataFromPayload = frskyD_Rx_SetRCdata;
protocolInit = frSkyDInit;
protocolDataReceived = frSkyDDataReceived;
protocolSetRcDataFromPayload = frSkyDSetRcData;
break;
#endif
}

23
src/main/target/MIDELICF3/target.h
View File

@ -95,8 +95,6 @@
#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC
#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC
#define USE_RX_CC2500
#define USE_RX_SPI
#define RX_SPI_INSTANCE SPI1
#define RX_NSS_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA
@ -105,23 +103,22 @@
#define DEFAULT_RX_FEATURE FEATURE_RX_SPI
#define RX_SPI_DEFAULT_PROTOCOL RX_SPI_FRSKY_D
#define FRSKY_BIND
#define USE_FRSKY_D_TELEMETRY
#define FRSKY_TELEMETRY
#define HUB
#define PA_LNA
#define DIVERSITY
#define USE_FRSKY_RX_PA_LNA
#define USE_FRSKY_RX_DIVERSITY
#define RX_NSS_PIN SPI1_NSS_PIN
#define RX_SCK_PIN SPI1_SCK_PIN
#define RX_MISO_PIN SPI1_MISO_PIN
#define RX_MOSI_PIN SPI1_MOSI_PIN
#define GDO_0_PIN PB0
#define ANT_SEL_PIN PB2
#define TX_EN_PIN PB1
#define RX_EN_PIN PB11
#define FRSKY_LED_PIN PB6
#define FRSKY_RX_GDO_0_PIN PB0
#define FRSKY_RX_ANT_SEL_PIN PB2
#define FRSKY_RX_TX_EN_PIN PB1
#define FRSKY_RX_RX_EN_PIN PB11
#define FRSKY_RX_LED_PIN PB6
#define BINDPLUG_PIN PC13
#define USE_SERIAL_4WAY_BLHELI_INTERFACE