src/test/unit/link_quality_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 #include <stdio.h>
  21 #include <string.h>
  22
  23 extern "C" {
  24     #include "platform.h"
  25     #include "build/debug.h"
  26
  27     #include "blackbox/blackbox.h"
  28     #include "blackbox/blackbox_io.h"
  29
  30     #include "common/crc.h"
  31     #include "common/printf.h"
  32     #include "common/streambuf.h"
  33     #include "common/time.h"
  34     #include "common/utils.h"
  35
  36     #include "config/config.h"
  37
  38     #include "drivers/osd_symbols.h"
  39     #include "drivers/persistent.h"
  40     #include "drivers/serial.h"
  41     #include "drivers/system.h"
  42
  43     #include "fc/core.h"
  44     #include "fc/rc_controls.h"
  45     #include "fc/rc_modes.h"
  46     #include "fc/runtime_config.h"
  47
  48     #include "flight/imu.h"
  49     #include "flight/mixer.h"
  50     #include "flight/pid.h"
  51
  52     #include "io/beeper.h"
  53     #include "io/gps.h"
  54     #include "io/serial.h"
  55
  56     #include "osd/osd.h"
  57     #include "osd/osd_elements.h"
  58     #include "osd/osd_warnings.h"
  59
  60     #include "pg/pg.h"
  61     #include "pg/pg_ids.h"
  62     #include "pg/rx.h"
  63
  64     #include "rx/rx.h"
  65
  66     #include "sensors/battery.h"
  67
  68     attitudeEulerAngles_t attitude;
  69     float rMat[3][3];
  70
  71     pidProfile_t *currentPidProfile;
  72     float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
  73     uint8_t GPS_numSat;
  74     uint16_t GPS_distanceToHome;
  75     int16_t GPS_directionToHome;
  76     uint32_t GPS_distanceFlownInCm;
  77     int32_t GPS_coord[2];
  78     gpsSolutionData_t gpsSol;
  79     float motor[8];
  80     acc_t acc;
  81     float accAverage[XYZ_AXIS_COUNT];
  82
  83     PG_REGISTER(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0);
  84     PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0);
  85     PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0);
  86     PG_REGISTER(pilotConfig_t, pilotConfig, PG_PILOT_CONFIG, 0);
  87     PG_REGISTER(imuConfig_t, imuConfig, PG_IMU_CONFIG, 0);
  88     PG_REGISTER(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0);
  89
  90     timeUs_t simulationTime = 0;
  91
  92     void osdUpdate(timeUs_t currentTimeUs);
  93     uint16_t updateLinkQualitySamples(uint16_t value);
  94 #define LINK_QUALITY_SAMPLE_COUNT 16
  95 }
  96
  97 /* #define DEBUG_OSD */
  98
  99 #include "unittest_macros.h"
 100 #include "unittest_displayport.h"
 101 #include "gtest/gtest.h"
 102
 103 extern "C" {
 104     PG_REGISTER(flight3DConfig_t, flight3DConfig, PG_MOTOR_3D_CONFIG, 0);
 105
 106     boxBitmask_t rcModeActivationMask;
 107     int16_t debug[DEBUG16_VALUE_COUNT];
 108     uint8_t debugMode = 0;
 109
 110     uint16_t updateLinkQualitySamples(uint16_t value);
 111
 112     extern uint16_t applyRxChannelRangeConfiguraton(int sample, const rxChannelRangeConfig_t *range);
 113 }
 114 void setDefaultSimulationState()
 115 {
 116     setLinkQualityDirect(LINK_QUALITY_MAX_VALUE);
 117     osdConfigMutable()->framerate_hz = 12;
 118 }
 119 /*
 120  * Performs a test of the OSD actions on arming.
 121  * (reused throughout the test suite)
 122  */
 123 void doTestArm(bool testEmpty = true)
 124 {
 125     // given
 126     // craft has been armed
 127     ENABLE_ARMING_FLAG(ARMED);
 128
 129     simulationTime += 0.1e6;
 130     // when
 131     // sufficient OSD updates have been called
 132     while (osdUpdateCheck(simulationTime, 0)) {
 133         osdUpdate(simulationTime);
 134         simulationTime += 10;
 135     }
 136
 137     // then
 138     // arming alert displayed
 139     displayPortTestBufferSubstring(12, 7, "ARMED");
 140
 141     // given
 142     // armed alert times out (0.5 seconds)
 143     simulationTime += 0.5e6;
 144
 145     // when
 146     // sufficient OSD updates have been called
 147     while (osdUpdateCheck(simulationTime, 0)) {
 148         osdUpdate(simulationTime);
 149         simulationTime += 10;
 150     }
 151
 152     // then
 153     // arming alert disappears
 154 #ifdef DEBUG_OSD
 155     displayPortTestPrint();
 156 #endif
 157     if (testEmpty) {
 158         displayPortTestBufferIsEmpty();
 159     }
 160 }
 161
 162 /*
 163  * Auxiliary function. Test is there're stats that must be shown
 164  */
 165 bool isSomeStatEnabled(void) {
 166     return (osdConfigMutable()->enabled_stats != 0);
 167 }
 168
 169 /*
 170  * Performs a test of the OSD actions on disarming.
 171  * (reused throughout the test suite)
 172  */
 173 void doTestDisarm()
 174 {
 175     // given
 176     // craft is disarmed after having been armed
 177     DISABLE_ARMING_FLAG(ARMED);
 178
 179     // when
 180     // sufficient OSD updates have been called
 181     while (osdUpdateCheck(simulationTime, 0)) {
 182         osdUpdate(simulationTime);
 183         simulationTime += 10;
 184     }
 185
 186     // then
 187     // post flight statistics displayed
 188     if (isSomeStatEnabled()) {
 189         displayPortTestBufferSubstring(2, 2, "  --- STATS ---");
 190     }
 191 }
 192
 193 /*
 194  * Tests initialisation of the OSD and the power on splash screen.
 195  */
 196 TEST(LQTest, TestInit)
 197 {
 198     // given
 199     // display port is initialised
 200     displayPortTestInit();
 201
 202     // and
 203     // default state values are set
 204     setDefaultSimulationState();
 205
 206     // and
 207     // this battery configuration (used for battery voltage elements)
 208     batteryConfigMutable()->vbatmincellvoltage = 330;
 209     batteryConfigMutable()->vbatmaxcellvoltage = 430;
 210
 211     // when
 212     // OSD is initialised
 213     osdInit(&testDisplayPort, OSD_DISPLAYPORT_DEVICE_AUTO);
 214
 215     while (osdUpdateCheck(simulationTime, 0)) {
 216         osdUpdate(simulationTime);
 217         simulationTime += 10;
 218     }
 219
 220     // then
 221     // display buffer should contain splash screen
 222     displayPortTestBufferSubstring(7, 8, "MENU:THR MID");
 223     displayPortTestBufferSubstring(11, 9, "+ YAW LEFT");
 224     displayPortTestBufferSubstring(11, 10, "+ PITCH UP");
 225
 226     // when
 227     // splash screen timeout has elapsed
 228     simulationTime += 4e6;
 229     while (osdUpdateCheck(simulationTime, 0)) {
 230         osdUpdate(simulationTime);
 231         simulationTime += 10;
 232     }
 233
 234     // then
 235     // display buffer should be empty
 236 #ifdef DEBUG_OSD
 237     displayPortTestPrint();
 238 #endif
 239     displayPortTestBufferIsEmpty();
 240 }
 241 /*
 242  * Tests the Tests the OSD_LINK_QUALITY element updateLinkQualitySamples  default LQ_SOURCE_NONE
 243  */
 244 TEST(LQTest, TestElement_LQ_SOURCE_NONE_SAMPLES)
 245 {
 246     // given
 247     linkQualitySource = LQ_SOURCE_NONE;
 248
 249     osdElementConfigMutable()->item_pos[OSD_LINK_QUALITY] = OSD_POS(8, 1) | OSD_PROFILE_1_FLAG;
 250     osdConfigMutable()->link_quality_alarm = 0;
 251
 252     osdAnalyzeActiveElements();
 253
 254     // when samples populated 100%
 255     for (int x = 0; x < LINK_QUALITY_SAMPLE_COUNT; x++) {
 256         setLinkQualityDirect(updateLinkQualitySamples(LINK_QUALITY_MAX_VALUE));
 257     }
 258
 259     simulationTime += 1000000;
 260
 261     while (osdUpdateCheck(simulationTime, 0)) {
 262         osdUpdate(simulationTime);
 263         simulationTime += 10;
 264     }
 265
 266     // then
 267     displayPortTestBufferSubstring(8, 1, "%c9", SYM_LINK_QUALITY);
 268
 269     // when  updateLinkQualitySamples used   50% rounds to 4
 270     for (int x = 0; x < LINK_QUALITY_SAMPLE_COUNT; x++) {
 271         setLinkQualityDirect(updateLinkQualitySamples(LINK_QUALITY_MAX_VALUE));
 272         setLinkQualityDirect(updateLinkQualitySamples(0));
 273     }
 274
 275     simulationTime += 1000000;
 276
 277     while (osdUpdateCheck(simulationTime, 0)) {
 278         osdUpdate(simulationTime);
 279         simulationTime += 10;
 280     }
 281
 282     // then
 283     displayPortTestBufferSubstring(8, 1, "%c4", SYM_LINK_QUALITY);
 284 }
 285 /*
 286  * Tests the Tests the OSD_LINK_QUALITY element values  default LQ_SOURCE_NONE
 287  */
 288 TEST(LQTest, TestElement_LQ_SOURCE_NONE_VALUES)
 289 {
 290     // given
 291
 292     linkQualitySource = LQ_SOURCE_NONE;
 293
 294     osdElementConfigMutable()->item_pos[OSD_LINK_QUALITY] = OSD_POS(8, 1) | OSD_PROFILE_1_FLAG;
 295     osdConfigMutable()->link_quality_alarm = 0;
 296
 297     osdAnalyzeActiveElements();
 298     // when LINK_QUALITY_MAX_VALUE to 1 by 10%
 299     uint16_t testscale = 0;
 300     for (int testdigit = 10; testdigit > 0; testdigit--) {
 301         testscale = testdigit * 102.3;
 302         setLinkQualityDirect(testscale);
 303         simulationTime += 100000;
 304         while (osdUpdateCheck(simulationTime, 0)) {
 305             osdUpdate(simulationTime);
 306             simulationTime += 10;
 307         }
 308 #ifdef DEBUG_OSD
 309         printf("%d %d\n",testscale, testdigit);
 310         displayPortTestPrint();
 311 #endif
 312         // then
 313         if (testdigit >= 10) {
 314             displayPortTestBufferSubstring(8, 1,"%c9", SYM_LINK_QUALITY);
 315         }else{
 316             displayPortTestBufferSubstring(8, 1,"%c%1d", SYM_LINK_QUALITY, testdigit - 1);
 317         }
 318     }
 319 }
 320
 321 /*
 322  * Tests the OSD_LINK_QUALITY element LQ RX_PROTOCOL_CRSF.
 323  */
 324 TEST(LQTest, TestElementLQ_PROTOCOL_CRSF_VALUES)
 325 {
 326     // given
 327     linkQualitySource = LQ_SOURCE_RX_PROTOCOL_CRSF;
 328
 329     osdElementConfigMutable()->item_pos[OSD_LINK_QUALITY] = OSD_POS(8, 1) | OSD_PROFILE_1_FLAG;
 330     osdConfigMutable()->link_quality_alarm = 0;
 331
 332     osdAnalyzeActiveElements();
 333
 334     simulationTime += 1000000;
 335     while (osdUpdateCheck(simulationTime, 0)) {
 336         osdUpdate(simulationTime);
 337         simulationTime += 10;
 338     }
 339
 340     // crsf setLinkQualityDirect 0-300;
 341
 342     for (uint8_t x = 0; x <= 99; x++) {
 343         for (uint8_t m = 0; m <= 4; m++) {
 344             // when x scaled
 345             setLinkQualityDirect(x);
 346             rxSetRfMode(m);
 347             // then rxGetLinkQuality Osd should be x
 348             // and RfMode should be m
 349             simulationTime += 100000;
 350             while (osdUpdateCheck(simulationTime, 0)) {
 351                 osdUpdate(simulationTime);
 352                 simulationTime += 10;
 353             }
 354             displayPortTestBufferSubstring(8, 1, "%c%1d:%2d", SYM_LINK_QUALITY, m, x);
 355         }
 356     }
 357 }
 358 /*
 359  * Tests the LQ Alarms
 360  *
 361 */
 362 TEST(LQTest, TestLQAlarm)
 363 {
 364     timeUs_t startTime = simulationTime;
 365     // given
 366     // default state is set
 367     setDefaultSimulationState();
 368
 369     linkQualitySource = LQ_SOURCE_NONE;
 370
 371     // and
 372     // the following OSD elements are visible
 373
 374     osdElementConfigMutable()->item_pos[OSD_LINK_QUALITY] = OSD_POS(8, 1)  | OSD_PROFILE_1_FLAG;
 375
 376     // and
 377     // this set of alarm values
 378
 379     osdConfigMutable()->link_quality_alarm = 80;
 380     stateFlags |= GPS_FIX | GPS_FIX_HOME;
 381
 382     osdAnalyzeActiveElements();
 383
 384     // and
 385     // using the metric unit system
 386     osdConfigMutable()->units = UNIT_METRIC;
 387
 388     // when
 389     // the craft is armed
 390     doTestArm(false);
 391
 392     for (int x = 0; x < LINK_QUALITY_SAMPLE_COUNT; x++) {
 393         setLinkQualityDirect(updateLinkQualitySamples(LINK_QUALITY_MAX_VALUE));
 394     }
 395
 396     // then
 397     // no elements should flash as all values are out of alarm range
 398     // Ensure a consistent start time for testing
 399     simulationTime += 5000000;
 400     simulationTime -= simulationTime % 1000000;
 401     startTime = simulationTime;
 402     for (int i = 0; i < 30; i++) {
 403         // Check for visibility every 100ms, elements should always be visible
 404         simulationTime = startTime + i*0.1e6;
 405         while (osdUpdateCheck(simulationTime, 0)) {
 406             osdUpdate(simulationTime);
 407             simulationTime += 10;
 408         }
 409
 410 #ifdef DEBUG_OSD
 411         printf("%d\n", i);
 412 #endif
 413         displayPortTestBufferSubstring(8,  1, "%c9", SYM_LINK_QUALITY);
 414
 415     }
 416
 417     setLinkQualityDirect(512);
 418     while (osdUpdateCheck(simulationTime, 0)) {
 419         osdUpdate(simulationTime);
 420         simulationTime += 10;
 421     }
 422
 423     // then
 424     // elements showing values in alarm range should flash
 425     simulationTime += 1000000;
 426     simulationTime -= simulationTime % 1000000;
 427     startTime = simulationTime;
 428     for (int i = 0; i < 15; i++) {
 429         // Blinking should happen at 2Hz
 430         simulationTime = startTime + i*0.25e6;
 431         while (osdUpdateCheck(simulationTime, 0)) {
 432             osdUpdate(simulationTime);
 433             simulationTime += 10;
 434         }
 435
 436 #ifdef DEBUG_OSD
 437         displayPortTestPrint();
 438 #endif
 439         if (i % 2 == 0) {
 440             displayPortTestBufferSubstring(8,  1, "%c5", SYM_LINK_QUALITY);
 441         } else {
 442             displayPortTestBufferIsEmpty();
 443         }
 444     }
 445
 446     doTestDisarm();
 447     simulationTime += 1000000;
 448     simulationTime -= simulationTime % 1000000;
 449     while (osdUpdateCheck(simulationTime, 0)) {
 450         osdUpdate(simulationTime);
 451         simulationTime += 10;
 452     }
 453 }
 454
 455 // STUBS
 456 extern "C" {
 457
 458     uint32_t micros() {
 459         return simulationTime;
 460     }
 461
 462     uint32_t microsISR() {
 463         return micros();
 464     }
 465
 466     uint32_t millis() {
 467         return micros() / 1000;
 468     }
 469
 470     bool featureIsEnabled(uint32_t) { return true; }
 471     void beeperConfirmationBeeps(uint8_t) {}
 472     bool isBeeperOn() { return false; }
 473     uint8_t getCurrentPidProfileIndex() { return 0; }
 474     uint8_t getCurrentControlRateProfileIndex() { return 0; }
 475     batteryState_e getBatteryState() { return BATTERY_OK; }
 476     uint8_t getBatteryCellCount() { return 4; }
 477     uint16_t getBatteryVoltage() { return 1680; }
 478     uint16_t getBatteryAverageCellVoltage() { return  420; }
 479     int32_t getAmperage() { return 0; }
 480     int32_t getMAhDrawn() { return 0; }
 481     int32_t getEstimatedAltitudeCm() { return 0; }
 482     int32_t getEstimatedVario() { return 0; }
 483     int32_t blackboxGetLogNumber() { return 0; }
 484     bool isBlackboxDeviceWorking() { return true; }
 485     bool isBlackboxDeviceFull() { return false; }
 486     serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, serialReceiveCallbackPtr, void *, uint32_t, portMode_e, portOptions_e) {return NULL;}
 487     const serialPortConfig_t *findSerialPortConfig(serialPortFunction_e ) {return NULL;}
 488     bool telemetryCheckRxPortShared(const serialPortConfig_t *) {return false;}
 489     bool cmsDisplayPortRegister(displayPort_t *) { return false; }
 490     uint16_t getCoreTemperatureCelsius(void) { return 0; }
 491     bool isFlipOverAfterCrashActive(void) { return false; }
 492     float pidItermAccelerator(void) { return 1.0; }
 493     uint8_t getMotorCount(void){ return 4; }
 494     bool areMotorsRunning(void){ return true; }
 495     bool pidOsdAntiGravityActive(void) { return false; }
 496     bool failsafeIsActive(void) { return false; }
 497     bool failsafeIsReceivingRxData(void) { return true; }
 498     bool gpsIsHealthy(void) { return true; }
 499     bool gpsRescueIsConfigured(void) { return false; }
 500     int8_t calculateThrottlePercent(void) { return 0; }
 501     uint32_t persistentObjectRead(persistentObjectId_e) { return 0; }
 502     void persistentObjectWrite(persistentObjectId_e, uint32_t) {}
 503     void failsafeOnRxSuspend(uint32_t ) {}
 504     void failsafeOnRxResume(void) {}
 505     void featureDisableImmediate(uint32_t) { }
 506     bool rxMspFrameComplete(void) { return false; }
 507     bool isPPMDataBeingReceived(void) { return false; }
 508     bool isPWMDataBeingReceived(void) { return false; }
 509     void resetPPMDataReceivedState(void){ }
 510     void failsafeOnValidDataReceived(void) { }
 511     void failsafeOnValidDataFailed(void) { }
 512     void pinioBoxTaskControl(void) { }
 513     bool taskUpdateRxMainInProgress() { return true; }
 514     void schedulerIgnoreTaskStateTime(void) { }
 515     void schedulerIgnoreTaskExecRate(void) { }
 516     bool schedulerGetIgnoreTaskExecTime() { return false; }
 517     void schedulerIgnoreTaskExecTime(void) { }
 518     void schedulerSetNextStateTime(timeDelta_t) {}
 519
 520     void rxPwmInit(rxRuntimeState_t *rxRuntimeState, rcReadRawDataFnPtr *callback)
 521     {
 522         UNUSED(rxRuntimeState);
 523         UNUSED(callback);
 524     }
 525
 526     bool sbusInit(rxConfig_t *initialRxConfig, rxRuntimeState_t *rxRuntimeState, rcReadRawDataFnPtr *callback)
 527     {
 528         UNUSED(initialRxConfig);
 529         UNUSED(rxRuntimeState);
 530         UNUSED(callback);
 531         return true;
 532     }
 533
 534     bool spektrumInit(rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState, rcReadRawDataFnPtr *callback)
 535     {
 536         UNUSED(rxConfig);
 537         UNUSED(rxRuntimeState);
 538         UNUSED(callback);
 539         return true;
 540     }
 541
 542     bool sumdInit(rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState, rcReadRawDataFnPtr *callback)
 543     {
 544         UNUSED(rxConfig);
 545         UNUSED(rxRuntimeState);
 546         UNUSED(callback);
 547         return true;
 548     }
 549
 550     bool sumhInit(rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState, rcReadRawDataFnPtr *callback)
 551     {
 552         UNUSED(rxConfig);
 553         UNUSED(rxRuntimeState);
 554         UNUSED(callback);
 555         return true;
 556     }
 557
 558     bool crsfRxInit(rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState, rcReadRawDataFnPtr *callback);
 559
 560     bool jetiExBusInit(rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState, rcReadRawDataFnPtr *callback)
 561     {
 562         UNUSED(rxConfig);
 563         UNUSED(rxRuntimeState);
 564         UNUSED(callback);
 565         return true;
 566     }
 567
 568     bool ibusInit(rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState, rcReadRawDataFnPtr *callback)
 569     {
 570         UNUSED(rxConfig);
 571         UNUSED(rxRuntimeState);
 572         UNUSED(callback);
 573         return true;
 574     }
 575
 576     bool xBusInit(rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState, rcReadRawDataFnPtr *callback)
 577     {
 578         UNUSED(rxConfig);
 579         UNUSED(rxRuntimeState);
 580         UNUSED(callback);
 581         return true;
 582     }
 583
 584     bool rxMspInit(rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState, rcReadRawDataFnPtr *callback)
 585     {
 586         UNUSED(rxConfig);
 587         UNUSED(rxRuntimeState);
 588         UNUSED(callback);
 589         return true;
 590     }
 591
 592     float pt1FilterGain(float f_cut, float dT)
 593     {
 594         UNUSED(f_cut);
 595         UNUSED(dT);
 596         return 0.0;
 597     }
 598
 599     void pt1FilterInit(pt1Filter_t *filter, float k)
 600     {
 601         UNUSED(filter);
 602         UNUSED(k);
 603     }
 604
 605     float pt1FilterApply(pt1Filter_t *filter, float input)
 606     {
 607         UNUSED(filter);
 608         UNUSED(input);
 609         return 0.0;
 610     }
 611
 612     bool isUpright(void) { return true; }
 613
 614     float getMotorOutputLow(void) { return 1000.0; }
 615
 616     float getMotorOutputHigh(void) { return 2047.0; }
 617 }