mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-23 08:15:30 +03:00
Merge pull request #7210 from phobos-/spi_dsm
Implemented SPI SPEKTRUM protocol
This commit is contained in:
Michael Keller
GitHub
commit
ee0d86ff20
13 changed files with 1492 additions and 3 deletions
|
|
@ -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.
|
||||
|
|
|
|||
391
src/test/unit/rx_spi_spektrum_unittest.cc
Normal file
391
src/test/unit/rx_spi_spektrum_unittest.cc
Normal file
|
|
@ -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…
Add table
Add a link
Reference in a new issue