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