src/test/unit/rx_spi_expresslrs_telemetry_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 /*
  19  * Based on https://github.com/ExpressLRS/ExpressLRS
  20  * Thanks to AlessandroAU, original creator of the ExpressLRS project.
  21  */
  22
  23 #include <stdint.h>
  24 #include <stdbool.h>
  25
  26 #include <limits.h>
  27
  28 extern "C" {
  29     #include "platform.h"
  30
  31     #include "build/version.h"
  32     #include "common/printf.h"
  33
  34     #include "drivers/io.h"
  35
  36     #include "pg/pg.h"
  37     #include "pg/pg_ids.h"
  38     #include "pg/rx.h"
  39
  40     #include "msp/msp.h"
  41     #include "msp/msp_serial.h"
  42
  43     #include "telemetry/telemetry.h"
  44     #include "telemetry/msp_shared.h"
  45     #include "rx/crsf_protocol.h"
  46     #include "rx/expresslrs_telemetry.h"
  47     #include "flight/imu.h"
  48
  49     #include "sensors/battery.h"
  50     #include "sensors/sensors.h"
  51     #include "sensors/acceleration.h"
  52
  53     #include "config/config.h"
  54
  55     #include "io/gps.h"
  56
  57     #include "msp/msp_protocol.h"
  58
  59     extern uint8_t tlmSensors;
  60     extern uint8_t currentPayloadIndex;
  61
  62     extern volatile bool mspReplyPending;
  63     extern volatile bool deviceInfoReplyPending;
  64
  65     bool airMode;
  66
  67     uint16_t testBatteryVoltage = 0;
  68     int32_t testAmperage = 0;
  69     int32_t testmAhDrawn = 0;
  70
  71     PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0);
  72     PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0);
  73 }
  74
  75 #include "unittest_macros.h"
  76 #include "gtest/gtest.h"
  77
  78 //make clean test_rx_spi_expresslrs_telemetry_unittest
  79 TEST(RxSpiExpressLrsTelemetryUnitTest, TestInit)
  80 {
  81     initTelemetry();
  82     EXPECT_EQ(tlmSensors, 15);
  83 }
  84
  85 static void testSetDataToTransmit(uint8_t payloadSize, uint8_t *payload)
  86 {
  87     uint8_t data[ELRS_TELEMETRY_BYTES_PER_CALL] = {0};
  88     uint8_t maxPackageIndex = (payloadSize - 1) / ELRS_TELEMETRY_BYTES_PER_CALL;
  89     uint8_t nextPackageIndex;
  90     bool confirmValue = true;
  91
  92     setTelemetryDataToTransmit(payloadSize, payload);
  93
  94     for (int j = 0; j <= maxPackageIndex; j++) {
  95         nextPackageIndex = getCurrentTelemetryPayload(data);
  96         if (j != maxPackageIndex) {
  97             EXPECT_EQ(1 + j, nextPackageIndex);
  98         } else {
  99             EXPECT_EQ(0, nextPackageIndex); //back to start
 100         }
 101         uint8_t maxLength = (j == maxPackageIndex) ? payloadSize % ELRS_TELEMETRY_BYTES_PER_CALL : ELRS_TELEMETRY_BYTES_PER_CALL;
 102         for (int i = 0; i < maxLength; i++) {
 103             EXPECT_EQ(payload[i + j * ELRS_TELEMETRY_BYTES_PER_CALL], data[i]);
 104         }
 105         EXPECT_EQ(true, isTelemetrySenderActive());
 106         confirmCurrentTelemetryPayload(confirmValue);
 107         confirmValue = !confirmValue;
 108     }
 109
 110     EXPECT_EQ(false, isTelemetrySenderActive());
 111 }
 112
 113 TEST(RxSpiExpressLrsTelemetryUnitTest, TestGps)
 114 {
 115     initTelemetry();
 116     currentPayloadIndex = 0;
 117
 118     gpsSol.llh.lat = 56 * GPS_DEGREES_DIVIDER;
 119     gpsSol.llh.lon = 163 * GPS_DEGREES_DIVIDER;
 120     gpsSol.llh.altCm = 2345 * 100;            // altitude in cm / 100 + 1000m offset, so CRSF value should be 3345
 121     gpsSol.groundSpeed = 1630;                // speed in cm/s, 16.3 m/s = 58.68 km/h, so CRSF (km/h *10) value is 587
 122     gpsSol.numSat = 9;
 123     gpsSol.groundCourse = 1479;     // degrees * 10
 124
 125     uint8_t *payload = 0;
 126     uint8_t payloadSize = 0;
 127
 128     getNextTelemetryPayload(&payloadSize, &payload);
 129     EXPECT_EQ(currentPayloadIndex, 1);
 130
 131     int32_t lattitude = payload[3] << 24 | payload[4] << 16 | payload[5] << 8 | payload[6];
 132     EXPECT_EQ(560000000, lattitude);
 133     int32_t longitude = payload[7] << 24 | payload[8] << 16 | payload[9] << 8 | payload[10];
 134     EXPECT_EQ(1630000000, longitude);
 135     uint16_t groundSpeed = payload[11] << 8 | payload[12];
 136     EXPECT_EQ(587, groundSpeed);
 137     uint16_t GPSheading = payload[13] << 8 | payload[14];
 138     EXPECT_EQ(14790, GPSheading);
 139     uint16_t altitude = payload[15] << 8 | payload[16];
 140     EXPECT_EQ(3345, altitude);
 141     uint8_t satelliteCount = payload[17];
 142     EXPECT_EQ(9, satelliteCount);
 143
 144     testSetDataToTransmit(payloadSize, payload);
 145 }
 146
 147 TEST(RxSpiExpressLrsTelemetryUnitTest, TestBattery)
 148 {
 149     initTelemetry();
 150     currentPayloadIndex = 1;
 151
 152     testBatteryVoltage = 330; // 3.3V = 3300 mv
 153     testAmperage = 2960; // = 29.60A = 29600mA - amperage is in 0.01A steps
 154     testmAhDrawn = 1234;
 155
 156     uint8_t *payload = 0;
 157     uint8_t payloadSize = 0;
 158
 159     getNextTelemetryPayload(&payloadSize, &payload);
 160     EXPECT_EQ(currentPayloadIndex, 2);
 161
 162     uint16_t voltage = payload[3] << 8 | payload[4]; // mV * 100
 163     EXPECT_EQ(33, voltage);
 164     uint16_t current = payload[5] << 8 | payload[6]; // mA * 100
 165     EXPECT_EQ(296, current);
 166     uint32_t capacity = payload[7] << 16 | payload[8] << 8 | payload[9]; // mAh
 167     EXPECT_EQ(1234, capacity);
 168     uint16_t remaining = payload[10]; // percent
 169     EXPECT_EQ(67, remaining);
 170
 171     testSetDataToTransmit(payloadSize, payload);
 172 }
 173
 174 TEST(RxSpiExpressLrsTelemetryUnitTest, TestAttitude)
 175 {
 176     initTelemetry();
 177     currentPayloadIndex = 2;
 178
 179     attitude.values.pitch = 678; // decidegrees == 1.183333232852155 rad
 180     attitude.values.roll = 1495; // 2.609267231731523 rad
 181     attitude.values.yaw = -1799; //3.139847324337799 rad
 182
 183     uint8_t *payload = 0;
 184     uint8_t payloadSize = 0;
 185
 186     getNextTelemetryPayload(&payloadSize, &payload);
 187     EXPECT_EQ(currentPayloadIndex, 3);
 188
 189     int16_t pitch = payload[3] << 8 | payload[4]; // rad / 10000
 190     EXPECT_EQ(11833, pitch);
 191     int16_t roll = payload[5] << 8 | payload[6];
 192     EXPECT_EQ(26092, roll);
 193     int16_t yaw = payload[7] << 8 | payload[8];
 194     EXPECT_EQ(-31398, yaw);
 195
 196     testSetDataToTransmit(payloadSize, payload);
 197 }
 198
 199 TEST(RxSpiExpressLrsTelemetryUnitTest, TestFlightMode)
 200 {
 201     initTelemetry();
 202     currentPayloadIndex = 3;
 203
 204     airMode = false;
 205
 206     uint8_t *payload = 0;
 207     uint8_t payloadSize = 0;
 208
 209     getNextTelemetryPayload(&payloadSize, &payload);
 210     EXPECT_EQ(currentPayloadIndex, 0);
 211
 212     EXPECT_EQ('W', payload[3]);
 213     EXPECT_EQ('A', payload[4]);
 214     EXPECT_EQ('I', payload[5]);
 215     EXPECT_EQ('T', payload[6]);
 216     EXPECT_EQ('*', payload[7]);
 217     EXPECT_EQ(0, payload[8]);
 218
 219     testSetDataToTransmit(payloadSize, payload);
 220 }
 221
 222 TEST(RxSpiExpressLrsTelemetryUnitTest, TestMspVersionRequest)
 223 {
 224     uint8_t request[15] = {238, 12, 122, 200, 234, 48, 0, 1, 1, 0, 0, 0, 0, 128, 0};
 225     uint8_t response[12] = {200, 10, 123, 234, 200, 48, 3, 1, 0, API_VERSION_MAJOR, API_VERSION_MINOR, 255};
 226     uint8_t data1[6] = {1, request[0], request[1], request[2], request[3], request[4]};
 227     uint8_t data2[6] = {2, request[5], request[6], request[7], request[8], request[9]};
 228     uint8_t data3[6] = {3, request[10], request[11], request[12], request[13], request[14]};
 229     uint8_t mspBuffer[15] = {0};
 230
 231     uint8_t *payload = 0;
 232     uint8_t payloadSize = 0;
 233
 234     EXPECT_EQ(CRSF_ADDRESS_CRSF_TRANSMITTER, request[0]);
 235     EXPECT_EQ(CRSF_FRAMETYPE_MSP_REQ, request[2]);
 236     EXPECT_EQ(CRSF_ADDRESS_FLIGHT_CONTROLLER, request[3]);
 237     EXPECT_EQ(CRSF_ADDRESS_RADIO_TRANSMITTER, request[4]);
 238
 239     initTelemetry();
 240     initSharedMsp();
 241
 242     setMspDataToReceive(15, mspBuffer);
 243     receiveMspData(data1[0], data1 + 1);
 244     receiveMspData(data2[0], data2 + 1);
 245     receiveMspData(data3[0], data3 + 1);
 246     EXPECT_FALSE(hasFinishedMspData());
 247     receiveMspData(0, 0);
 248     EXPECT_TRUE(hasFinishedMspData());
 249
 250     processMspPacket(mspBuffer);
 251     EXPECT_TRUE(mspReplyPending);
 252
 253     getNextTelemetryPayload(&payloadSize, &payload);
 254
 255     EXPECT_EQ(payload[1] + 2, payloadSize);
 256     EXPECT_EQ(CRSF_FRAMETYPE_MSP_RESP, payload[2]);
 257     EXPECT_EQ(CRSF_ADDRESS_RADIO_TRANSMITTER, payload[3]);
 258     EXPECT_EQ(CRSF_ADDRESS_FLIGHT_CONTROLLER, payload[4]);
 259
 260     for (int i = 0; i < 12; i++) {
 261         EXPECT_EQ(response[i], payload[i]);
 262     }
 263
 264     testSetDataToTransmit(payloadSize, payload);
 265 }
 266
 267 TEST(RxSpiExpressLrsTelemetryUnitTest, TestMspPidRequest)
 268 {
 269     uint8_t pidRequest[15] = {0x00,0x0D,0x7A,0xC8,0xEA,0x30,0x00,0x70,0x70,0x00,0x00,0x00,0x00,0x69, 0x00};
 270     uint8_t data1[6] = {1, pidRequest[0], pidRequest[1], pidRequest[2], pidRequest[3], pidRequest[4]};
 271     uint8_t data2[6] = {2, pidRequest[5], pidRequest[6], pidRequest[7], pidRequest[8], pidRequest[9]};
 272     uint8_t data3[6] = {3, pidRequest[10], pidRequest[11], pidRequest[12], pidRequest[13], pidRequest[14]};
 273     uint8_t mspBuffer[15] = {0};
 274
 275     uint8_t *payload = 0;
 276     uint8_t payloadSize = 0;
 277
 278     initTelemetry();
 279     initSharedMsp();
 280
 281     setMspDataToReceive(sizeof(mspBuffer), mspBuffer);
 282     receiveMspData(data1[0], data1 + 1);
 283     EXPECT_FALSE(hasFinishedMspData());
 284     receiveMspData(data2[0], data2 + 1);
 285     EXPECT_FALSE(hasFinishedMspData());
 286     receiveMspData(data3[0], data3 + 1);
 287     EXPECT_FALSE(hasFinishedMspData());
 288     receiveMspData(0, 0);
 289     EXPECT_TRUE(hasFinishedMspData());
 290     for (int i = 0; i < 15; i ++) {
 291         EXPECT_EQ(mspBuffer[i], pidRequest[i]);
 292     }
 293     EXPECT_FALSE(mspReplyPending);
 294
 295     processMspPacket(mspBuffer);
 296     EXPECT_TRUE(mspReplyPending);
 297
 298     getNextTelemetryPayload(&payloadSize, &payload);
 299     EXPECT_FALSE(mspReplyPending);
 300
 301     EXPECT_EQ(payloadSize, payload[1] + 2);
 302     EXPECT_EQ(CRSF_FRAMETYPE_MSP_RESP, payload[2]);
 303     EXPECT_EQ(CRSF_ADDRESS_RADIO_TRANSMITTER, payload[3]);
 304     EXPECT_EQ(CRSF_ADDRESS_FLIGHT_CONTROLLER, payload[4]);
 305     EXPECT_EQ(0x31, payload[5]); //0x30 + 1 since it is second request, see msp_shared.c:L204
 306     EXPECT_EQ(0x1E, payload[6]);
 307     EXPECT_EQ(0x70, payload[7]);
 308     for (int i = 1; i <= 30; i++) {
 309         EXPECT_EQ(i, payload[i + 7]);
 310     }
 311     EXPECT_EQ(0x1E, payload[37]);
 312
 313     testSetDataToTransmit(payloadSize, payload);
 314 }
 315
 316 TEST(RxSpiExpressLrsTelemetryUnitTest, TestMspVtxRequest)
 317 {
 318     uint8_t vtxRequest[15] = {0x00,0x0C,0x7C,0xC8,0xEA,0x30,0x04,0x59,0x18,0x00,0x01,0x00,0x44,0x5E, 0x00};
 319     uint8_t data1[6] = {1, vtxRequest[0], vtxRequest[1], vtxRequest[2], vtxRequest[3], vtxRequest[4]};
 320     uint8_t data2[6] = {2, vtxRequest[5], vtxRequest[6], vtxRequest[7], vtxRequest[8], vtxRequest[9]};
 321     uint8_t data3[6] = {3, vtxRequest[10], vtxRequest[11], vtxRequest[12], vtxRequest[13], vtxRequest[14]};
 322     uint8_t mspBuffer[15] = {0};
 323
 324     uint8_t *payload = 0;
 325     uint8_t payloadSize = 0;
 326
 327     initTelemetry();
 328     initSharedMsp();
 329
 330     setMspDataToReceive(sizeof(mspBuffer), mspBuffer);
 331     receiveMspData(data1[0], data1 + 1);
 332     receiveMspData(data2[0], data2 + 1);
 333     receiveMspData(data3[0], data3 + 1);
 334     EXPECT_FALSE(hasFinishedMspData());
 335     receiveMspData(0, 0);
 336     EXPECT_TRUE(hasFinishedMspData());
 337
 338     processMspPacket(mspBuffer);
 339     EXPECT_TRUE(mspReplyPending);
 340
 341     getNextTelemetryPayload(&payloadSize, &payload);
 342
 343     EXPECT_EQ(payloadSize, payload[1] + 2);
 344     EXPECT_EQ(CRSF_FRAMETYPE_MSP_RESP, payload[2]);
 345     EXPECT_EQ(CRSF_ADDRESS_RADIO_TRANSMITTER, payload[3]);
 346     EXPECT_EQ(CRSF_ADDRESS_FLIGHT_CONTROLLER, payload[4]);
 347     EXPECT_EQ(0x32, payload[5]); //0x30 + 2 since it is third request, see msp_shared.c:L204
 348     EXPECT_EQ(0x00, payload[6]);
 349     EXPECT_EQ(0x59, payload[7]);
 350
 351     testSetDataToTransmit(payloadSize, payload);
 352 }
 353
 354 TEST(RxSpiExpressLrsTelemetryUnitTest, TestDeviceInfoResp)
 355 {
 356     uint8_t mspBuffer[15] = {0};
 357
 358     uint8_t *payload = 0;
 359     uint8_t payloadSize = 0;
 360
 361     uint8_t pingData[4] = {1, CRSF_ADDRESS_CRSF_TRANSMITTER, 1, CRSF_FRAMETYPE_DEVICE_PING};
 362
 363     initTelemetry();
 364     initSharedMsp();
 365
 366     setMspDataToReceive(sizeof(mspBuffer), mspBuffer);
 367     receiveMspData(pingData[0], pingData + 1);
 368     EXPECT_FALSE(hasFinishedMspData());
 369     receiveMspData(0, pingData + 1);
 370     EXPECT_TRUE(hasFinishedMspData());
 371
 372     EXPECT_FALSE(deviceInfoReplyPending);
 373
 374     processMspPacket(mspBuffer);
 375     EXPECT_TRUE(deviceInfoReplyPending);
 376
 377     getNextTelemetryPayload(&payloadSize, &payload);
 378     EXPECT_FALSE(deviceInfoReplyPending);
 379
 380     EXPECT_EQ(CRSF_FRAMETYPE_DEVICE_INFO, payload[2]);
 381     EXPECT_EQ(CRSF_ADDRESS_RADIO_TRANSMITTER, payload[3]);
 382     EXPECT_EQ(CRSF_ADDRESS_FLIGHT_CONTROLLER, payload[4]);
 383     EXPECT_EQ(0x01, payload[payloadSize - 2]);
 384     EXPECT_EQ(0, payload[payloadSize - 3]);
 385
 386     testSetDataToTransmit(payloadSize, payload);
 387 }
 388
 389 // STUBS
 390
 391 extern "C" {
 392
 393     attitudeEulerAngles_t attitude = { { 0, 0, 0 } };     // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
 394     gpsSolutionData_t gpsSol;
 395     rssiSource_e rssiSource;
 396     uint8_t armingFlags;
 397     uint8_t stateFlags;
 398     uint16_t flightModeFlags;
 399
 400     uint32_t microsISR(void) {return 0; }
 401
 402     void beeperConfirmationBeeps(uint8_t ) {}
 403
 404     uint8_t calculateBatteryPercentageRemaining(void) {return 67; }
 405
 406     int32_t getAmperage(void) {return testAmperage; }
 407
 408     uint16_t getBatteryVoltage(void) {return testBatteryVoltage; }
 409
 410     uint16_t getLegacyBatteryVoltage(void) {return (testBatteryVoltage + 5) / 10; }
 411
 412     uint16_t getBatteryAverageCellVoltage(void) {return 0; }
 413
 414     batteryState_e getBatteryState(void) {return BATTERY_OK; }
 415
 416     bool featureIsEnabled(uint32_t) {return true; }
 417     bool telemetryIsSensorEnabled(sensor_e) {return true; }
 418     bool sensors(uint32_t ) { return true; }
 419
 420     bool airmodeIsEnabled(void) {return airMode; }
 421
 422     bool isBatteryVoltageConfigured(void) { return true; }
 423     bool isAmperageConfigured(void) { return true; }
 424
 425     const serialPortConfig_t *findSerialPortConfig(serialPortFunction_e) { return NULL;}
 426     serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, serialReceiveCallbackPtr, void *, uint32_t, portMode_e, portOptions_e) { return NULL; }
 427     void serialWriteBuf(serialPort_t *, const uint8_t *, int) {}
 428
 429     int32_t getEstimatedAltitudeCm(void) { return gpsSol.llh.altCm; }
 430
 431     int32_t getMAhDrawn(void) { return testmAhDrawn; }
 432
 433     bool isArmingDisabled(void) { return false; }
 434
 435     mspDescriptor_t mspDescriptorAlloc(void) {return 0; }
 436
 437     uint8_t mspSerialOutBuf[MSP_PORT_OUTBUF_SIZE];
 438
 439     mspResult_e mspFcProcessCommand(mspDescriptor_t srcDesc, mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn) {
 440
 441         UNUSED(srcDesc);
 442         UNUSED(mspPostProcessFn);
 443
 444         sbuf_t *dst = &reply->buf;
 445         const uint8_t cmdMSP = cmd->cmd;
 446         reply->cmd = cmd->cmd;
 447
 448         if (cmdMSP == 0x70) {
 449             for (unsigned int ii=1; ii<=30; ii++) {
 450                 sbufWriteU8(dst, ii);
 451             }
 452         } else if (cmdMSP == 0xCA) {
 453             return MSP_RESULT_ACK;
 454         } else if (cmdMSP == 0x01) {
 455             sbufWriteU8(dst, MSP_PROTOCOL_VERSION);
 456             sbufWriteU8(dst, API_VERSION_MAJOR);
 457             sbufWriteU8(dst, API_VERSION_MINOR);
 458         }
 459
 460         return MSP_RESULT_ACK;
 461     }
 462
 463     timeUs_t rxFrameTimeUs(void) { return 0; }
 464
 465 }