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