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