Merge pull request #7210 from phobos-/spi_dsm
Implemented SPI SPEKTRUM protocol
This commit is contained in:
Michael Keller
GitHub
commit
ee0d86ff20
|
|
@ -77,4 +77,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
|
|||
"RC_SMOOTHING_RATE",
|
||||
"ANTI_GRAVITY",
|
||||
"DYN_LPF",
|
||||
"RX_SPEKTRUM_SPI",
|
||||
};
|
||||
|
|
|
|||
|
|
@ -95,6 +95,7 @@ typedef enum {
|
|||
DEBUG_RC_SMOOTHING_RATE,
|
||||
DEBUG_ANTI_GRAVITY,
|
||||
DEBUG_DYN_LPF,
|
||||
DEBUG_RX_SPEKTRUM_SPI,
|
||||
DEBUG_COUNT
|
||||
} debugType_e;
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,180 @@
|
|||
/*
|
||||
* This file is part of Cleanflight and Betaflight.
|
||||
*
|
||||
* Cleanflight and Betaflight are free software. You can redistribute
|
||||
* this software and/or modify this software under the terms of the
|
||||
* GNU General Public License as published by the Free Software
|
||||
* Foundation, either version 3 of the License, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* Cleanflight and Betaflight are distributed in the hope that they
|
||||
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
|
||||
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||
* See the GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this software.
|
||||
*
|
||||
* If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#ifdef USE_RX_SPEKTRUM
|
||||
|
||||
#include "drivers/bus_spi.h"
|
||||
#include "drivers/exti.h"
|
||||
#include "drivers/io.h"
|
||||
#include "drivers/io_impl.h"
|
||||
#include "drivers/nvic.h"
|
||||
#include "drivers/rx/rx_cyrf6936.h"
|
||||
#include "drivers/rx/rx_spi.h"
|
||||
#include "drivers/time.h"
|
||||
|
||||
static IO_t rxIntIO = IO_NONE;
|
||||
static extiCallbackRec_t cyrf6936extiCallbackRec;
|
||||
static volatile uint32_t timeEvent = 0;
|
||||
static volatile bool occurEvent = false;
|
||||
volatile bool isError = false;
|
||||
|
||||
void cyrf6936ExtiHandler(extiCallbackRec_t *cb)
|
||||
{
|
||||
UNUSED(cb);
|
||||
|
||||
if (IORead(rxIntIO) == 0) {
|
||||
timeEvent = micros();
|
||||
occurEvent = true;
|
||||
}
|
||||
}
|
||||
|
||||
bool cyrf6936RxFinished(uint32_t *timeStamp)
|
||||
{
|
||||
if (occurEvent) {
|
||||
if (timeStamp) {
|
||||
*timeStamp = timeEvent;
|
||||
}
|
||||
|
||||
uint8_t rxIrqStatus = cyrf6936ReadRegister(CYRF6936_RX_IRQ_STATUS);
|
||||
if ((rxIrqStatus & CYRF6936_RXC_IRQ) || (rxIrqStatus & CYRF6936_RXE_IRQ)) {
|
||||
isError = (rxIrqStatus & CYRF6936_RXE_IRQ) > 0x0;
|
||||
}
|
||||
|
||||
occurEvent = false;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool cyrf6936Init(void)
|
||||
{
|
||||
spiDeviceByInstance(RX_SPI_INSTANCE);
|
||||
rxIntIO = IOGetByTag(IO_TAG(RX_IRQ_PIN));
|
||||
IOInit(rxIntIO, OWNER_RX_SPI_CS, 0);
|
||||
EXTIHandlerInit(&cyrf6936extiCallbackRec, cyrf6936ExtiHandler);
|
||||
EXTIConfig(rxIntIO, &cyrf6936extiCallbackRec, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IPD, EXTI_TRIGGER_FALLING);
|
||||
EXTIEnable(rxIntIO, false);
|
||||
|
||||
uint16_t timeout = 1000;
|
||||
do { // Check if chip has waken up
|
||||
cyrf6936WriteRegister(CYRF6936_XACT_CFG, 0x82);
|
||||
} while ((cyrf6936ReadRegister(CYRF6936_XACT_CFG) != 0x82) && timeout--);
|
||||
|
||||
// Soft reset
|
||||
cyrf6936WriteRegister(CYRF6936_MODE_OVERRIDE, CYRF6936_RST);
|
||||
|
||||
// Verify the CYRF chip is responding
|
||||
return cyrf6936ReadRegister(CYRF6936_FRAMING_CFG) == 0xA5;
|
||||
}
|
||||
|
||||
void cyrf6936WriteRegister(const uint8_t address, const uint8_t data)
|
||||
{
|
||||
rxSpiWriteCommand(CYRF6936_DIR | address, data);
|
||||
}
|
||||
|
||||
void cyrf6936WriteBlock(const uint8_t address, const uint8_t *data, const uint8_t length)
|
||||
{
|
||||
rxSpiWriteCommandMulti(CYRF6936_DIR | address, &data[0], length);
|
||||
}
|
||||
|
||||
uint8_t cyrf6936ReadRegister(const uint8_t address)
|
||||
{
|
||||
return rxSpiReadCommand(address, 0xFF);
|
||||
}
|
||||
|
||||
void cyrf6936ReadBlock(const uint8_t address, uint8_t data[], const uint8_t length)
|
||||
{
|
||||
rxSpiReadCommandMulti(address, 0xFF, &data[0], length);
|
||||
}
|
||||
|
||||
uint8_t cyrf6936GetRssi(void)
|
||||
{
|
||||
return cyrf6936ReadRegister(CYRF6936_RSSI) & 0x1F; //5 bit value 0 - 31
|
||||
}
|
||||
|
||||
uint8_t cyrf6936GetRxStatus(void)
|
||||
{
|
||||
return cyrf6936ReadRegister(CYRF6936_RX_STATUS);
|
||||
}
|
||||
|
||||
void cyrf6936SetConfigLen(const uint8_t config[][2], const uint8_t length)
|
||||
{
|
||||
for (unsigned i = 0; i < length; i++) {
|
||||
cyrf6936WriteRegister(config[i][0], config[i][1]);
|
||||
}
|
||||
}
|
||||
|
||||
void cyrf6936SetChannel(const uint8_t chan)
|
||||
{
|
||||
cyrf6936WriteRegister(CYRF6936_CHANNEL, chan);
|
||||
}
|
||||
|
||||
void cyrf6936SetMode(const uint8_t mode, const bool force)
|
||||
{
|
||||
if (force) {
|
||||
cyrf6936WriteRegister(CYRF6936_XACT_CFG, mode | CYRF6936_FRC_END);
|
||||
} else {
|
||||
cyrf6936WriteRegister(CYRF6936_XACT_CFG, mode);
|
||||
}
|
||||
}
|
||||
|
||||
void cyrf6936SetCrcSeed(const uint16_t crc)
|
||||
{
|
||||
cyrf6936WriteRegister(CYRF6936_CRC_SEED_LSB, crc & 0xff);
|
||||
cyrf6936WriteRegister(CYRF6936_CRC_SEED_MSB, crc >> 8);
|
||||
}
|
||||
|
||||
void cyrf6936SetSopCode(const uint8_t *sopcode)
|
||||
{
|
||||
cyrf6936WriteBlock(CYRF6936_SOP_CODE, sopcode, 8);
|
||||
}
|
||||
|
||||
void cyrf6936SetDataCode(const uint8_t *datacode)
|
||||
{
|
||||
cyrf6936WriteBlock(CYRF6936_DATA_CODE, datacode, 16);
|
||||
}
|
||||
|
||||
void cyrf6936SendLen(const uint8_t *data, const uint8_t length)
|
||||
{
|
||||
cyrf6936WriteRegister(CYRF6936_TX_LENGTH, length);
|
||||
cyrf6936WriteRegister(CYRF6936_TX_CTRL, CYRF6936_TX_CLR);
|
||||
cyrf6936WriteBlock(CYRF6936_TX_BUFFER, data, length);
|
||||
cyrf6936WriteRegister(CYRF6936_TX_CTRL, CYRF6936_TX_GO);
|
||||
}
|
||||
|
||||
void cyrf6936StartRecv(void)
|
||||
{
|
||||
cyrf6936WriteRegister(CYRF6936_RX_IRQ_STATUS, CYRF6936_RXOW_IRQ);
|
||||
cyrf6936WriteRegister(CYRF6936_RX_CTRL, CYRF6936_RX_GO | CYRF6936_RXC_IRQEN | CYRF6936_RXE_IRQEN);
|
||||
EXTIEnable(rxIntIO, true);
|
||||
}
|
||||
|
||||
void cyrf6936RecvLen(uint8_t *data, const uint8_t length)
|
||||
{
|
||||
cyrf6936ReadBlock(CYRF6936_RX_BUFFER, data, length);
|
||||
}
|
||||
|
||||
#endif /* USE_RX_SPEKTRUM */
|
||||
|
|
@ -0,0 +1,226 @@
|
|||
/*
|
||||
* This file is part of Cleanflight and Betaflight.
|
||||
*
|
||||
* Cleanflight and Betaflight are free software. You can redistribute
|
||||
* this software and/or modify this software under the terms of the
|
||||
* GNU General Public License as published by the Free Software
|
||||
* Foundation, either version 3 of the License, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* Cleanflight and Betaflight are distributed in the hope that they
|
||||
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
|
||||
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||
* See the GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this software.
|
||||
*
|
||||
* If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
enum {
|
||||
CYRF6936_CHANNEL = 0x00,
|
||||
CYRF6936_TX_LENGTH = 0x01,
|
||||
CYRF6936_TX_CTRL = 0x02,
|
||||
CYRF6936_TX_CFG = 0x03,
|
||||
CYRF6936_TX_IRQ_STATUS = 0x04,
|
||||
CYRF6936_RX_CTRL = 0x05,
|
||||
CYRF6936_RX_CFG = 0x06,
|
||||
CYRF6936_RX_IRQ_STATUS = 0x07,
|
||||
CYRF6936_RX_STATUS = 0x08,
|
||||
CYRF6936_RX_COUNT = 0x09,
|
||||
CYRF6936_RX_LENGTH = 0x0A,
|
||||
CYRF6936_PWR_CTRL = 0x0B,
|
||||
CYRF6936_XTAL_CTRL = 0x0C,
|
||||
CYRF6936_IO_CFG = 0x0D,
|
||||
CYRF6936_GPIO_CTRL = 0x0E,
|
||||
CYRF6936_XACT_CFG = 0x0F,
|
||||
CYRF6936_FRAMING_CFG = 0x10,
|
||||
CYRF6936_DATA32_THOLD = 0x11,
|
||||
CYRF6936_DATA64_THOLD = 0x12,
|
||||
CYRF6936_RSSI = 0x13,
|
||||
CYRF6936_EOP_CTRL = 0x14,
|
||||
CYRF6936_CRC_SEED_LSB = 0x15,
|
||||
CYRF6936_CRC_SEED_MSB = 0x16,
|
||||
CYRF6936_TX_CRC_LSB = 0x17,
|
||||
CYRF6936_TX_CRC_MSB = 0x18,
|
||||
CYRF6936_RX_CRC_LSB = 0x19,
|
||||
CYRF6936_RX_CRC_MSB = 0x1A,
|
||||
CYRF6936_TX_OFFSET_LSB = 0x1B,
|
||||
CYRF6936_TX_OFFSET_MSB = 0x1C,
|
||||
CYRF6936_MODE_OVERRIDE = 0x1D,
|
||||
CYRF6936_RX_OVERRIDE = 0x1E,
|
||||
CYRF6936_TX_OVERRIDE = 0x1F,
|
||||
CYRF6936_TX_BUFFER = 0x20,
|
||||
CYRF6936_RX_BUFFER = 0x21,
|
||||
CYRF6936_SOP_CODE = 0x22,
|
||||
CYRF6936_DATA_CODE = 0x23,
|
||||
CYRF6936_PREAMBLE = 0x24,
|
||||
CYRF6936_MFG_ID = 0x25,
|
||||
CYRF6936_XTAL_CFG = 0x26,
|
||||
CYRF6936_CLK_OFFSET = 0x27,
|
||||
CYRF6936_CLK_EN = 0x28,
|
||||
CYRF6936_RX_ABORT = 0x29,
|
||||
CYRF6936_AUTO_CAL_TIME = 0x32,
|
||||
CYRF6936_AUTO_CAL_OFFSET = 0x35,
|
||||
CYRF6936_ANALOG_CTRL = 0x39,
|
||||
};
|
||||
// ENABLE WRITING
|
||||
#define CYRF6936_DIR (1<<7)
|
||||
|
||||
// CYRF6936_MODE_OVERRIDE
|
||||
#define CYRF6936_RST (1<<0)
|
||||
|
||||
// CYRF6936_CLK_EN
|
||||
#define CYRF6936_RXF (1<<1)
|
||||
|
||||
// CYRF6936_XACT_CFG
|
||||
enum {
|
||||
CYRF6936_MODE_SLEEP = (0x0 << 2),
|
||||
CYRF6936_MODE_IDLE = (0x1 << 2),
|
||||
CYRF6936_MODE_SYNTH_TX = (0x2 << 2),
|
||||
CYRF6936_MODE_SYNTH_RX = (0x3 << 2),
|
||||
CYRF6936_MODE_RX = (0x4 << 2),
|
||||
};
|
||||
#define CYRF6936_FRC_END (1<<5)
|
||||
#define CYRF6936_ACK_EN (1<<7)
|
||||
|
||||
// CYRF6936_IO_CFG
|
||||
#define CYRF6936_IRQ_GPIO (1<<0)
|
||||
#define CYRF6936_SPI_3PIN (1<<1)
|
||||
#define CYRF6936_PACTL_GPIO (1<<2)
|
||||
#define CYRF6936_PACTL_OD (1<<3)
|
||||
#define CYRF6936_XOUT_OD (1<<4)
|
||||
#define CYRF6936_MISO_OD (1<<5)
|
||||
#define CYRF6936_IRQ_POL (1<<6)
|
||||
#define CYRF6936_IRQ_OD (1<<7)
|
||||
|
||||
// CYRF6936_FRAMING_CFG
|
||||
#define CYRF6936_LEN_EN (1<<5)
|
||||
#define CYRF6936_SOP_LEN (1<<6)
|
||||
#define CYRF6936_SOP_EN (1<<7)
|
||||
|
||||
// CYRF6936_RX_STATUS
|
||||
enum {
|
||||
CYRF6936_RX_DATA_MODE_GFSK = 0x00,
|
||||
CYRF6936_RX_DATA_MODE_8DR = 0x01,
|
||||
CYRF6936_RX_DATA_MODE_DDR = 0x10,
|
||||
CYRF6936_RX_DATA_MODE_NV = 0x11,
|
||||
};
|
||||
#define CYRF6936_RX_CODE (1<<2)
|
||||
#define CYRF6936_BAD_CRC (1<<3)
|
||||
#define CYRF6936_CRC0 (1<<4)
|
||||
#define CYRF6936_EOP_ERR (1<<5)
|
||||
#define CYRF6936_PKT_ERR (1<<6)
|
||||
#define CYRF6936_RX_ACK (1<<7)
|
||||
|
||||
// CYRF6936_TX_IRQ_STATUS
|
||||
#define CYRF6936_TXE_IRQ (1<<0)
|
||||
#define CYRF6936_TXC_IRQ (1<<1)
|
||||
#define CYRF6936_TXBERR_IRQ (1<<2)
|
||||
#define CYRF6936_TXB0_IRQ (1<<3)
|
||||
#define CYRF6936_TXB8_IRQ (1<<4)
|
||||
#define CYRF6936_TXB15_IRQ (1<<5)
|
||||
#define CYRF6936_LV_IRQ (1<<6)
|
||||
#define CYRF6936_OS_IRQ (1<<7)
|
||||
|
||||
// CYRF6936_RX_IRQ_STATUS
|
||||
#define CYRF6936_RXE_IRQ (1<<0)
|
||||
#define CYRF6936_RXC_IRQ (1<<1)
|
||||
#define CYRF6936_RXBERR_IRQ (1<<2)
|
||||
#define CYRF6936_RXB1_IRQ (1<<3)
|
||||
#define CYRF6936_RXB8_IRQ (1<<4)
|
||||
#define CYRF6936_RXB16_IRQ (1<<5)
|
||||
#define CYRF6936_SOPDET_IRQ (1<<6)
|
||||
#define CYRF6936_RXOW_IRQ (1<<7)
|
||||
|
||||
// CYRF6936_TX_CTRL
|
||||
#define CYRF6936_TXE_IRQEN (1<<0)
|
||||
#define CYRF6936_TXC_IRQEN (1<<1)
|
||||
#define CYRF6936_TXBERR_IRQEN (1<<2)
|
||||
#define CYRF6936_TXB0_IRQEN (1<<3)
|
||||
#define CYRF6936_TXB8_IRQEN (1<<4)
|
||||
#define CYRF6936_TXB15_IRQEN (1<<5)
|
||||
#define CYRF6936_TX_CLR (1<<6)
|
||||
#define CYRF6936_TX_GO (1<<7)
|
||||
|
||||
// CYRF6936_RX_CTRL
|
||||
#define CYRF6936_RXE_IRQEN (1<<0)
|
||||
#define CYRF6936_RXC_IRQEN (1<<1)
|
||||
#define CYRF6936_RXBERR_IRQEN (1<<2)
|
||||
#define CYRF6936_RXB1_IRQEN (1<<3)
|
||||
#define CYRF6936_RXB8_IRQEN (1<<4)
|
||||
#define CYRF6936_RXB16_IRQEN (1<<5)
|
||||
#define CYRF6936_RSVD (1<<6)
|
||||
#define CYRF6936_RX_GO (1<<7)
|
||||
|
||||
// CYRF6936_RX_OVERRIDE
|
||||
#define CYRF6936_ACE (1<<1)
|
||||
#define CYRF6936_DIS_RXCRC (1<<2)
|
||||
#define CYRF6936_DIS_CRC0 (1<<3)
|
||||
#define CYRF6936_FRC_RXDR (1<<4)
|
||||
#define CYRF6936_MAN_RXACK (1<<5)
|
||||
#define CYRF6936_RXTX_DLY (1<<6)
|
||||
#define CYRF6936_ACK_RX (1<<7)
|
||||
|
||||
// CYRF6936_TX_OVERRIDE
|
||||
#define CYRF6936_TX_INV (1<<0)
|
||||
#define CYRF6936_DIS_TXCRC (1<<2)
|
||||
#define CYRF6936_OVRD_ACK (1<<3)
|
||||
#define CYRF6936_MAN_TXACK (1<<4)
|
||||
#define CYRF6936_FRC_PRE (1<<6)
|
||||
#define CYRF6936_ACK_TX (1<<7)
|
||||
|
||||
// CYRF6936_RX_CFG
|
||||
#define CYRF6936_VLD_EN (1<<0)
|
||||
#define CYRF6936_RXOW_EN (1<<1)
|
||||
#define CYRF6936_FAST_TURN_EN (1<<3)
|
||||
#define CYRF6936_HILO (1<<4)
|
||||
#define CYRF6936_ATT (1<<5)
|
||||
#define CYRF6936_LNA (1<<6)
|
||||
#define CYRF6936_AGC_EN (1<<7)
|
||||
|
||||
// CYRF6936_TX_CFG
|
||||
enum {
|
||||
CYRF6936_PA_M35 = 0x0,
|
||||
CYRF6936_PA_M30 = 0x1,
|
||||
CYRF6936_PA_M24 = 0x2,
|
||||
CYRF6936_PA_M18 = 0x3,
|
||||
CYRF6936_PA_M13 = 0x4,
|
||||
CYRF6936_PA_M5 = 0x5,
|
||||
CYRF6936_PA_0 = 0x6,
|
||||
CYRF6936_PA_4 = 0x7,
|
||||
};
|
||||
enum {
|
||||
CYRF6936_DATA_MODE_GFSK = (0x0 << 3),
|
||||
CYRF6936_DATA_MODE_8DR = (0x1 << 3),
|
||||
CYRF6936_DATA_MODE_DDR = (0x2 << 3),
|
||||
CYRF6936_DATA_MODE_SDR = (0x3 << 3),
|
||||
};
|
||||
#define CYRF6936_DATA_CODE_LENGTH (1<<5)
|
||||
|
||||
extern volatile bool isError;
|
||||
|
||||
bool cyrf6936Init(void);
|
||||
|
||||
bool cyrf6936RxFinished(uint32_t *timeStamp);
|
||||
|
||||
void cyrf6936WriteRegister(const uint8_t address, const uint8_t data);
|
||||
void cyrf6936WriteBlock(const uint8_t address, const uint8_t *data, const uint8_t length);
|
||||
uint8_t cyrf6936ReadRegister(const uint8_t address);
|
||||
void cyrf6936ReadBlock(const uint8_t address, uint8_t *data, const uint8_t length);
|
||||
|
||||
uint8_t cyrf6936GetRssi(void);
|
||||
uint8_t cyrf6936GetRxStatus(void);
|
||||
void cyrf6936SetConfigLen(const uint8_t config[][2], const uint8_t length);
|
||||
void cyrf6936SetChannel(const uint8_t chan);
|
||||
void cyrf6936SetMode(const uint8_t mode, const bool force);
|
||||
void cyrf6936SetCrcSeed(const uint16_t crc);
|
||||
void cyrf6936SetSopCode(const uint8_t *sopcode);
|
||||
void cyrf6936SetDataCode(const uint8_t *datacode);
|
||||
|
||||
void cyrf6936SendLen(const uint8_t *data, const uint8_t length);
|
||||
void cyrf6936StartRecv(void);
|
||||
void cyrf6936RecvLen(uint8_t *data, const uint8_t length);
|
||||
|
|
@ -2616,7 +2616,10 @@ void cliRxSpiBind(char *cmdline){
|
|||
case RX_SPI_A7105_FLYSKY:
|
||||
case RX_SPI_A7105_FLYSKY_2A:
|
||||
#endif
|
||||
#if defined(USE_RX_FRSKY_SPI) || defined(USE_RX_SFHSS_SPI) || defined(USE_RX_FLYSKY)
|
||||
#ifdef USE_RX_SPEKTRUM
|
||||
case RX_SPI_CYRF6936_DSM:
|
||||
#endif
|
||||
#if defined(USE_RX_FRSKY_SPI) || defined(USE_RX_SFHSS_SPI) || defined(USE_RX_FLYSKY) || defined(USE_RX_SPEKTRUM)
|
||||
rxSpiBind();
|
||||
cliPrint("Binding...");
|
||||
break;
|
||||
|
|
|
|||
|
|
@ -90,6 +90,7 @@
|
|||
#include "rx/cc2500_frsky_common.h"
|
||||
#include "rx/cc2500_sfhss.h"
|
||||
#include "rx/spektrum.h"
|
||||
#include "rx/cyrf6936_spektrum.h"
|
||||
|
||||
#include "sensors/acceleration.h"
|
||||
#include "sensors/barometer.h"
|
||||
|
|
@ -227,7 +228,8 @@ static const char * const lookupTableRxSpi[] = {
|
|||
"FLYSKY",
|
||||
"FLYSKY_2A",
|
||||
"KN",
|
||||
"SFHSS"
|
||||
"SFHSS",
|
||||
"SPEKTRUM"
|
||||
};
|
||||
#endif
|
||||
|
||||
|
|
@ -1259,6 +1261,11 @@ const clivalue_t valueTable[] = {
|
|||
#ifdef USE_MCO
|
||||
{ "mco2_on_pc9", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MCO_CONFIG, offsetof(mcoConfig_t, enabled[1]) },
|
||||
#endif
|
||||
#ifdef USE_RX_SPEKTRUM
|
||||
{ "spektrum_spi_protocol", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 255 }, PG_RX_SPEKTRUM_SPI_CONFIG, offsetof(spektrumConfig_t, protocol) },
|
||||
{ "spektrum_spi_mfg_id", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = 4, PG_RX_SPEKTRUM_SPI_CONFIG, offsetof(spektrumConfig_t, mfgId) },
|
||||
{ "spektrum_spi_num_channels", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, DSM_MAX_CHANNEL_COUNT }, PG_RX_SPEKTRUM_SPI_CONFIG, offsetof(spektrumConfig_t, numChannels) },
|
||||
#endif
|
||||
};
|
||||
|
||||
const uint16_t valueTableEntryCount = ARRAYLEN(valueTable);
|
||||
|
|
|
|||
|
|
@ -136,7 +136,8 @@
|
|||
#define PG_RCDEVICE_CONFIG 539
|
||||
#define PG_GYRO_DEVICE_CONFIG 540
|
||||
#define PG_MCO_CONFIG 541
|
||||
#define PG_BETAFLIGHT_END 541
|
||||
#define PG_RX_SPEKTRUM_SPI_CONFIG 542
|
||||
#define PG_BETAFLIGHT_END 542
|
||||
|
||||
|
||||
// OSD configuration (subject to change)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,610 @@
|
|||
/*
|
||||
* This file is part of Cleanflight and Betaflight.
|
||||
*
|
||||
* Cleanflight and Betaflight are free software. You can redistribute
|
||||
* this software and/or modify this software under the terms of the
|
||||
* GNU General Public License as published by the Free Software
|
||||
* Foundation, either version 3 of the License, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* Cleanflight and Betaflight are distributed in the hope that they
|
||||
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
|
||||
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||
* See the GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this software.
|
||||
*
|
||||
* If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <string.h>
|
||||
#include "platform.h"
|
||||
|
||||
#ifdef USE_RX_SPEKTRUM
|
||||
|
||||
#include "build/debug.h"
|
||||
|
||||
#include "drivers/io.h"
|
||||
#include "drivers/rx/rx_cyrf6936.h"
|
||||
#include "drivers/system.h"
|
||||
#include "drivers/time.h"
|
||||
|
||||
#include "sensors/battery.h"
|
||||
|
||||
#include "fc/config.h"
|
||||
#include "config/feature.h"
|
||||
|
||||
#include "pg/pg.h"
|
||||
#include "pg/pg_ids.h"
|
||||
#include "pg/rx_spi.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
#include "rx/rx_spi.h"
|
||||
#include "rx/rx_spi_common.h"
|
||||
|
||||
#include "rx/cyrf6936_spektrum.h"
|
||||
|
||||
static const uint8_t pnCodes[5][9][8] = {
|
||||
{ /* Row 0 */
|
||||
/* Col 0 */ {0x03, 0xBC, 0x6E, 0x8A, 0xEF, 0xBD, 0xFE, 0xF8},
|
||||
/* Col 1 */ {0x88, 0x17, 0x13, 0x3B, 0x2D, 0xBF, 0x06, 0xD6},
|
||||
/* Col 2 */ {0xF1, 0x94, 0x30, 0x21, 0xA1, 0x1C, 0x88, 0xA9},
|
||||
/* Col 3 */ {0xD0, 0xD2, 0x8E, 0xBC, 0x82, 0x2F, 0xE3, 0xB4},
|
||||
/* Col 4 */ {0x8C, 0xFA, 0x47, 0x9B, 0x83, 0xA5, 0x66, 0xD0},
|
||||
/* Col 5 */ {0x07, 0xBD, 0x9F, 0x26, 0xC8, 0x31, 0x0F, 0xB8},
|
||||
/* Col 6 */ {0xEF, 0x03, 0x95, 0x89, 0xB4, 0x71, 0x61, 0x9D},
|
||||
/* Col 7 */ {0x40, 0xBA, 0x97, 0xD5, 0x86, 0x4F, 0xCC, 0xD1},
|
||||
/* Col 8 */ {0xD7, 0xA1, 0x54, 0xB1, 0x5E, 0x89, 0xAE, 0x86}
|
||||
},
|
||||
{ /* Row 1 */
|
||||
/* Col 0 */ {0x83, 0xF7, 0xA8, 0x2D, 0x7A, 0x44, 0x64, 0xD3},
|
||||
/* Col 1 */ {0x3F, 0x2C, 0x4E, 0xAA, 0x71, 0x48, 0x7A, 0xC9},
|
||||
/* Col 2 */ {0x17, 0xFF, 0x9E, 0x21, 0x36, 0x90, 0xC7, 0x82},
|
||||
/* Col 3 */ {0xBC, 0x5D, 0x9A, 0x5B, 0xEE, 0x7F, 0x42, 0xEB},
|
||||
/* Col 4 */ {0x24, 0xF5, 0xDD, 0xF8, 0x7A, 0x77, 0x74, 0xE7},
|
||||
/* Col 5 */ {0x3D, 0x70, 0x7C, 0x94, 0xDC, 0x84, 0xAD, 0x95},
|
||||
/* Col 6 */ {0x1E, 0x6A, 0xF0, 0x37, 0x52, 0x7B, 0x11, 0xD4},
|
||||
/* Col 7 */ {0x62, 0xF5, 0x2B, 0xAA, 0xFC, 0x33, 0xBF, 0xAF},
|
||||
/* Col 8 */ {0x40, 0x56, 0x32, 0xD9, 0x0F, 0xD9, 0x5D, 0x97}
|
||||
},
|
||||
{ /* Row 2 */
|
||||
/* Col 0 */ {0x40, 0x56, 0x32, 0xD9, 0x0F, 0xD9, 0x5D, 0x97},
|
||||
/* Col 1 */ {0x8E, 0x4A, 0xD0, 0xA9, 0xA7, 0xFF, 0x20, 0xCA},
|
||||
/* Col 2 */ {0x4C, 0x97, 0x9D, 0xBF, 0xB8, 0x3D, 0xB5, 0xBE},
|
||||
/* Col 3 */ {0x0C, 0x5D, 0x24, 0x30, 0x9F, 0xCA, 0x6D, 0xBD},
|
||||
/* Col 4 */ {0x50, 0x14, 0x33, 0xDE, 0xF1, 0x78, 0x95, 0xAD},
|
||||
/* Col 5 */ {0x0C, 0x3C, 0xFA, 0xF9, 0xF0, 0xF2, 0x10, 0xC9},
|
||||
/* Col 6 */ {0xF4, 0xDA, 0x06, 0xDB, 0xBF, 0x4E, 0x6F, 0xB3},
|
||||
/* Col 7 */ {0x9E, 0x08, 0xD1, 0xAE, 0x59, 0x5E, 0xE8, 0xF0},
|
||||
/* Col 8 */ {0xC0, 0x90, 0x8F, 0xBB, 0x7C, 0x8E, 0x2B, 0x8E}
|
||||
},
|
||||
{ /* Row 3 */
|
||||
/* Col 0 */ {0xC0, 0x90, 0x8F, 0xBB, 0x7C, 0x8E, 0x2B, 0x8E},
|
||||
/* Col 1 */ {0x80, 0x69, 0x26, 0x80, 0x08, 0xF8, 0x49, 0xE7},
|
||||
/* Col 2 */ {0x7D, 0x2D, 0x49, 0x54, 0xD0, 0x80, 0x40, 0xC1},
|
||||
/* Col 3 */ {0xB6, 0xF2, 0xE6, 0x1B, 0x80, 0x5A, 0x36, 0xB4},
|
||||
/* Col 4 */ {0x42, 0xAE, 0x9C, 0x1C, 0xDA, 0x67, 0x05, 0xF6},
|
||||
/* Col 5 */ {0x9B, 0x75, 0xF7, 0xE0, 0x14, 0x8D, 0xB5, 0x80},
|
||||
/* Col 6 */ {0xBF, 0x54, 0x98, 0xB9, 0xB7, 0x30, 0x5A, 0x88},
|
||||
/* Col 7 */ {0x35, 0xD1, 0xFC, 0x97, 0x23, 0xD4, 0xC9, 0x88},
|
||||
/* Col 8 */ {0xE1, 0xD6, 0x31, 0x26, 0x5F, 0xBD, 0x40, 0x93}
|
||||
},
|
||||
{ /* Row 4 */
|
||||
/* Col 0 */ {0xE1, 0xD6, 0x31, 0x26, 0x5F, 0xBD, 0x40, 0x93},
|
||||
/* Col 1 */ {0xDC, 0x68, 0x08, 0x99, 0x97, 0xAE, 0xAF, 0x8C},
|
||||
/* Col 2 */ {0xC3, 0x0E, 0x01, 0x16, 0x0E, 0x32, 0x06, 0xBA},
|
||||
/* Col 3 */ {0xE0, 0x83, 0x01, 0xFA, 0xAB, 0x3E, 0x8F, 0xAC},
|
||||
/* Col 4 */ {0x5C, 0xD5, 0x9C, 0xB8, 0x46, 0x9C, 0x7D, 0x84},
|
||||
/* Col 5 */ {0xF1, 0xC6, 0xFE, 0x5C, 0x9D, 0xA5, 0x4F, 0xB7},
|
||||
/* Col 6 */ {0x58, 0xB5, 0xB3, 0xDD, 0x0E, 0x28, 0xF1, 0xB0},
|
||||
/* Col 7 */ {0x5F, 0x30, 0x3B, 0x56, 0x96, 0x45, 0xF4, 0xA1},
|
||||
/* Col 8 */ {0x03, 0xBC, 0x6E, 0x8A, 0xEF, 0xBD, 0xFE, 0xF8}
|
||||
},
|
||||
};
|
||||
|
||||
static const uint8_t cyrf6936Config[][2] = {
|
||||
{CYRF6936_CLK_EN, CYRF6936_RXF}, // Enable the clock
|
||||
{CYRF6936_AUTO_CAL_TIME, 0x3C}, // From manual, needed for initialization
|
||||
{CYRF6936_AUTO_CAL_OFFSET, 0x14}, // From manual, needed for initialization
|
||||
{CYRF6936_RX_CFG, CYRF6936_LNA | CYRF6936_FAST_TURN_EN}, // Enable low noise amplifier and fast turning
|
||||
{CYRF6936_TX_OFFSET_LSB, 0x55}, // From manual, typical configuration
|
||||
{CYRF6936_TX_OFFSET_MSB, 0x05}, // From manual, typical configuration
|
||||
{CYRF6936_XACT_CFG, CYRF6936_MODE_SYNTH_RX | CYRF6936_FRC_END}, // Force in Synth RX mode
|
||||
{CYRF6936_TX_CFG, CYRF6936_DATA_CODE_LENGTH | CYRF6936_DATA_MODE_SDR | CYRF6936_PA_4}, // Enable 64 chip codes, SDR mode and amplifier +4dBm
|
||||
{CYRF6936_DATA64_THOLD, 0x0E}, // From manual, typical configuration
|
||||
};
|
||||
static const uint8_t cyrf6936BindConfig[][2] = {
|
||||
{CYRF6936_TX_CFG, CYRF6936_DATA_CODE_LENGTH | CYRF6936_DATA_MODE_SDR | CYRF6936_PA_4}, // Enable 64 chip codes, SDR mode and amplifier +4dBm
|
||||
{CYRF6936_FRAMING_CFG, CYRF6936_SOP_LEN | 0xE}, // Set SOP CODE to 64 chips and SOP Correlator Threshold to 0xE
|
||||
{CYRF6936_RX_OVERRIDE, CYRF6936_FRC_RXDR | CYRF6936_DIS_RXCRC}, // Force receive data rate and disable receive CRC checker
|
||||
{CYRF6936_EOP_CTRL, 0x02}, // Only enable EOP symbol count of 2
|
||||
{CYRF6936_TX_OVERRIDE, CYRF6936_DIS_TXCRC}, // Disable transmit CRC generate
|
||||
};
|
||||
static const uint8_t cyrf6936TransferConfig[][2] = {
|
||||
{CYRF6936_TX_CFG, CYRF6936_DATA_CODE_LENGTH | CYRF6936_DATA_MODE_8DR | CYRF6936_PA_4}, // Enable 64 chip codes, 8DR mode and amplifier +4dBm
|
||||
{CYRF6936_FRAMING_CFG, CYRF6936_SOP_EN | CYRF6936_SOP_LEN | CYRF6936_LEN_EN | 0xE}, // Set SOP CODE enable, SOP CODE to 64 chips, Packet length enable, and SOP Correlator Threshold to 0xE
|
||||
{CYRF6936_TX_OVERRIDE, 0x00}, // Reset TX overrides
|
||||
{CYRF6936_RX_OVERRIDE, 0x00}, // Reset RX overrides
|
||||
};
|
||||
|
||||
typedef enum {
|
||||
DSM2_22 = 0x01,
|
||||
DSM2_11 = 0x02,
|
||||
DSM2_11_DX8 = 0x12,
|
||||
DSMX_22 = 0xA2,
|
||||
DSMX_11 = 0xB2,
|
||||
} dsm_protocol_e;
|
||||
|
||||
#define IS_DSM2(x) (x == DSM2_22 || x == DSM2_11 || x == DSM2_11_DX8)
|
||||
#define IS_DSMX(x) (!IS_DSM2(x))
|
||||
|
||||
#define CHECK_MFG_ID(protocol, packet, id) ((IS_DSM2(protocol) && packet[0] == (~id[2]&0xFF) && packet[1] == (~id[3]&0xFF)) || \
|
||||
(IS_DSMX(protocol) && packet[0] == id[2] && packet[1] == id[3]))
|
||||
|
||||
typedef enum {
|
||||
DSM_RECEIVER_BIND = 0x0,
|
||||
DSM_RECEIVER_SYNC_A = 0x1,
|
||||
DSM_RECEIVER_SYNC_B = 0x2,
|
||||
DSM_RECEIVER_RECV = 0x3,
|
||||
#ifdef USE_RX_SPEKTRUM_TELEMETRY
|
||||
DSM_RECEIVER_TLM = 0x4,
|
||||
#endif
|
||||
} dsm_receiver_status_e;
|
||||
|
||||
typedef struct dsmReceiver_s {
|
||||
dsm_receiver_status_e status;
|
||||
dsm_protocol_e protocol;
|
||||
|
||||
uint8_t mfgId[4];
|
||||
|
||||
uint8_t rfChannel;
|
||||
uint8_t rfChannelIdx;
|
||||
uint8_t rfChannels[23];
|
||||
|
||||
uint8_t sopCol;
|
||||
uint8_t dataCol;
|
||||
uint16_t crcSeed;
|
||||
|
||||
uint8_t missedPackets;
|
||||
uint8_t numChannels;
|
||||
|
||||
bool bound;
|
||||
|
||||
uint32_t timeout;
|
||||
uint32_t timeLastPacket;
|
||||
|
||||
#ifdef USE_RX_SPEKTRUM_TELEMETRY
|
||||
uint32_t timeLastTelemetry;
|
||||
bool sendTelemetry;
|
||||
#endif
|
||||
} dsmReceiver_t;
|
||||
|
||||
STATIC_UNIT_TESTED dsmReceiver_t dsmReceiver;
|
||||
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(spektrumConfig_t, spektrumConfig, PG_RX_SPEKTRUM_SPI_CONFIG, 0);
|
||||
PG_RESET_TEMPLATE(spektrumConfig_t, spektrumConfig, .protocol = 0, .mfgId = {0, 0, 0, 0}, .numChannels = 0);
|
||||
|
||||
static void dsmGenerateDsmxChannels(void)
|
||||
{
|
||||
unsigned idx = 0;
|
||||
const uint32_t id = ~((dsmReceiver.mfgId[0] << 24) | (dsmReceiver.mfgId[1] << 16) | (dsmReceiver.mfgId[2] << 8) | (dsmReceiver.mfgId[3] << 0));
|
||||
uint32_t idTmp = id;
|
||||
|
||||
while (idx < 23) {
|
||||
unsigned i;
|
||||
unsigned count3To27 = 0, count28To51 = 0, count52To76 = 0;
|
||||
|
||||
idTmp = idTmp * 0x0019660D + 0x3C6EF35F; // Randomization
|
||||
const uint8_t nextCh = ((idTmp >> 8) % 0x49) + 3;
|
||||
if (((nextCh ^ id) & 0x01) == 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
for (i = 0; i < idx; i++) {
|
||||
if (dsmReceiver.rfChannels[i] == nextCh) {
|
||||
break;
|
||||
}
|
||||
|
||||
if (dsmReceiver.rfChannels[i] <= 27) {
|
||||
count3To27++;
|
||||
} else if (dsmReceiver.rfChannels[i] <= 51) {
|
||||
count28To51++;
|
||||
} else {
|
||||
count52To76++;
|
||||
}
|
||||
}
|
||||
|
||||
if (i != idx) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if ((nextCh < 28 && count3To27 < 8)
|
||||
|| (nextCh >= 28 && nextCh < 52 && count28To51 < 7)
|
||||
|| (nextCh >= 52 && count52To76 < 8)) {
|
||||
dsmReceiver.rfChannels[idx++] = nextCh;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void dsmSetChannel(const uint8_t channel, const uint8_t sopCol, const uint8_t dataCol, const uint16_t crcSeed)
|
||||
{
|
||||
const uint8_t pnRow = IS_DSM2(dsmReceiver.protocol) ? channel % 5 : (channel - 2) % 5;
|
||||
|
||||
cyrf6936SetCrcSeed(crcSeed);
|
||||
cyrf6936SetSopCode(pnCodes[pnRow][sopCol]);
|
||||
cyrf6936SetDataCode(pnCodes[pnRow][dataCol]);
|
||||
|
||||
cyrf6936SetChannel(channel);
|
||||
}
|
||||
|
||||
static void dsmReceiverSetNextSyncChannel(void)
|
||||
{
|
||||
dsmReceiver.crcSeed = ~dsmReceiver.crcSeed;
|
||||
dsmReceiver.rfChannel = (dsmReceiver.rfChannel + 1) % DSM_MAX_RF_CHANNEL;
|
||||
dsmSetChannel(dsmReceiver.rfChannel, dsmReceiver.sopCol, dsmReceiver.dataCol, dsmReceiver.crcSeed);
|
||||
}
|
||||
|
||||
static void dsmReceiverSetNextChannel(void)
|
||||
{
|
||||
dsmReceiver.rfChannelIdx = IS_DSM2(dsmReceiver.protocol) ? (dsmReceiver.rfChannelIdx + 1) % 2 : (dsmReceiver.rfChannelIdx + 1) % 23;
|
||||
dsmReceiver.crcSeed = ~dsmReceiver.crcSeed;
|
||||
dsmReceiver.rfChannel = dsmReceiver.rfChannels[dsmReceiver.rfChannelIdx];
|
||||
dsmSetChannel(dsmReceiver.rfChannel, dsmReceiver.sopCol, dsmReceiver.dataCol, dsmReceiver.crcSeed);
|
||||
}
|
||||
|
||||
static void resetReceiveTimeout(const uint32_t timeStamp)
|
||||
{
|
||||
dsmReceiver.timeLastPacket = timeStamp;
|
||||
if (dsmReceiver.crcSeed == ((dsmReceiver.mfgId[0] << 8) + dsmReceiver.mfgId[1])) {
|
||||
dsmReceiver.timeout = DSM_RECV_SHORT_TIMEOUT_US;
|
||||
} else {
|
||||
dsmReceiver.timeout = (dsmReceiver.numChannels < 8 ? DSM_RECV_LONG_TIMEOUT_US : DSM_RECV_MID_TIMEOUT_US);
|
||||
}
|
||||
}
|
||||
|
||||
static void checkTimeout(void)
|
||||
{
|
||||
const uint32_t time = micros();
|
||||
|
||||
#ifdef USE_RX_SPEKTRUM_TELEMETRY
|
||||
if (featureIsEnabled(FEATURE_TELEMETRY) && (time - dsmReceiver.timeLastTelemetry) > DSM_TELEMETRY_TIME_US) {
|
||||
dsmReceiver.timeLastTelemetry = time;
|
||||
dsmReceiver.sendTelemetry = true;
|
||||
}
|
||||
#endif
|
||||
|
||||
if ((time - dsmReceiver.timeLastPacket) > dsmReceiver.timeout) {
|
||||
cyrf6936SetMode(CYRF6936_MODE_SYNTH_RX, true);
|
||||
cyrf6936WriteRegister(CYRF6936_RX_ABORT, 0x00);
|
||||
|
||||
dsmReceiver.timeLastPacket += dsmReceiver.timeout;
|
||||
|
||||
switch (dsmReceiver.status) {
|
||||
case DSM_RECEIVER_BIND:
|
||||
dsmReceiver.rfChannel = (dsmReceiver.rfChannel + 2) % DSM_MAX_RF_CHANNEL;
|
||||
cyrf6936SetChannel(dsmReceiver.rfChannel);
|
||||
dsmReceiver.timeout = DSM_BIND_TIMEOUT_US;
|
||||
cyrf6936StartRecv();
|
||||
break;
|
||||
case DSM_RECEIVER_SYNC_A:
|
||||
case DSM_RECEIVER_SYNC_B:
|
||||
IS_DSM2(dsmReceiver.protocol) ? dsmReceiverSetNextSyncChannel() : dsmReceiverSetNextChannel();
|
||||
dsmReceiver.timeout = DSM_SYNC_TIMEOUT_US;
|
||||
cyrf6936StartRecv();
|
||||
break;
|
||||
case DSM_RECEIVER_RECV:
|
||||
dsmReceiver.missedPackets++;
|
||||
DEBUG_SET(DEBUG_RX_SPEKTRUM_SPI, 0, dsmReceiver.missedPackets);
|
||||
if (dsmReceiver.missedPackets < DSM_MAX_MISSED_PACKETS) {
|
||||
dsmReceiverSetNextChannel();
|
||||
if (dsmReceiver.crcSeed == ((dsmReceiver.mfgId[0] << 8) + dsmReceiver.mfgId[1])) {
|
||||
dsmReceiver.timeout = DSM_RECV_SHORT_TIMEOUT_US;
|
||||
} else {
|
||||
dsmReceiver.timeout = DSM_RECV_TIMEOUT_OFFSET_US + (dsmReceiver.numChannels < 8 ? DSM_RECV_LONG_TIMEOUT_US : DSM_RECV_MID_TIMEOUT_US);
|
||||
}
|
||||
cyrf6936StartRecv();
|
||||
} else {
|
||||
setRssiDirect(0, RSSI_SOURCE_RX_PROTOCOL);
|
||||
dsmReceiver.status = DSM_RECEIVER_SYNC_A;
|
||||
dsmReceiver.timeout = DSM_SYNC_TIMEOUT_US;
|
||||
}
|
||||
break;
|
||||
#ifdef USE_RX_SPEKTRUM_TELEMETRY
|
||||
case DSM_RECEIVER_TLM:
|
||||
DEBUG_SET(DEBUG_RX_SPEKTRUM_SPI, 2, (cyrf6936ReadRegister(CYRF6936_TX_IRQ_STATUS) & CYRF6936_TXC_IRQ) == 0);
|
||||
dsmReceiverSetNextChannel();
|
||||
dsmReceiver.status = DSM_RECEIVER_RECV;
|
||||
dsmReceiver.timeout = (dsmReceiver.numChannels < 8 ? DSM_RECV_LONG_TIMEOUT_US : DSM_RECV_MID_TIMEOUT_US) - DSM_TELEMETRY_TIMEOUT_US;
|
||||
cyrf6936StartRecv();
|
||||
break;
|
||||
#endif
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void dsmReceiverStartBind(void)
|
||||
{
|
||||
uint8_t dataCode[16];
|
||||
dsmReceiver.status = DSM_RECEIVER_BIND;
|
||||
|
||||
cyrf6936SetConfigLen(cyrf6936BindConfig, ARRAYLEN(cyrf6936BindConfig));
|
||||
|
||||
memcpy(dataCode, pnCodes[0][8], 8);
|
||||
memcpy(dataCode + 8, pnCodes[0][8], 8);
|
||||
cyrf6936SetDataCode(dataCode);
|
||||
|
||||
dsmReceiver.rfChannel = DSM_INITIAL_BIND_CHANNEL;
|
||||
cyrf6936SetChannel(dsmReceiver.rfChannel);
|
||||
dsmReceiver.timeLastPacket = micros();
|
||||
dsmReceiver.timeout = DSM_BIND_TIMEOUT_US;
|
||||
cyrf6936StartRecv();
|
||||
}
|
||||
|
||||
static void dsmReceiverStartTransfer(void)
|
||||
{
|
||||
dsmReceiver.status = DSM_RECEIVER_SYNC_A;
|
||||
dsmReceiver.rfChannelIdx = 0;
|
||||
dsmReceiver.missedPackets = 0;
|
||||
|
||||
cyrf6936SetConfigLen(cyrf6936TransferConfig, ARRAYLEN(cyrf6936TransferConfig));
|
||||
|
||||
dsmReceiver.numChannels = spektrumConfig()->numChannels;
|
||||
dsmReceiver.protocol = spektrumConfig()->protocol;
|
||||
|
||||
dsmReceiver.crcSeed = ~((dsmReceiver.mfgId[0] << 8) + dsmReceiver.mfgId[1]);
|
||||
dsmReceiver.sopCol = (dsmReceiver.mfgId[0] + dsmReceiver.mfgId[1] + dsmReceiver.mfgId[2] + 2) & 0x07;
|
||||
dsmReceiver.dataCol = 7 - dsmReceiver.sopCol;
|
||||
|
||||
if (IS_DSMX(dsmReceiver.protocol)) {
|
||||
dsmGenerateDsmxChannels();
|
||||
dsmReceiver.rfChannelIdx = 22;
|
||||
dsmReceiverSetNextChannel();
|
||||
} else {
|
||||
memset(dsmReceiver.rfChannels, 0, 23);
|
||||
dsmReceiverSetNextSyncChannel();
|
||||
}
|
||||
|
||||
dsmReceiver.timeLastPacket = micros();
|
||||
dsmReceiver.timeout = DSM_SYNC_TIMEOUT_US << 2;
|
||||
cyrf6936StartRecv();
|
||||
}
|
||||
|
||||
bool spektrumSpiInit(const struct rxSpiConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig)
|
||||
{
|
||||
rxSpiCommonIOInit(rxConfig);
|
||||
|
||||
rxRuntimeConfig->channelCount = DSM_MAX_CHANNEL_COUNT;
|
||||
|
||||
if (!cyrf6936Init()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (rssiSource == RSSI_SOURCE_NONE) {
|
||||
rssiSource = RSSI_SOURCE_RX_PROTOCOL;
|
||||
}
|
||||
|
||||
cyrf6936SetConfigLen(cyrf6936Config, ARRAYLEN(cyrf6936Config));
|
||||
|
||||
#ifdef USE_RX_SPEKTRUM_TELEMETRY
|
||||
dsmReceiver.timeLastTelemetry = micros() + DSM_TELEMETRY_TIME_US;
|
||||
dsmReceiver.sendTelemetry = false;
|
||||
#endif
|
||||
|
||||
if (spektrumConfig()->mfgId[0] || spektrumConfig()->mfgId[1]
|
||||
|| spektrumConfig()->mfgId[2] || spektrumConfig()->mfgId[3]) {
|
||||
dsmReceiver.bound = true;
|
||||
memcpy(dsmReceiver.mfgId, spektrumConfig()->mfgId, 4);
|
||||
dsmReceiverStartTransfer();
|
||||
} else {
|
||||
dsmReceiver.bound = false;
|
||||
dsmReceiverStartBind();
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
#ifdef USE_RX_SPEKTRUM_TELEMETRY
|
||||
static void dsmSendTelemetryPacket(void)
|
||||
{
|
||||
uint8_t packet[9];
|
||||
const uint16_t voltage = getBatteryVoltage() * 10;
|
||||
|
||||
packet[0] = DSM_TELEMETRY_FRAME_RPM;
|
||||
packet[1] = 0xFF; //sid
|
||||
packet[2] = 0xFF; //rpm
|
||||
packet[3] = 0xFF; //rpm
|
||||
packet[4] = voltage >> 8;
|
||||
packet[5] = voltage & 0xFF;
|
||||
packet[6] = 0x7F; //temperature
|
||||
packet[7] = 0xFF; //temperature
|
||||
packet[8] = getRssiPercent();
|
||||
cyrf6936SetMode(CYRF6936_MODE_IDLE, true);
|
||||
cyrf6936SendLen(packet, 9);
|
||||
}
|
||||
#endif
|
||||
|
||||
void spektrumSpiSetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload)
|
||||
{
|
||||
if (rcData && payload) {
|
||||
const uint8_t divider = (uint8_t) IS_DSMX(dsmReceiver.protocol);
|
||||
const uint8_t bitShift = 10 + divider;
|
||||
const uint16_t valueMax = IS_DSMX(dsmReceiver.protocol) ? 0x7FF : 0x3FF;
|
||||
|
||||
for (unsigned i = 0; i < 7; i++) {
|
||||
const uint16_t tmp = (payload[2 * i] << 8) + payload[2 * i + 1];
|
||||
const uint8_t chan = (tmp >> bitShift) & 0x0F;
|
||||
const int16_t val = (tmp & valueMax) >> divider;
|
||||
|
||||
if (chan < dsmReceiver.numChannels) {
|
||||
rcData[chan] = 988 + val;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static bool isValidPacket(const uint8_t *packet)
|
||||
{
|
||||
DEBUG_SET(DEBUG_RX_SPEKTRUM_SPI, 1, isError);
|
||||
if (isError) {
|
||||
if (dsmReceiver.status != DSM_RECEIVER_RECV && (cyrf6936GetRxStatus() & CYRF6936_BAD_CRC)) {
|
||||
dsmReceiver.crcSeed = ~dsmReceiver.crcSeed;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
if (!CHECK_MFG_ID(dsmReceiver.protocol, packet, dsmReceiver.mfgId)) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
rx_spi_received_e spektrumReadPacket(uint8_t *payload, const uint32_t timeStamp)
|
||||
{
|
||||
rx_spi_received_e result = RX_SPI_RECEIVED_NONE;
|
||||
|
||||
uint8_t packetLength, packet[16];
|
||||
|
||||
packetLength = cyrf6936ReadRegister(CYRF6936_RX_COUNT);
|
||||
cyrf6936RecvLen(packet, packetLength);
|
||||
|
||||
cyrf6936WriteRegister(CYRF6936_XACT_CFG, CYRF6936_MODE_SYNTH_RX | CYRF6936_FRC_END);
|
||||
cyrf6936WriteRegister(CYRF6936_RX_ABORT, 0x00);
|
||||
|
||||
if (packetLength < 2) {
|
||||
return result;
|
||||
}
|
||||
|
||||
switch (dsmReceiver.status) {
|
||||
case DSM_RECEIVER_BIND:
|
||||
if (packet[0] != packet[4] || packet[1] != packet[5]
|
||||
|| packet[2] != packet[6] || packet[3] != packet[7]) {
|
||||
|
||||
dsmReceiver.timeLastPacket = timeStamp;
|
||||
dsmReceiver.timeout = DSM_BIND_TIMEOUT_US;
|
||||
break;
|
||||
}
|
||||
|
||||
unsigned i;
|
||||
uint16_t bindSum = 384 - 0x10;
|
||||
for (i = 0; i < 8; i++) {
|
||||
bindSum += packet[i];
|
||||
}
|
||||
|
||||
if (packet[8] != bindSum >> 8 || packet[9] != (bindSum & 0xFF)) {
|
||||
break;
|
||||
}
|
||||
|
||||
for (i = 8; i < 14; i++) {
|
||||
bindSum += packet[i];
|
||||
}
|
||||
|
||||
if (packet[14] != bindSum >> 8 || packet[15] != (bindSum & 0xFF)) {
|
||||
break;
|
||||
}
|
||||
|
||||
dsmReceiver.mfgId[0] = ~packet[0];
|
||||
dsmReceiver.mfgId[1] = ~packet[1];
|
||||
dsmReceiver.mfgId[2] = ~packet[2];
|
||||
dsmReceiver.mfgId[3] = ~packet[3];
|
||||
dsmReceiver.numChannels = packet[11];
|
||||
dsmReceiver.protocol = packet[12];
|
||||
memcpy(spektrumConfigMutable()->mfgId, dsmReceiver.mfgId, 4);
|
||||
spektrumConfigMutable()->numChannels = dsmReceiver.numChannels;
|
||||
spektrumConfigMutable()->protocol = dsmReceiver.protocol;
|
||||
writeEEPROM();
|
||||
|
||||
dsmReceiverStartTransfer();
|
||||
|
||||
dsmReceiver.bound = true;
|
||||
result = RX_SPI_RECEIVED_BIND;
|
||||
break;
|
||||
case DSM_RECEIVER_SYNC_A:
|
||||
if (isValidPacket(packet)) {
|
||||
if (IS_DSM2(dsmReceiver.protocol)) {
|
||||
dsmReceiver.rfChannels[0] = dsmReceiver.rfChannel;
|
||||
dsmReceiver.rfChannels[1] = dsmReceiver.rfChannel;
|
||||
|
||||
dsmReceiver.status = DSM_RECEIVER_SYNC_B;
|
||||
} else {
|
||||
dsmReceiver.status = DSM_RECEIVER_RECV;
|
||||
dsmReceiver.missedPackets = 0;
|
||||
}
|
||||
|
||||
IS_DSM2(dsmReceiver.protocol) ? dsmReceiverSetNextSyncChannel() : dsmReceiverSetNextChannel();
|
||||
resetReceiveTimeout(timeStamp);
|
||||
cyrf6936StartRecv();
|
||||
}
|
||||
break;
|
||||
case DSM_RECEIVER_SYNC_B:
|
||||
if (isValidPacket(packet)) {
|
||||
if (dsmReceiver.crcSeed != ((dsmReceiver.mfgId[0] << 8) + dsmReceiver.mfgId[1])) {
|
||||
dsmReceiver.rfChannels[0] = dsmReceiver.rfChannel;
|
||||
} else {
|
||||
dsmReceiver.rfChannels[1] = dsmReceiver.rfChannel;
|
||||
}
|
||||
|
||||
if (dsmReceiver.rfChannels[0] != dsmReceiver.rfChannels[1]) {
|
||||
dsmReceiver.status = DSM_RECEIVER_RECV;
|
||||
dsmReceiver.missedPackets = 0;
|
||||
dsmReceiverSetNextChannel();
|
||||
resetReceiveTimeout(timeStamp);
|
||||
cyrf6936StartRecv();
|
||||
}
|
||||
}
|
||||
break;
|
||||
case DSM_RECEIVER_RECV:
|
||||
if (isValidPacket(packet)) {
|
||||
setRssi(403 + cyrf6936GetRssi() * 20, RSSI_SOURCE_RX_PROTOCOL);
|
||||
dsmReceiver.missedPackets = 0;
|
||||
DEBUG_SET(DEBUG_RX_SPEKTRUM_SPI, 0, dsmReceiver.missedPackets);
|
||||
memcpy(payload, &packet[2], 14);
|
||||
#ifdef USE_RX_SPEKTRUM_TELEMETRY
|
||||
if (dsmReceiver.sendTelemetry && dsmReceiver.crcSeed == ((dsmReceiver.mfgId[0] << 8) + dsmReceiver.mfgId[1])) {
|
||||
dsmSendTelemetryPacket();
|
||||
dsmReceiver.sendTelemetry = false;
|
||||
dsmReceiver.timeLastPacket = timeStamp;
|
||||
dsmReceiver.timeout = DSM_TELEMETRY_TIMEOUT_US;
|
||||
dsmReceiver.status = DSM_RECEIVER_TLM;
|
||||
} else {
|
||||
#endif
|
||||
dsmReceiverSetNextChannel();
|
||||
resetReceiveTimeout(timeStamp);
|
||||
cyrf6936StartRecv();
|
||||
#ifdef USE_RX_SPEKTRUM_TELEMETRY
|
||||
}
|
||||
#endif
|
||||
result = RX_SPI_RECEIVED_DATA;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
rx_spi_received_e spektrumSpiDataReceived(uint8_t *payload)
|
||||
{
|
||||
rx_spi_received_e result = RX_SPI_RECEIVED_NONE;
|
||||
uint32_t timeStamp;
|
||||
|
||||
if (rxSpiCheckBindRequested(true)) {
|
||||
dsmReceiver.bound = false;
|
||||
dsmReceiverStartBind();
|
||||
}
|
||||
|
||||
if (cyrf6936RxFinished(&timeStamp)) {
|
||||
result = spektrumReadPacket(payload, timeStamp);
|
||||
}
|
||||
|
||||
checkTimeout();
|
||||
|
||||
dsmReceiver.bound ? rxSpiLedBlinkRxLoss(result) : rxSpiLedBlinkBind();
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
#endif /* USE_RX_SPEKTRUM */
|
||||
|
|
@ -0,0 +1,53 @@
|
|||
/*
|
||||
* This file is part of Cleanflight and Betaflight.
|
||||
*
|
||||
* Cleanflight and Betaflight are free software. You can redistribute
|
||||
* this software and/or modify this software under the terms of the
|
||||
* GNU General Public License as published by the Free Software
|
||||
* Foundation, either version 3 of the License, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* Cleanflight and Betaflight are distributed in the hope that they
|
||||
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
|
||||
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||
* See the GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this software.
|
||||
*
|
||||
* If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#define DSM_BIND_TIMEOUT_US 10000
|
||||
#define DSM_SYNC_TIMEOUT_US 20000
|
||||
#define DSM_RECV_LONG_TIMEOUT_US 18010
|
||||
#define DSM_RECV_MID_TIMEOUT_US 7000
|
||||
#define DSM_RECV_SHORT_TIMEOUT_US 4010
|
||||
#define DSM_RECV_TIMEOUT_OFFSET_US 1000
|
||||
|
||||
#define DSM_MAX_MISSED_PACKETS 31
|
||||
|
||||
#define DSM_MAX_RF_CHANNEL 0x4F
|
||||
#define DSM_MAX_CHANNEL_COUNT 12
|
||||
#define DSM_INITIAL_BIND_CHANNEL 0x0D
|
||||
|
||||
#define DSM_TELEMETRY_FRAME_RPM 0x7E
|
||||
#define DSM_TELEMETRY_TIMEOUT_US 1500
|
||||
#define DSM_TELEMETRY_TIME_US 176000
|
||||
|
||||
typedef struct spektrumConfig_s {
|
||||
uint8_t protocol;
|
||||
uint8_t mfgId[4];
|
||||
uint8_t numChannels;
|
||||
} spektrumConfig_t;
|
||||
|
||||
PG_DECLARE(spektrumConfig_t, spektrumConfig);
|
||||
|
||||
bool spektrumSpiInit(const struct rxSpiConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig);
|
||||
void spektrumSpiSetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload);
|
||||
rx_spi_received_e spektrumSpiDataReceived(uint8_t *payload);
|
||||
|
|
@ -48,6 +48,7 @@
|
|||
#include "rx/nrf24_kn.h"
|
||||
#include "rx/flysky.h"
|
||||
#include "rx/cc2500_sfhss.h"
|
||||
#include "rx/cyrf6936_spektrum.h"
|
||||
|
||||
|
||||
uint16_t rxSpiRcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
||||
|
|
@ -152,6 +153,13 @@ STATIC_UNIT_TESTED bool rxSpiSetProtocol(rx_spi_protocol_e protocol)
|
|||
protocolDataReceived = sfhssSpiDataReceived;
|
||||
protocolSetRcDataFromPayload = sfhssSpiSetRcData;
|
||||
break;
|
||||
#endif
|
||||
#ifdef USE_RX_SPEKTRUM
|
||||
case RX_SPI_CYRF6936_DSM:
|
||||
protocolInit = spektrumSpiInit;
|
||||
protocolDataReceived = spektrumSpiDataReceived;
|
||||
protocolSetRcDataFromPayload = spektrumSpiSetRcDataFromPayload;
|
||||
break;
|
||||
#endif
|
||||
}
|
||||
return true;
|
||||
|
|
|
|||
|
|
@ -40,6 +40,7 @@ typedef enum {
|
|||
RX_SPI_A7105_FLYSKY_2A,
|
||||
RX_SPI_NRF24_KN,
|
||||
RX_SPI_SFHSS,
|
||||
RX_SPI_CYRF6936_DSM,
|
||||
RX_SPI_PROTOCOL_COUNT
|
||||
} rx_spi_protocol_e;
|
||||
|
||||
|
|
|
|||
|
|
@ -353,6 +353,13 @@ vtx_unittest_DEFINES := \
|
|||
USE_VTX_CONTROL= \
|
||||
USE_VTX_SMARTAUDIO=
|
||||
|
||||
rx_spi_spektrum_unittest_SRC := \
|
||||
$(USER_DIR)/rx/cyrf6936_spektrum.c
|
||||
|
||||
rx_spi_spektrum_unittest_DEFINES := \
|
||||
USE_RX_SPI \
|
||||
USE_RX_SPEKTRUM
|
||||
|
||||
# Please tweak the following variable definitions as needed by your
|
||||
# project, except GTEST_HEADERS, which you can use in your own targets
|
||||
# but shouldn't modify.
|
||||
|
|
|
|||
|
|
@ -0,0 +1,391 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* Cleanflight is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
#include <limits.h>
|
||||
|
||||
extern "C" {
|
||||
#include "platform.h"
|
||||
|
||||
#include "pg/pg.h"
|
||||
#include "pg/pg_ids.h"
|
||||
#include "pg/rx_spi.h"
|
||||
|
||||
#include "rx/rx_spi.h"
|
||||
#include "rx/cyrf6936_spektrum.h"
|
||||
|
||||
typedef enum {
|
||||
DSM2_22 = 0x01,
|
||||
DSM2_11 = 0x02,
|
||||
DSM2_11_DX8 = 0x12,
|
||||
DSMX_22 = 0xA2,
|
||||
DSMX_11 = 0xB2,
|
||||
} dsm_protocol_e;
|
||||
|
||||
#define IS_DSM2(x) (x == DSM2_22 || x == DSM2_11 || x == DSM2_11_DX8)
|
||||
#define IS_DSMX(x) (!IS_DSM2(x))
|
||||
|
||||
typedef enum {
|
||||
DSM_RECEIVER_BIND = 0x0,
|
||||
DSM_RECEIVER_SYNC_A = 0x1,
|
||||
DSM_RECEIVER_SYNC_B = 0x2,
|
||||
DSM_RECEIVER_RECV = 0x3,
|
||||
#ifdef USE_RX_SPEKTRUM_TELEMETRY
|
||||
DSM_RECEIVER_TLM = 0x4,
|
||||
#endif
|
||||
} dsm_receiver_status_e;
|
||||
|
||||
typedef struct dsmReceiver_s {
|
||||
dsm_receiver_status_e status;
|
||||
dsm_protocol_e protocol;
|
||||
|
||||
uint8_t mfgId[4];
|
||||
|
||||
uint8_t rfChannel;
|
||||
uint8_t rfChannelIdx;
|
||||
uint8_t rfChannels[23];
|
||||
|
||||
uint8_t sopCol;
|
||||
uint8_t dataCol;
|
||||
uint16_t crcSeed;
|
||||
|
||||
uint8_t missedPackets;
|
||||
uint8_t numChannels;
|
||||
|
||||
bool bound;
|
||||
|
||||
uint32_t timeout;
|
||||
uint32_t timeLastPacket;
|
||||
|
||||
#ifdef USE_RX_SPEKTRUM_TELEMETRY
|
||||
uint32_t timeLastTelemetry;
|
||||
bool sendTelemetry;
|
||||
#endif
|
||||
} dsmReceiver_t;
|
||||
|
||||
extern dsmReceiver_t dsmReceiver;
|
||||
extern bool isError = false;
|
||||
|
||||
static const dsmReceiver_t empty = dsmReceiver_t();
|
||||
static rxRuntimeConfig_t config = rxRuntimeConfig_t();
|
||||
static uint8_t packetLen;
|
||||
static uint8_t packet[16];
|
||||
static uint16_t rssi = 0;
|
||||
static uint8_t cyrfRssi;
|
||||
|
||||
/* DeviationTx code */
|
||||
#define CHAN_MAX_VALUE 10000
|
||||
#define CHAN_MIN_VALUE -10000
|
||||
static void buildDataPacket(bool upper, const int16_t *sticks, uint8_t *pkt)
|
||||
{
|
||||
const uint8_t chMap7[] = {1, 5, 2, 3, 0, 4, 6};
|
||||
const uint8_t chMap12[] = {1, 5, 2, 4, 6, 10, 0xff, 0, 7, 3, 8, 9, 11, 0xff};
|
||||
const uint8_t chMap10[] = {1, 5, 2, 3, 4, 6, 8, 1, 5, 2, 3, 0, 7, 9};
|
||||
const uint8_t *chMap;
|
||||
|
||||
uint8_t bits = IS_DSMX(dsmReceiver.protocol) ? 11 : 10;
|
||||
if (dsmReceiver.numChannels < 8) {
|
||||
chMap = chMap7;
|
||||
} else if (dsmReceiver.numChannels < 11) {
|
||||
chMap = chMap10;
|
||||
} else {
|
||||
chMap = chMap12;
|
||||
}
|
||||
uint16_t max = 1 << bits;
|
||||
uint16_t pct100 = (uint32_t) max * 100 / 150;
|
||||
for (uint32_t i = 0; i < 7; i++) {
|
||||
uint8_t idx = chMap[upper * 7 + i];
|
||||
int32_t value;
|
||||
if ((chMap[upper * 7 + i] == 0xff) || ((dsmReceiver.numChannels > 7) && (chMap[upper * 7 + i] > dsmReceiver.numChannels - 1))) {
|
||||
value = 0xffff;
|
||||
} else {
|
||||
value = (int32_t) sticks[idx] * (pct100 / 2) / CHAN_MAX_VALUE + (max / 2);
|
||||
if (value >= max) {
|
||||
value = max - 1;
|
||||
} else if (value < 0) {
|
||||
value = 0;
|
||||
}
|
||||
value = (upper && i == 0 ? 0x8000 : 0) | (chMap[upper * 7 + i] << bits) | value;
|
||||
}
|
||||
pkt[i * 2] = (value >> 8) & 0xff;
|
||||
pkt[i * 2 + 1] = (value >> 0) & 0xff;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#include "unittest_macros.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
//make clean test_rx_spi_spektrum_unittest
|
||||
TEST(RxSpiSpektrumUnitTest, TestInitUnbound)
|
||||
{
|
||||
dsmReceiver = empty;
|
||||
spektrumSpiInit(nullptr, &config);
|
||||
EXPECT_FALSE(dsmReceiver.bound);
|
||||
EXPECT_EQ(DSM_RECEIVER_BIND, dsmReceiver.status);
|
||||
EXPECT_EQ(DSM_INITIAL_BIND_CHANNEL, dsmReceiver.rfChannel);
|
||||
}
|
||||
|
||||
TEST(RxSpiSpektrumUnitTest, TestInitBound)
|
||||
{
|
||||
const uint8_t validMfgId[4] = {0xd4, 0x62, 0xd6, 0xad};
|
||||
const uint16_t validCrcSeed = (uint16_t) ~((validMfgId[0] << 8) + validMfgId[1]);
|
||||
const uint16_t changedSeed = ~validCrcSeed;
|
||||
|
||||
dsmReceiver = empty;
|
||||
memcpy(spektrumConfigMutable()->mfgId, validMfgId, 4);
|
||||
spektrumConfigMutable()->numChannels = 7;
|
||||
|
||||
spektrumConfigMutable()->protocol = DSMX_11;
|
||||
|
||||
bool result = spektrumSpiInit(nullptr, &config);
|
||||
|
||||
EXPECT_TRUE(result);
|
||||
EXPECT_TRUE(dsmReceiver.bound);
|
||||
for (int i = 0; i < 4; i++) {
|
||||
EXPECT_EQ(validMfgId[i], dsmReceiver.mfgId[i]);
|
||||
}
|
||||
EXPECT_EQ(7, dsmReceiver.numChannels);
|
||||
EXPECT_EQ(DSMX_11, dsmReceiver.protocol);
|
||||
EXPECT_EQ(DSM_RECEIVER_SYNC_A, dsmReceiver.status);
|
||||
EXPECT_EQ(changedSeed, dsmReceiver.crcSeed);
|
||||
EXPECT_EQ(6, dsmReceiver.sopCol);
|
||||
EXPECT_EQ(1, dsmReceiver.dataCol);
|
||||
EXPECT_EQ(0, dsmReceiver.rfChannelIdx);
|
||||
EXPECT_TRUE(dsmReceiver.rfChannels[22] != 0);
|
||||
EXPECT_EQ(dsmReceiver.rfChannels[dsmReceiver.rfChannelIdx], dsmReceiver.rfChannel);
|
||||
|
||||
dsmReceiver = empty;
|
||||
spektrumConfigMutable()->protocol = DSM2_11;
|
||||
|
||||
spektrumSpiInit(nullptr, &config);
|
||||
|
||||
EXPECT_TRUE(dsmReceiver.bound);
|
||||
EXPECT_EQ(DSM2_11, dsmReceiver.protocol);
|
||||
EXPECT_TRUE(dsmReceiver.rfChannels[22] == 0); //uninitialized for dsm2
|
||||
EXPECT_EQ(1, dsmReceiver.rfChannel);
|
||||
}
|
||||
|
||||
TEST(RxSpiSpektrumUnitTest, TestDecodeBindPacket)
|
||||
{
|
||||
const uint8_t bindPacket[] = {0xD7, 0xA1, 0x54, 0xB1, 0xD7, 0xA1, 0x54, 0xB1, 0x06, 0x6A, 0x01, 7, DSMX_22, 0x00, 0x07, 0x84};
|
||||
packetLen = sizeof(packet);
|
||||
memcpy(packet, bindPacket, packetLen);
|
||||
dsmReceiver = empty;
|
||||
dsmReceiver.status = DSM_RECEIVER_BIND;
|
||||
|
||||
rx_spi_received_e result = spektrumSpiDataReceived(nullptr);
|
||||
EXPECT_EQ(RX_SPI_RECEIVED_BIND, result);
|
||||
EXPECT_EQ(7, dsmReceiver.numChannels);
|
||||
EXPECT_EQ(DSMX_22, dsmReceiver.protocol);
|
||||
EXPECT_EQ(0x4E, dsmReceiver.mfgId[3]);
|
||||
EXPECT_EQ(spektrumConfig()->numChannels, dsmReceiver.numChannels);
|
||||
EXPECT_EQ(spektrumConfig()->protocol, dsmReceiver.protocol);
|
||||
for (int i = 0; i < 4; i++) {
|
||||
EXPECT_EQ(spektrumConfig()->mfgId[i], dsmReceiver.mfgId[i]);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(RxSpiSpektrumUnitTest, TestReceiverSyncLogic)
|
||||
{
|
||||
const uint8_t mfgId[4] = {0xAA, 0xBB, 0xCC, 0xDD};
|
||||
packetLen = 2;
|
||||
|
||||
//DSMX SYNCA -> RECV
|
||||
dsmReceiver = empty;
|
||||
memcpy(dsmReceiver.mfgId, mfgId, 4);
|
||||
dsmReceiver.status = DSM_RECEIVER_SYNC_A;
|
||||
dsmReceiver.protocol = DSMX_11;
|
||||
dsmReceiver.crcSeed = 12345;
|
||||
dsmReceiver.missedPackets = 1;
|
||||
dsmReceiver.rfChannel = 10;
|
||||
dsmReceiver.rfChannelIdx = 1;
|
||||
dsmReceiver.rfChannels[2] = 5;
|
||||
packet[0] = mfgId[2];
|
||||
packet[1] = mfgId[3];
|
||||
|
||||
spektrumSpiDataReceived(nullptr);
|
||||
EXPECT_EQ(DSM_RECEIVER_RECV, dsmReceiver.status);
|
||||
EXPECT_EQ(53190, dsmReceiver.crcSeed);
|
||||
EXPECT_EQ(0, dsmReceiver.missedPackets);
|
||||
EXPECT_EQ(2, dsmReceiver.rfChannelIdx);
|
||||
EXPECT_EQ(5, dsmReceiver.rfChannel);
|
||||
|
||||
//DSM2 SYNCA -> SYNCB
|
||||
dsmReceiver = empty;
|
||||
memcpy(dsmReceiver.mfgId, mfgId, 4);
|
||||
dsmReceiver.status = DSM_RECEIVER_SYNC_A;
|
||||
dsmReceiver.protocol = DSM2_22;
|
||||
dsmReceiver.crcSeed = 12345;
|
||||
dsmReceiver.missedPackets = 1;
|
||||
dsmReceiver.rfChannel = 10;
|
||||
dsmReceiver.rfChannelIdx = 1;
|
||||
packet[0] = (~mfgId[2]&0xFF);
|
||||
packet[1] = (~mfgId[3]&0xFF);
|
||||
|
||||
spektrumSpiDataReceived(nullptr);
|
||||
EXPECT_EQ(DSM_RECEIVER_SYNC_B, dsmReceiver.status);
|
||||
EXPECT_EQ(53190, dsmReceiver.crcSeed);
|
||||
EXPECT_EQ(dsmReceiver.rfChannel, dsmReceiver.rfChannels[0] + 1);
|
||||
EXPECT_EQ(dsmReceiver.rfChannel, dsmReceiver.rfChannels[1] + 1);
|
||||
EXPECT_EQ(1, dsmReceiver.missedPackets);
|
||||
EXPECT_EQ(1, dsmReceiver.rfChannelIdx);
|
||||
|
||||
//DSM2 SYNCB -> RECV
|
||||
dsmReceiver.rfChannel = dsmReceiver.rfChannel+1;
|
||||
spektrumSpiDataReceived((uint8_t *) packet);
|
||||
EXPECT_EQ(DSM_RECEIVER_RECV, dsmReceiver.status);
|
||||
EXPECT_EQ(12345, dsmReceiver.crcSeed);
|
||||
EXPECT_EQ(0, dsmReceiver.missedPackets);
|
||||
EXPECT_EQ(0, dsmReceiver.rfChannelIdx);
|
||||
EXPECT_EQ(12, dsmReceiver.rfChannel);
|
||||
}
|
||||
|
||||
TEST(RxSpiSpektrumUnitTest, TestReceiveData)
|
||||
{
|
||||
const uint8_t mfgId[4] = {0xAA, 0xBB, 0xCC, 0xDD};
|
||||
cyrfRssi = 31;
|
||||
packetLen = 16;
|
||||
dsmReceiver = empty;
|
||||
memcpy(dsmReceiver.mfgId, mfgId, 4);
|
||||
dsmReceiver.status = DSM_RECEIVER_RECV;
|
||||
dsmReceiver.protocol = DSMX_11;
|
||||
dsmReceiver.crcSeed = 12345;
|
||||
dsmReceiver.numChannels = 14;
|
||||
dsmReceiver.missedPackets = 1;
|
||||
dsmReceiver.rfChannelIdx = 22;
|
||||
dsmReceiver.rfChannels[0] = 5;
|
||||
const uint8_t dataPacket[16] = {mfgId[2], mfgId[3], 0x01, 0x12, 0x23, 0x34, 0x45, 0x56, 0x67, 0x78, 0x89, 0x9A, 0xAB, 0xBC, 0xCD, 0xDE};
|
||||
memcpy(packet, dataPacket, packetLen);
|
||||
|
||||
uint8_t data[16];
|
||||
rx_spi_received_e result = spektrumSpiDataReceived((uint8_t *) data);
|
||||
EXPECT_EQ(RX_SPI_RECEIVED_DATA, result);
|
||||
EXPECT_EQ(0, dsmReceiver.missedPackets);
|
||||
EXPECT_EQ(0, dsmReceiver.rfChannelIdx);
|
||||
EXPECT_EQ(5, dsmReceiver.rfChannel);
|
||||
EXPECT_EQ(53190, dsmReceiver.crcSeed);
|
||||
EXPECT_EQ(1023, rssi);
|
||||
for (int i = 0; i < 14; i++) {
|
||||
EXPECT_EQ(packet[i+2], data[i]);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(RxSpiSpektrumUnitTest, TestConvertDataPacket)
|
||||
{
|
||||
dsmReceiver = empty;
|
||||
|
||||
const int16_t sticks[12] = {CHAN_MAX_VALUE, CHAN_MIN_VALUE, 0, -7500, 7500, -5000, 5000, -2500, 2500, 6666, -3333, 250};
|
||||
const uint16_t bfSticks[12] = {1841, 1159, 1500, 1244, 1755, 1329, 1670, 1415, 1585, 1727, 1386, 1508};
|
||||
const uint8_t testNumChan[3] = {7, 10, 12};
|
||||
|
||||
uint8_t dataPacket[16];
|
||||
uint16_t rcData[16];
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
dsmReceiver.numChannels = testNumChan[i];
|
||||
memset(rcData, 0, sizeof(rcData));
|
||||
memset(dataPacket, 0, sizeof(dataPacket));
|
||||
|
||||
if (dsmReceiver.numChannels > 7) { //we need two packets to update all channels
|
||||
buildDataPacket(false, sticks, dataPacket);
|
||||
spektrumSpiSetRcDataFromPayload((uint16_t *) rcData, (uint8_t *) dataPacket);
|
||||
memset(dataPacket, 0, sizeof(dataPacket));
|
||||
buildDataPacket(true, sticks, dataPacket);
|
||||
spektrumSpiSetRcDataFromPayload((uint16_t *) rcData, (uint8_t *) dataPacket);
|
||||
} else { // we need only one packet
|
||||
buildDataPacket(false, sticks, dataPacket);
|
||||
spektrumSpiSetRcDataFromPayload((uint16_t *) rcData, (uint8_t *) dataPacket);
|
||||
}
|
||||
for (int k = 0; k < 16; k++) {
|
||||
if (k<dsmReceiver.numChannels) {
|
||||
EXPECT_NEAR(bfSticks[k], rcData[k], 1); //conversion error +-1
|
||||
} else {
|
||||
EXPECT_EQ(0, rcData[k]);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// STUBS
|
||||
|
||||
extern "C" {
|
||||
|
||||
int16_t *debug;
|
||||
uint8_t debugMode;
|
||||
|
||||
rssiSource_e rssiSource;
|
||||
void setRssi(uint16_t newRssi, rssiSource_e )
|
||||
{
|
||||
rssi = newRssi;
|
||||
}
|
||||
void setRssiDirect(uint16_t , rssiSource_e ) {}
|
||||
|
||||
uint32_t micros(void) { return 0; }
|
||||
uint32_t millis(void) { return 0; }
|
||||
|
||||
void IOConfigGPIO(void) {}
|
||||
bool IORead(IO_t ) { return true; }
|
||||
void IOInit(void) {}
|
||||
IO_t IOGetByTag(ioTag_t ) { return IO_NONE; }
|
||||
void IOHi(IO_t ) {}
|
||||
void IOLo(IO_t ) {}
|
||||
void writeEEPROM(void) {}
|
||||
|
||||
bool cyrf6936Init(void) { return true; }
|
||||
bool cyrf6936RxFinished (uint32_t *timestamp)
|
||||
{
|
||||
*timestamp = 0;
|
||||
return true;
|
||||
}
|
||||
void cyrf6936WriteRegister(const uint8_t , const uint8_t ) {}
|
||||
uint8_t cyrf6936ReadRegister(const uint8_t reg)
|
||||
{
|
||||
if (reg == 0x09) {//CYRF6936_RX_COUNT
|
||||
return packetLen;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
uint8_t cyrf6936GetRxStatus(void) { return 0; }
|
||||
void cyrf6936SetConfigLen(const uint8_t , const uint8_t ) {}
|
||||
void cyrf6936SetChannel(const uint8_t ) {}
|
||||
void cyrf6936SetMode(const uint8_t , const bool ) {}
|
||||
void cyrf6936SetCrcSeed(const uint16_t ) {}
|
||||
void cyrf6936SetSopCode(const uint8_t ) {}
|
||||
void cyrf6936SetDataCode(const uint8_t ) {}
|
||||
void cyrf6936StartRecv(void) {}
|
||||
void cyrf6936RecvLen(uint8_t *data, const uint8_t length)
|
||||
{
|
||||
if (length == packetLen) {
|
||||
memcpy(data, packet, packetLen);
|
||||
}
|
||||
}
|
||||
uint8_t cyrf6936GetRssi(void)
|
||||
{
|
||||
return cyrfRssi;
|
||||
}
|
||||
|
||||
void rxSpiCommonIOInit(const rxSpiConfig_t *) {}
|
||||
void rxSpiLedBlinkRxLoss(rx_spi_received_e ) {}
|
||||
void rxSpiLedBlinkBind(void) {};
|
||||
bool rxSpiCheckBindRequested(bool )
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue