src/test/unit/rx_spi_spektrum_unittest.cc

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdint.h>
  19 #include <stdbool.h>
  20
  21 #include <limits.h>
  22
  23 extern "C" {
  24     #include "platform.h"
  25
  26     #include "drivers/io.h"
  27
  28     #include "pg/pg.h"
  29     #include "pg/pg_ids.h"
  30     #include "pg/rx_spi.h"
  31
  32     #include "rx/rx_spi.h"
  33     #include "rx/cyrf6936_spektrum.h"
  34
  35     typedef enum {
  36         DSM2_22     = 0x01,
  37         DSM2_11     = 0x02,
  38         DSM2_11_DX8 = 0x12,
  39         DSMX_22     = 0xA2,
  40         DSMX_11     = 0xB2,
  41     } dsm_protocol_e;
  42
  43     #define IS_DSM2(x) (x == DSM2_22 || x == DSM2_11 || x == DSM2_11_DX8)
  44     #define IS_DSMX(x) (!IS_DSM2(x))
  45
  46     typedef enum {
  47         DSM_RECEIVER_BIND = 0,
  48         DSM_RECEIVER_BIND2,
  49         DSM_RECEIVER_SYNC_A,
  50         DSM_RECEIVER_SYNC_B,
  51         DSM_RECEIVER_RECV,
  52     #ifdef USE_RX_SPEKTRUM_TELEMETRY
  53         DSM_RECEIVER_TLM,
  54     #endif
  55     } dsm_receiver_status_e;
  56
  57     typedef struct dsmReceiver_s {
  58         dsm_receiver_status_e status;
  59         dsm_protocol_e protocol;
  60
  61         uint8_t mfgId[4];
  62
  63         uint8_t rfChannel;
  64         uint8_t rfChannelIdx;
  65         uint8_t rfChannels[23];
  66
  67         uint8_t sopCol;
  68         uint8_t dataCol;
  69         uint16_t crcSeed;
  70
  71         uint8_t missedPackets;
  72         uint8_t numChannels;
  73
  74         bool bound;
  75
  76         uint32_t timeout;
  77         uint32_t timeLastPacket;
  78
  79         uint16_t bindPackets;
  80
  81     #ifdef USE_RX_SPEKTRUM_TELEMETRY
  82         uint32_t timeLastTelemetry;
  83         bool sendTelemetry;
  84     #endif
  85     } dsmReceiver_t;
  86
  87     extern dsmReceiver_t dsmReceiver;
  88     extern bool isError = false;
  89
  90     static const dsmReceiver_t empty = dsmReceiver_t();
  91     static rxRuntimeState_t config = rxRuntimeState_t();
  92     static rxSpiExtiConfig_t extiConfig;
  93     static uint8_t packetLen;
  94     static uint8_t packet[16];
  95     static uint16_t rssi = 0;
  96     static uint8_t cyrfRssi;
  97
  98     /* DeviationTx code */
  99     #define CHAN_MAX_VALUE 10000
 100     #define CHAN_MIN_VALUE -10000
 101     static void buildDataPacket(bool upper, const int16_t *sticks, uint8_t *pkt)
 102     {
 103         const uint8_t chMap7[] = {1, 5, 2, 3, 0, 4, 6};
 104         const uint8_t chMap12[] = {1, 5, 2, 4, 6, 10, 0xff, 0, 7, 3, 8, 9, 11, 0xff};
 105         const uint8_t chMap10[] = {1, 5, 2, 3, 4, 6, 8, 1, 5, 2, 3, 0, 7, 9};
 106         const uint8_t *chMap;
 107
 108         uint8_t bits = IS_DSMX(dsmReceiver.protocol) ? 11 : 10;
 109         if (dsmReceiver.numChannels < 8) {
 110             chMap = chMap7;
 111         } else if (dsmReceiver.numChannels < 11) {
 112             chMap = chMap10;
 113         } else {
 114             chMap = chMap12;
 115         }
 116         uint16_t max = 1 << bits;
 117         uint16_t pct100 = (uint32_t) max * 100 / 150;
 118         for (uint32_t i = 0; i < 7; i++) {
 119             uint8_t idx = chMap[upper * 7 + i];
 120             int32_t value;
 121             if ((chMap[upper * 7 + i] == 0xff) || ((dsmReceiver.numChannels > 7) && (chMap[upper * 7 + i] > dsmReceiver.numChannels - 1))) {
 122                 value = 0xffff;
 123             } else {
 124                 value = (int32_t) sticks[idx] * (pct100 / 2) / CHAN_MAX_VALUE + (max / 2);
 125                 if (value >= max) {
 126                     value = max - 1;
 127                 } else if (value < 0) {
 128                     value = 0;
 129                 }
 130                 value = (upper && i == 0 ? 0x8000 : 0) | (chMap[upper * 7 + i] << bits) | value;
 131             }
 132             pkt[i * 2] = (value >> 8) & 0xff;
 133             pkt[i * 2 + 1] = (value >> 0) & 0xff;
 134         }
 135     }
 136
 137     static const rxSpiConfig_t injectedConfig = {
 138         .extiIoTag = IO_TAG(PA0),
 139     };
 140 }
 141
 142 #include "unittest_macros.h"
 143 #include "gtest/gtest.h"
 144
 145 //make clean test_rx_spi_spektrum_unittest
 146 TEST(RxSpiSpektrumUnitTest, TestInitUnbound)
 147 {
 148     dsmReceiver = empty;
 149     spektrumSpiInit(&injectedConfig, &config, &extiConfig);
 150     EXPECT_FALSE(dsmReceiver.bound);
 151     EXPECT_EQ(DSM_RECEIVER_BIND, dsmReceiver.status);
 152     EXPECT_EQ(DSM_INITIAL_BIND_CHANNEL, dsmReceiver.rfChannel);
 153 }
 154
 155 TEST(RxSpiSpektrumUnitTest, TestInitBound)
 156 {
 157     const uint8_t validMfgId[4] = {0xd4, 0x62, 0xd6, 0xad};
 158     const uint16_t validCrcSeed = (uint16_t) ~((validMfgId[0] << 8) + validMfgId[1]);
 159     const uint16_t changedSeed = ~validCrcSeed;
 160
 161     dsmReceiver = empty;
 162     memcpy(spektrumConfigMutable()->mfgId, validMfgId, 4);
 163     spektrumConfigMutable()->numChannels = 7;
 164
 165     spektrumConfigMutable()->protocol = DSMX_11;
 166
 167     bool result = spektrumSpiInit(&injectedConfig, &config, &extiConfig);
 168
 169     EXPECT_TRUE(result);
 170     EXPECT_TRUE(dsmReceiver.bound);
 171     for (int i = 0; i < 4; i++) {
 172         EXPECT_EQ(validMfgId[i], dsmReceiver.mfgId[i]);
 173     }
 174     EXPECT_EQ(7, dsmReceiver.numChannels);
 175     EXPECT_EQ(DSMX_11, dsmReceiver.protocol);
 176     EXPECT_EQ(DSM_RECEIVER_SYNC_A, dsmReceiver.status);
 177     EXPECT_EQ(changedSeed, dsmReceiver.crcSeed);
 178     EXPECT_EQ(6, dsmReceiver.sopCol);
 179     EXPECT_EQ(1, dsmReceiver.dataCol);
 180     EXPECT_EQ(0, dsmReceiver.rfChannelIdx);
 181     EXPECT_TRUE(dsmReceiver.rfChannels[22] != 0);
 182     EXPECT_EQ(dsmReceiver.rfChannels[dsmReceiver.rfChannelIdx], dsmReceiver.rfChannel);
 183
 184     dsmReceiver = empty;
 185     spektrumConfigMutable()->protocol = DSM2_11;
 186
 187     spektrumSpiInit(&injectedConfig, &config, &extiConfig);
 188
 189     EXPECT_TRUE(dsmReceiver.bound);
 190     EXPECT_EQ(DSM2_11, dsmReceiver.protocol);
 191     EXPECT_TRUE(dsmReceiver.rfChannels[22] == 0); //uninitialized for dsm2
 192     EXPECT_EQ(1, dsmReceiver.rfChannel);
 193 }
 194
 195 TEST(RxSpiSpektrumUnitTest, TestDecodeBindPacket)
 196 {
 197     const uint8_t bindPacket[] = {0xD7, 0xA1, 0x54, 0xB1, 0xD7, 0xA1, 0x54, 0xB1, 0x06, 0x6A, 0x01, 7, DSMX_22, 0x00, 0x07, 0x84};
 198     packetLen = sizeof(packet);
 199     memcpy(packet, bindPacket, packetLen);
 200     dsmReceiver = empty;
 201     dsmReceiver.status = DSM_RECEIVER_BIND;
 202
 203     rx_spi_received_e result = spektrumSpiDataReceived(nullptr);
 204     EXPECT_EQ(RX_SPI_RECEIVED_BIND, result);
 205     EXPECT_EQ(7, dsmReceiver.numChannels);
 206     EXPECT_EQ(DSMX_22, dsmReceiver.protocol);
 207     EXPECT_EQ(0x4E, dsmReceiver.mfgId[3]);
 208     EXPECT_EQ(spektrumConfig()->numChannels, dsmReceiver.numChannels);
 209     EXPECT_EQ(spektrumConfig()->protocol, dsmReceiver.protocol);
 210     for (int i = 0; i < 4; i++) {
 211         EXPECT_EQ(spektrumConfig()->mfgId[i], dsmReceiver.mfgId[i]);
 212     }
 213 }
 214
 215 TEST(RxSpiSpektrumUnitTest, TestReceiverSyncLogic)
 216 {
 217     const uint8_t mfgId[4] = {0xAA, 0xBB, 0xCC, 0xDD};
 218     packetLen = 2;
 219
 220     //DSMX SYNCA -> RECV
 221     dsmReceiver = empty;
 222     memcpy(dsmReceiver.mfgId, mfgId, 4);
 223     dsmReceiver.status = DSM_RECEIVER_SYNC_A;
 224     dsmReceiver.protocol = DSMX_11;
 225     dsmReceiver.crcSeed = 12345;
 226     dsmReceiver.missedPackets = 1;
 227     dsmReceiver.rfChannel = 10;
 228     dsmReceiver.rfChannelIdx = 1;
 229     dsmReceiver.rfChannels[2] = 5;
 230     packet[0] = mfgId[2];
 231     packet[1] = mfgId[3];
 232
 233     spektrumSpiDataReceived(nullptr);
 234     EXPECT_EQ(DSM_RECEIVER_RECV, dsmReceiver.status);
 235     EXPECT_EQ(53190, dsmReceiver.crcSeed);
 236     EXPECT_EQ(0, dsmReceiver.missedPackets);
 237     EXPECT_EQ(2, dsmReceiver.rfChannelIdx);
 238     EXPECT_EQ(5, dsmReceiver.rfChannel);
 239
 240     //DSM2 SYNCA -> SYNCB
 241     dsmReceiver = empty;
 242     memcpy(dsmReceiver.mfgId, mfgId, 4);
 243     dsmReceiver.status = DSM_RECEIVER_SYNC_A;
 244     dsmReceiver.protocol = DSM2_22;
 245     dsmReceiver.crcSeed = 12345;
 246     dsmReceiver.missedPackets = 1;
 247     dsmReceiver.rfChannel = 10;
 248     dsmReceiver.rfChannelIdx = 1;
 249     packet[0] = (~mfgId[2]&0xFF);
 250     packet[1] = (~mfgId[3]&0xFF);
 251
 252     spektrumSpiDataReceived(nullptr);
 253     EXPECT_EQ(DSM_RECEIVER_SYNC_B, dsmReceiver.status);
 254     EXPECT_EQ(53190, dsmReceiver.crcSeed);
 255     EXPECT_EQ(dsmReceiver.rfChannel, dsmReceiver.rfChannels[0] + 1);
 256     EXPECT_EQ(dsmReceiver.rfChannel, dsmReceiver.rfChannels[1] + 1);
 257     EXPECT_EQ(1, dsmReceiver.missedPackets);
 258     EXPECT_EQ(1, dsmReceiver.rfChannelIdx);
 259
 260     //DSM2 SYNCB -> RECV
 261     dsmReceiver.rfChannel = dsmReceiver.rfChannel+1;
 262     spektrumSpiDataReceived((uint8_t *) packet);
 263     EXPECT_EQ(DSM_RECEIVER_RECV, dsmReceiver.status);
 264     EXPECT_EQ(12345, dsmReceiver.crcSeed);
 265     EXPECT_EQ(0, dsmReceiver.missedPackets);
 266     EXPECT_EQ(0, dsmReceiver.rfChannelIdx);
 267     EXPECT_EQ(12, dsmReceiver.rfChannel);
 268 }
 269
 270 TEST(RxSpiSpektrumUnitTest, TestReceiveData)
 271 {
 272     const uint8_t mfgId[4] = {0xAA, 0xBB, 0xCC, 0xDD};
 273     cyrfRssi = 31;
 274     packetLen = 16;
 275     dsmReceiver = empty;
 276     memcpy(dsmReceiver.mfgId, mfgId, 4);
 277     dsmReceiver.status = DSM_RECEIVER_RECV;
 278     dsmReceiver.protocol = DSMX_11;
 279     dsmReceiver.crcSeed = 12345;
 280     dsmReceiver.numChannels = 14;
 281     dsmReceiver.missedPackets = 1;
 282     dsmReceiver.rfChannelIdx = 22;
 283     dsmReceiver.rfChannels[0] = 5;
 284     const uint8_t dataPacket[16] = {mfgId[2], mfgId[3], 0x01, 0x12, 0x23, 0x34, 0x45, 0x56, 0x67, 0x78, 0x89, 0x9A, 0xAB, 0xBC, 0xCD, 0xDE};
 285     memcpy(packet, dataPacket, packetLen);
 286
 287     uint8_t data[16];
 288     rx_spi_received_e result = spektrumSpiDataReceived((uint8_t *) data);
 289     EXPECT_EQ(RX_SPI_RECEIVED_DATA, result);
 290     EXPECT_EQ(0, dsmReceiver.missedPackets);
 291     EXPECT_EQ(0, dsmReceiver.rfChannelIdx);
 292     EXPECT_EQ(5, dsmReceiver.rfChannel);
 293     EXPECT_EQ(53190, dsmReceiver.crcSeed);
 294     EXPECT_EQ(1023, rssi);
 295     for (int i = 0; i < 14; i++) {
 296         EXPECT_EQ(packet[i+2], data[i]);
 297     }
 298 }
 299
 300 TEST(RxSpiSpektrumUnitTest, TestConvertDataPacket)
 301 {
 302     dsmReceiver = empty;
 303
 304     const int16_t sticks[12] = {CHAN_MAX_VALUE, CHAN_MIN_VALUE, 0, -7500, 7500, -5000, 5000, -2500, 2500, 6666, -3333, 250};
 305     const uint16_t bfSticks[12] = {1841, 1159, 1500, 1244, 1755, 1329, 1670, 1415, 1585, 1727, 1386, 1508};
 306     const uint8_t testNumChan[3] = {7, 10, 12};
 307
 308     uint8_t dataPacket[16];
 309     uint16_t rcData[16];
 310
 311     for (int i = 0; i < 3; i++) {
 312         dsmReceiver.numChannels = testNumChan[i];
 313         memset(rcData, 0, sizeof(rcData));
 314         memset(dataPacket, 0, sizeof(dataPacket));
 315
 316         if (dsmReceiver.numChannels > 7) { //we need two packets to update all channels
 317             buildDataPacket(false, sticks, dataPacket);
 318             spektrumSpiSetRcDataFromPayload((uint16_t *) rcData, (uint8_t *) dataPacket);
 319             memset(dataPacket, 0, sizeof(dataPacket));
 320             buildDataPacket(true, sticks, dataPacket);
 321             spektrumSpiSetRcDataFromPayload((uint16_t *) rcData, (uint8_t *) dataPacket);
 322         } else { // we need only one packet
 323             buildDataPacket(false, sticks, dataPacket);
 324             spektrumSpiSetRcDataFromPayload((uint16_t *) rcData, (uint8_t *) dataPacket);
 325         }
 326         for (int k = 0; k < 16; k++) {
 327             if (k<dsmReceiver.numChannels) {
 328                 EXPECT_NEAR(bfSticks[k], rcData[k], 1); //conversion error +-1
 329             } else {
 330                 EXPECT_EQ(0, rcData[k]);
 331             }
 332         }
 333     }
 334 }
 335
 336 // STUBS
 337
 338 extern "C" {
 339
 340     int16_t *debug;
 341     uint8_t debugMode;
 342
 343     rssiSource_e rssiSource;
 344     void setRssi(uint16_t newRssi, rssiSource_e )
 345     {
 346         rssi = newRssi;
 347     }
 348     void setRssiDirect(uint16_t , rssiSource_e ) {}
 349
 350     uint32_t micros(void) { return 0; }
 351     uint32_t millis(void) { return 0; }
 352
 353     bool IORead(IO_t ) { return true; }
 354     IO_t IOGetByTag(ioTag_t ) { return (IO_t)1; }
 355     void IOHi(IO_t ) {}
 356     void IOLo(IO_t ) {}
 357     void writeEEPROM(void) {}
 358
 359     bool cyrf6936Init(IO_t ) { return true; }
 360     bool cyrf6936RxFinished (uint32_t *timestamp)
 361     {
 362         *timestamp = 0;
 363         return true;
 364     }
 365     void cyrf6936WriteRegister(const uint8_t , const uint8_t ) {}
 366     uint8_t cyrf6936ReadRegister(const uint8_t reg)
 367     {
 368         if (reg == 0x09) {//CYRF6936_RX_COUNT
 369             return packetLen;
 370         }
 371         return 0;
 372     }
 373     uint8_t cyrf6936GetRxStatus(void) { return 0; }
 374     void cyrf6936SetConfigLen(const uint8_t , const uint8_t ) {}
 375     void cyrf6936SetChannel(const uint8_t ) {}
 376     void cyrf6936SetMode(const uint8_t , const bool ) {}
 377     void cyrf6936SetCrcSeed(const uint16_t ) {}
 378     void cyrf6936SetSopCode(const uint8_t ) {}
 379     void cyrf6936SetDataCode(const uint8_t ) {}
 380     void cyrf6936StartRecv(void) {}
 381     void cyrf6936SendLen(uint8_t *, const uint8_t ) {}
 382     void cyrf6936RecvLen(uint8_t *data, const uint8_t length)
 383     {
 384         if (length == packetLen) {
 385             memcpy(data, packet, packetLen);
 386         }
 387     }
 388     uint8_t cyrf6936GetRssi(void)
 389     {
 390         return cyrfRssi;
 391     }
 392
 393     void rxSpiCommonIOInit(const rxSpiConfig_t *) {}
 394     void rxSpiLedBlinkRxLoss(rx_spi_received_e ) {}
 395     void rxSpiLedBlinkBind(void) {};
 396     bool rxSpiCheckBindRequested(bool)
 397     {
 398         return false;
 399     }
 400     bool rxSpiExtiConfigured(void) { return true; }
 401 }