src/test/unit/telemetry_crsf_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 <stdbool.h>
  19 #include <stdint.h>
  20 #include <string.h>
  21
  22 #include <limits.h>
  23
  24 extern "C" {
  25     #include <platform.h>
  26
  27     #include "build/debug.h"
  28
  29     #include "common/axis.h"
  30     #include "common/crc.h"
  31     #include "common/filter.h"
  32     #include "common/gps_conversion.h"
  33     #include "common/maths.h"
  34     #include "common/printf.h"
  35     #include "common/typeconversion.h"
  36
  37     #include "pg/pg.h"
  38     #include "pg/pg_ids.h"
  39     #include "pg/rx.h"
  40
  41     #include "drivers/serial.h"
  42     #include "drivers/system.h"
  43
  44     #include "config/config.h"
  45     #include "fc/runtime_config.h"
  46     #include "fc/rc_modes.h"
  47
  48     #include "flight/pid.h"
  49     #include "flight/gps_rescue.h"
  50     #include "flight/imu.h"
  51
  52     #include "io/gps.h"
  53     #include "io/serial.h"
  54
  55     #include "rx/rx.h"
  56     #include "rx/crsf.h"
  57
  58     #include "sensors/battery.h"
  59     #include "sensors/sensors.h"
  60     #include "sensors/acceleration.h"
  61     #include "sensors/barometer.h"
  62
  63     #include "msp/msp_serial.h"
  64
  65     #include "telemetry/crsf.h"
  66     #include "telemetry/telemetry.h"
  67     #include "telemetry/msp_shared.h"
  68
  69     rssiSource_e rssiSource;
  70     bool airMode;
  71     baro_t baro;
  72
  73     uint16_t testBatteryVoltage = 0;
  74     int32_t testAmperage = 0;
  75     int32_t testmAhDrawn = 0;
  76
  77     serialPort_t *telemetrySharedPort;
  78
  79     int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType);
  80
  81     PG_REGISTER(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0);
  82     PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0);
  83     PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0);
  84     PG_REGISTER(rxConfig_t, rxConfig, PG_RX_CONFIG, 0);
  85     PG_REGISTER(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0);
  86     PG_REGISTER(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 0);
  87 }
  88
  89 #include "unittest_macros.h"
  90 #include "gtest/gtest.h"
  91
  92 uint8_t crfsCrc(uint8_t *frame, int frameLen)
  93 {
  94     uint8_t crc = 0;
  95     for (int ii = 2; ii < frameLen - 1; ++ii) {
  96         crc = crc8_dvb_s2(crc, frame[ii]);
  97     }
  98     return crc;
  99 }
 100
 101 /*
 102 int32_t     Latitude ( degree / 10`000`000 )
 103 int32_t     Longitude (degree / 10`000`000 )
 104 uint16_t    Groundspeed ( km/h / 10 )
 105 uint16_t    GPS heading ( degree / 100 )
 106 uint16      Altitude ( meter  1000m offset )
 107 uint8_t     Satellites in use ( counter )
 108 uint16_t GPS_distanceToHome;        // distance to home point in meters
 109 */
 110 #define FRAME_HEADER_FOOTER_LEN 4
 111
 112 TEST(TelemetryCrsfTest, TestGPS)
 113 {
 114     uint8_t frame[CRSF_FRAME_SIZE_MAX];
 115
 116     int frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_GPS);
 117     EXPECT_EQ(CRSF_FRAME_GPS_PAYLOAD_SIZE + FRAME_HEADER_FOOTER_LEN, frameLen);
 118     EXPECT_EQ(CRSF_SYNC_BYTE, frame[0]); // address
 119     EXPECT_EQ(17, frame[1]); // length
 120     EXPECT_EQ(0x02, frame[2]); // type
 121     int32_t lattitude = frame[3] << 24 | frame[4] << 16 | frame[5] << 8 | frame[6];
 122     EXPECT_EQ(0, lattitude);
 123     int32_t longitude = frame[7] << 24 | frame[8] << 16 | frame[9] << 8 | frame[10];
 124     EXPECT_EQ(0, longitude);
 125     uint16_t groundSpeed = frame[11] << 8 | frame[12];
 126     EXPECT_EQ(0, groundSpeed);
 127     uint16_t GPSheading = frame[13] << 8 | frame[14];
 128     EXPECT_EQ(0, GPSheading);
 129     uint16_t altitude = frame[15] << 8 | frame[16];
 130     EXPECT_EQ(1000, altitude);
 131     uint8_t satelliteCount = frame[17];
 132     EXPECT_EQ(0, satelliteCount);
 133     EXPECT_EQ(crfsCrc(frame, frameLen), frame[18]);
 134
 135     gpsSol.llh.lat = 56 * GPS_DEGREES_DIVIDER;
 136     gpsSol.llh.lon = 163 * GPS_DEGREES_DIVIDER;
 137     ENABLE_STATE(GPS_FIX);
 138     gpsSol.llh.altCm = 2345 * 100;            // altitude in cm / 100 + 1000m offset, so CRSF value should be 3345
 139     gpsSol.groundSpeed = 1630;                // speed in cm/s, 16.3 m/s = 58.68 km/h, so CRSF (km/h *10) value is 587
 140     gpsSol.numSat = 9;
 141     gpsSol.groundCourse = 1479;     // degrees * 10
 142     frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_GPS);
 143     lattitude = frame[3] << 24 | frame[4] << 16 | frame[5] << 8 | frame[6];
 144     EXPECT_EQ(560000000, lattitude);
 145     longitude = frame[7] << 24 | frame[8] << 16 | frame[9] << 8 | frame[10];
 146     EXPECT_EQ(1630000000, longitude);
 147     groundSpeed = frame[11] << 8 | frame[12];
 148     EXPECT_EQ(587, groundSpeed);
 149     GPSheading = frame[13] << 8 | frame[14];
 150     EXPECT_EQ(14790, GPSheading);
 151     altitude = frame[15] << 8 | frame[16];
 152     EXPECT_EQ(3345, altitude);
 153     satelliteCount = frame[17];
 154     EXPECT_EQ(9, satelliteCount);
 155     EXPECT_EQ(crfsCrc(frame, frameLen), frame[18]);
 156 }
 157
 158 TEST(TelemetryCrsfTest, TestBattery)
 159 {
 160     uint8_t frame[CRSF_FRAME_SIZE_MAX];
 161
 162     testBatteryVoltage = 0; // 0.1V units
 163     int frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_BATTERY_SENSOR);
 164     EXPECT_EQ(CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + FRAME_HEADER_FOOTER_LEN, frameLen);
 165     EXPECT_EQ(CRSF_SYNC_BYTE, frame[0]); // address
 166     EXPECT_EQ(10, frame[1]); // length
 167     EXPECT_EQ(0x08, frame[2]); // type
 168     uint16_t voltage = frame[3] << 8 | frame[4]; // mV * 100
 169     EXPECT_EQ(0, voltage);
 170     uint16_t current = frame[5] << 8 | frame[6]; // mA * 100
 171     EXPECT_EQ(0, current);
 172     uint32_t capacity = frame[7] << 16 | frame[8] << 8 | frame [9]; // mAh
 173     EXPECT_EQ(0, capacity);
 174     uint16_t remaining = frame[10]; // percent
 175     EXPECT_EQ(67, remaining);
 176     EXPECT_EQ(crfsCrc(frame, frameLen), frame[11]);
 177
 178     testBatteryVoltage = 330; // 3.3V = 3300 mv
 179     testAmperage = 2960; // = 29.60A = 29600mA - amperage is in 0.01A steps
 180     testmAhDrawn = 1234;
 181     frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_BATTERY_SENSOR);
 182     voltage = frame[3] << 8 | frame[4]; // mV * 100
 183     EXPECT_EQ(33, voltage);
 184     current = frame[5] << 8 | frame[6]; // mA * 100
 185     EXPECT_EQ(296, current);
 186     capacity = frame[7] << 16 | frame[8] << 8 | frame [9]; // mAh
 187     EXPECT_EQ(1234, capacity);
 188     remaining = frame[10]; // percent
 189     EXPECT_EQ(67, remaining);
 190     EXPECT_EQ(crfsCrc(frame, frameLen), frame[11]);
 191 }
 192
 193 TEST(TelemetryCrsfTest, TestAttitude)
 194 {
 195     uint8_t frame[CRSF_FRAME_SIZE_MAX];
 196
 197     attitude.values.pitch = 0;
 198     attitude.values.roll = 0;
 199     attitude.values.yaw = 0;
 200     int frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_ATTITUDE);
 201     EXPECT_EQ(CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE + FRAME_HEADER_FOOTER_LEN, frameLen);
 202     EXPECT_EQ(CRSF_SYNC_BYTE, frame[0]); // address
 203     EXPECT_EQ(8, frame[1]); // length
 204     EXPECT_EQ(0x1e, frame[2]); // type
 205     int16_t pitch = frame[3] << 8 | frame[4]; // rad / 10000
 206     EXPECT_EQ(0, pitch);
 207     int16_t roll = frame[5] << 8 | frame[6];
 208     EXPECT_EQ(0, roll);
 209     int16_t yaw = frame[7] << 8 | frame[8];
 210     EXPECT_EQ(0, yaw);
 211     EXPECT_EQ(crfsCrc(frame, frameLen), frame[9]);
 212
 213     attitude.values.pitch = 678; // decidegrees == 1.183333232852155 rad
 214     attitude.values.roll = 1495; // 2.609267231731523 rad
 215     attitude.values.yaw = -1799; //3.139847324337799 rad
 216     frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_ATTITUDE);
 217     pitch = frame[3] << 8 | frame[4]; // rad / 10000
 218     EXPECT_EQ(11833, pitch);
 219     roll = frame[5] << 8 | frame[6];
 220     EXPECT_EQ(26092, roll);
 221     yaw = frame[7] << 8 | frame[8];
 222     EXPECT_EQ(-31398, yaw);
 223     EXPECT_EQ(crfsCrc(frame, frameLen), frame[9]);
 224 }
 225
 226 TEST(TelemetryCrsfTest, TestFlightMode)
 227 {
 228     uint8_t frame[CRSF_FRAME_SIZE_MAX];
 229
 230     ENABLE_STATE(GPS_FIX);
 231     ENABLE_STATE(GPS_FIX_HOME);
 232
 233     airMode = false;
 234
 235     DISABLE_ARMING_FLAG(ARMED);
 236
 237     // nothing set, so ACRO mode
 238     int frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_FLIGHT_MODE);
 239     EXPECT_EQ(6 + FRAME_HEADER_FOOTER_LEN, frameLen);
 240     EXPECT_EQ(CRSF_SYNC_BYTE, frame[0]); // address
 241     EXPECT_EQ(8, frame[1]); // length
 242     EXPECT_EQ(0x21, frame[2]); // type
 243     EXPECT_EQ('A', frame[3]);
 244     EXPECT_EQ('C', frame[4]);
 245     EXPECT_EQ('R', frame[5]);
 246     EXPECT_EQ('O', frame[6]);
 247     EXPECT_EQ('*', frame[7]);
 248     EXPECT_EQ(0, frame[8]);
 249     EXPECT_EQ(crfsCrc(frame, frameLen), frame[9]);
 250
 251     ENABLE_ARMING_FLAG(ARMED);
 252
 253     frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_FLIGHT_MODE);
 254     EXPECT_EQ(5 + FRAME_HEADER_FOOTER_LEN, frameLen);
 255     EXPECT_EQ(CRSF_SYNC_BYTE, frame[0]); // address
 256     EXPECT_EQ(7, frame[1]); // length
 257     EXPECT_EQ(0x21, frame[2]); // type
 258     EXPECT_EQ('A', frame[3]);
 259     EXPECT_EQ('C', frame[4]);
 260     EXPECT_EQ('R', frame[5]);
 261     EXPECT_EQ('O', frame[6]);
 262     EXPECT_EQ(0, frame[7]);
 263     EXPECT_EQ(crfsCrc(frame, frameLen), frame[8]);
 264
 265     enableFlightMode(ANGLE_MODE);
 266     EXPECT_EQ(ANGLE_MODE, FLIGHT_MODE(ANGLE_MODE));
 267     frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_FLIGHT_MODE);
 268     EXPECT_EQ(5 + FRAME_HEADER_FOOTER_LEN, frameLen);
 269     EXPECT_EQ(CRSF_SYNC_BYTE, frame[0]); // address
 270     EXPECT_EQ(7, frame[1]); // length
 271     EXPECT_EQ(0x21, frame[2]); // type
 272     EXPECT_EQ('A', frame[3]);
 273     EXPECT_EQ('N', frame[4]);
 274     EXPECT_EQ('G', frame[5]);
 275     EXPECT_EQ('L', frame[6]);
 276     EXPECT_EQ(0, frame[7]);
 277     EXPECT_EQ(crfsCrc(frame, frameLen), frame[8]);
 278
 279     disableFlightMode(ANGLE_MODE);
 280     enableFlightMode(HORIZON_MODE);
 281     EXPECT_EQ(HORIZON_MODE, FLIGHT_MODE(HORIZON_MODE));
 282     frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_FLIGHT_MODE);
 283     EXPECT_EQ(4 + FRAME_HEADER_FOOTER_LEN, frameLen);
 284     EXPECT_EQ(CRSF_SYNC_BYTE, frame[0]); // address
 285     EXPECT_EQ(6, frame[1]); // length
 286     EXPECT_EQ(0x21, frame[2]); // type
 287     EXPECT_EQ('H', frame[3]);
 288     EXPECT_EQ('O', frame[4]);
 289     EXPECT_EQ('R', frame[5]);
 290     EXPECT_EQ(0, frame[6]);
 291     EXPECT_EQ(crfsCrc(frame, frameLen), frame[7]);
 292
 293     disableFlightMode(HORIZON_MODE);
 294     airMode = true;
 295     frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_FLIGHT_MODE);
 296     EXPECT_EQ(4 + FRAME_HEADER_FOOTER_LEN, frameLen);
 297     EXPECT_EQ(CRSF_SYNC_BYTE, frame[0]); // address
 298     EXPECT_EQ(6, frame[1]); // length
 299     EXPECT_EQ(0x21, frame[2]); // type
 300     EXPECT_EQ('A', frame[3]);
 301     EXPECT_EQ('I', frame[4]);
 302     EXPECT_EQ('R', frame[5]);
 303     EXPECT_EQ(0, frame[6]);
 304     EXPECT_EQ(crfsCrc(frame, frameLen), frame[7]);
 305 }
 306
 307 // STUBS
 308
 309 extern "C" {
 310
 311 int16_t debug[DEBUG16_VALUE_COUNT];
 312
 313 const uint32_t baudRates[] = {0, 9600, 19200, 38400, 57600, 115200, 230400, 250000, 400000}; // see baudRate_e
 314
 315 uint16_t batteryWarningVoltage;
 316 uint8_t useHottAlarmSoundPeriod (void) { return 0; }
 317
 318 attitudeEulerAngles_t attitude = { { 0, 0, 0 } };     // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
 319
 320 uint16_t GPS_distanceToHome;        // distance to home point in meters
 321 gpsSolutionData_t gpsSol;
 322
 323 void beeperConfirmationBeeps(uint8_t beepCount) {UNUSED(beepCount);}
 324
 325 uint32_t micros(void) {return 0;}
 326 uint32_t microsISR(void) {return micros();}
 327
 328 bool featureIsEnabled(uint32_t) {return true;}
 329
 330 uint32_t serialRxBytesWaiting(const serialPort_t *) {return 0;}
 331 uint32_t serialTxBytesFree(const serialPort_t *) {return 0;}
 332 uint8_t serialRead(serialPort_t *) {return 0;}
 333 void serialWrite(serialPort_t *, uint8_t) {}
 334 void serialWriteBuf(serialPort_t *, const uint8_t *, int) {}
 335 void serialSetMode(serialPort_t *, portMode_e) {}
 336 serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, serialReceiveCallbackPtr, void *, uint32_t, portMode_e, portOptions_e) {return NULL;}
 337 void closeSerialPort(serialPort_t *) {}
 338 bool isSerialTransmitBufferEmpty(const serialPort_t *) { return true; }
 339
 340 const serialPortConfig_t *findSerialPortConfig(serialPortFunction_e) {return NULL;}
 341
 342 bool telemetryDetermineEnabledState(portSharing_e) {return true;}
 343 bool telemetryCheckRxPortShared(const serialPortConfig_t *, SerialRXType) {return true;}
 344 bool telemetryIsSensorEnabled(sensor_e) {return true;}
 345
 346 portSharing_e determinePortSharing(const serialPortConfig_t *, serialPortFunction_e) {return PORTSHARING_NOT_SHARED;}
 347
 348 bool isAirmodeEnabled(void) {return airMode;}
 349
 350 int32_t getAmperage(void)
 351 {
 352     return testAmperage;
 353 }
 354
 355 uint16_t getBatteryVoltage(void)
 356 {
 357     return testBatteryVoltage;
 358 }
 359
 360 uint16_t getLegacyBatteryVoltage(void)
 361 {
 362     return (testBatteryVoltage + 5) / 10;
 363 }
 364
 365 uint16_t getBatteryAverageCellVoltage(void)
 366 {
 367     return 0;
 368 }
 369
 370 batteryState_e getBatteryState(void)
 371 {
 372     return BATTERY_OK;
 373 }
 374
 375 uint8_t calculateBatteryPercentageRemaining(void)
 376 {
 377     return 67;
 378 }
 379
 380 int32_t getEstimatedAltitudeCm(void)
 381 {
 382         return gpsSol.llh.altCm;    // function returns cm not m.
 383 }
 384
 385 int16_t getEstimatedVario(void) { return 0; }
 386
 387 int32_t getMAhDrawn(void)
 388 {
 389   return testmAhDrawn;
 390 }
 391
 392 bool sendMspReply(uint8_t, mspResponseFnPtr) { return false; }
 393 bool handleMspFrame(uint8_t *, uint8_t, uint8_t *)  { return false; }
 394 bool isBatteryVoltageConfigured(void) { return true; }
 395 bool isAmperageConfigured(void) { return true; }
 396 timeUs_t rxFrameTimeUs(void) { return 0; }
 397 bool IS_RC_MODE_ACTIVE(boxId_e) { return false; }
 398 bool gpsRescueIsConfigured(void) { return false; }
 399 }