src/test/unit/telemetry_ibus_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 <string.h>
  20
  21 extern "C" {
  22 #include "platform.h"
  23 #include "common/utils.h"
  24 #include "pg/pg.h"
  25 #include "drivers/serial.h"
  26 #include "io/serial.h"
  27 #include "io/gps.h"
  28 #include "flight/imu.h"
  29 #include "config/config.h"
  30 #include "fc/rc_controls.h"
  31 #include "telemetry/telemetry.h"
  32 #include "telemetry/ibus.h"
  33 #include "sensors/gyro.h"
  34 #include "sensors/battery.h"
  35 #include "sensors/barometer.h"
  36 #include "sensors/acceleration.h"
  37 #include "scheduler/scheduler.h"
  38 #include "fc/tasks.h"
  39 }
  40
  41 #include "unittest_macros.h"
  42 #include "gtest/gtest.h"
  43
  44
  45 extern "C" {
  46     uint8_t armingFlags = 0;
  47     uint8_t stateFlags = 0;
  48     uint16_t flightModeFlags = 0;
  49     uint8_t testBatteryCellCount =3;
  50     float rcCommand[4] = {0, 0, 0, 0};
  51     telemetryConfig_t telemetryConfig_System;
  52     batteryConfig_s batteryConfig_System;
  53     attitudeEulerAngles_t attitude = EULER_INITIALIZE;
  54     acc_t acc;
  55     baro_t baro;
  56     gpsSolutionData_t gpsSol;
  57     uint16_t GPS_distanceToHome;
  58 }
  59
  60 static int16_t gyroTemperature;
  61 int16_t gyroGetTemperature(void) {
  62     return gyroTemperature;
  63 }
  64
  65 static uint16_t vbat = 1000;
  66 uint16_t getVbat(void)
  67 {
  68     return vbat;
  69 }
  70
  71 extern "C" {
  72 static int32_t amperage = 100;
  73 static int32_t estimatedVario = 0;
  74 static uint8_t batteryRemaining = 0;
  75 static throttleStatus_e throttleStatus = THROTTLE_HIGH;
  76 static uint32_t definedFeatures = 0;
  77 static uint32_t definedSensors = SENSOR_GYRO | SENSOR_ACC | SENSOR_MAG | SENSOR_SONAR | SENSOR_GPS | SENSOR_GPSMAG;
  78 static uint16_t testBatteryVoltage = 1000;
  79
  80 int32_t getAmperage(void)
  81 {
  82     return amperage;
  83 }
  84
  85 int32_t getEstimatedVario(void)
  86 {
  87     return estimatedVario;
  88 }
  89
  90 uint8_t calculateBatteryPercentageRemaining(void)
  91 {
  92     return batteryRemaining;
  93 }
  94
  95 uint16_t getBatteryAverageCellVoltage(void)
  96 {
  97     return testBatteryVoltage / testBatteryCellCount;
  98 }
  99
 100 int32_t getMAhDrawn(void)
 101 {
 102     return 0;
 103 }
 104
 105 throttleStatus_e calculateThrottleStatus(void)
 106 {
 107     return throttleStatus;
 108 }
 109
 110 bool featureIsEnabled(uint32_t mask)
 111 {
 112     return (definedFeatures & mask) != 0;
 113 }
 114
 115 bool sensors(sensors_e sensor)
 116 {
 117     return (definedSensors & sensor) != 0;
 118 }
 119 }
 120
 121 #define SERIAL_BUFFER_SIZE 256
 122
 123 typedef struct serialPortStub_s {
 124     uint8_t buffer[SERIAL_BUFFER_SIZE];
 125     int pos = 0;
 126     int end = 0;
 127 } serialPortStub_t;
 128
 129
 130 uint16_t getBatteryVoltage(void)
 131 {
 132     return testBatteryVoltage;
 133 }
 134
 135 uint8_t getBatteryCellCount(void) {
 136     return testBatteryCellCount;
 137 }
 138
 139 static serialPortStub_t serialWriteStub;
 140 static serialPortStub_t serialReadStub;
 141
 142 #define SERIAL_PORT_DUMMY_IDENTIFIER  (serialPortIdentifier_e)0x12
 143 serialPort_t serialTestInstance;
 144 serialPortConfig_t serialTestInstanceConfig = {
 145     .identifier = SERIAL_PORT_DUMMY_IDENTIFIER,
 146     .functionMask = 0
 147 };
 148
 149 static serialPortConfig_t *findSerialPortConfig_stub_retval;
 150 static portSharing_e determinePortSharing_stub_retval;
 151 static bool portIsShared = false;
 152 static bool openSerial_called = false;
 153 static bool telemetryDetermineEnabledState_stub_retval;
 154
 155 void rescheduleTask(taskId_e taskId, timeDelta_t newPeriodUs)
 156 {
 157     EXPECT_EQ(TASK_TELEMETRY, taskId);
 158     EXPECT_EQ(1000, newPeriodUs);
 159 }
 160
 161
 162
 163 const serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function)
 164 {
 165     EXPECT_EQ(FUNCTION_TELEMETRY_IBUS, function);
 166     return findSerialPortConfig_stub_retval;
 167 }
 168
 169
 170 portSharing_e determinePortSharing(const serialPortConfig_t *portConfig, serialPortFunction_e function)
 171 {
 172     EXPECT_EQ(findSerialPortConfig_stub_retval, portConfig);
 173     EXPECT_EQ(FUNCTION_TELEMETRY_IBUS, function);
 174     return PORTSHARING_UNUSED;
 175 }
 176
 177
 178 bool telemetryDetermineEnabledState(portSharing_e portSharing)
 179 {
 180     (void) portSharing;
 181     return telemetryDetermineEnabledState_stub_retval;
 182 }
 183
 184
 185 bool telemetryIsSensorEnabled(sensor_e sensor) {
 186     UNUSED(sensor);
 187     return true;
 188 }
 189
 190
 191 bool isSerialPortShared(const serialPortConfig_t *portConfig,
 192                         uint16_t functionMask,
 193                         serialPortFunction_e sharedWithFunction)
 194 {
 195     EXPECT_EQ(findSerialPortConfig_stub_retval, portConfig);
 196     EXPECT_EQ(FUNCTION_RX_SERIAL, functionMask);
 197     EXPECT_EQ(FUNCTION_TELEMETRY_IBUS, sharedWithFunction);
 198     return portIsShared;
 199 }
 200
 201
 202 serialPortConfig_t *findSerialPortConfig(uint16_t mask)
 203 {
 204     EXPECT_EQ(FUNCTION_TELEMETRY_IBUS, mask);
 205     return findSerialPortConfig_stub_retval ;
 206 }
 207
 208
 209 serialPort_t *openSerialPort(
 210     serialPortIdentifier_e identifier,
 211     serialPortFunction_e function,
 212     serialReceiveCallbackPtr callback,
 213     void *callbackData,
 214     uint32_t baudrate,
 215     portMode_e mode,
 216     portOptions_e options
 217 )
 218 {
 219     openSerial_called = true;
 220     UNUSED(callback);
 221     UNUSED(callbackData);
 222     EXPECT_EQ(SERIAL_PORT_DUMMY_IDENTIFIER, identifier);
 223     EXPECT_EQ(SERIAL_BIDIR, options);
 224     EXPECT_EQ(FUNCTION_TELEMETRY_IBUS, function);
 225     EXPECT_EQ(115200, baudrate);
 226     EXPECT_EQ(MODE_RXTX, mode);
 227     return &serialTestInstance;
 228 }
 229
 230 void closeSerialPort(serialPort_t *serialPort)
 231 {
 232     EXPECT_EQ(&serialTestInstance, serialPort);
 233 }
 234
 235
 236 void serialWrite(serialPort_t *instance, uint8_t ch)
 237 {
 238     EXPECT_EQ(&serialTestInstance, instance);
 239     EXPECT_LT(serialWriteStub.pos, sizeof(serialWriteStub.buffer));
 240     serialWriteStub.buffer[serialWriteStub.pos++] = ch;
 241     serialReadStub.buffer[serialReadStub.end++] = ch; //characters echoes back on the shared wire
 242     //printf("w: %02d 0x%02x\n", serialWriteStub.pos, ch);
 243 }
 244
 245
 246 uint32_t serialRxBytesWaiting(const serialPort_t *instance)
 247 {
 248     EXPECT_EQ(&serialTestInstance, instance);
 249     EXPECT_GE(serialReadStub.end, serialReadStub.pos);
 250     int ret = serialReadStub.end - serialReadStub.pos;
 251     if (ret < 0) {
 252         ret = 0;
 253     }
 254     //printf("serialRxBytesWaiting: %d\n", ret);
 255     return ret;
 256 }
 257
 258
 259 uint8_t serialRead(serialPort_t *instance)
 260 {
 261     EXPECT_EQ(&serialTestInstance, instance);
 262     EXPECT_LT(serialReadStub.pos, serialReadStub.end);
 263     const uint8_t ch = serialReadStub.buffer[serialReadStub.pos++];
 264     return ch;
 265 }
 266
 267
 268 void serialTestResetBuffers()
 269 {
 270     memset(&serialReadStub, 0, sizeof(serialReadStub));
 271     memset(&serialWriteStub, 0, sizeof(serialWriteStub));
 272 }
 273
 274 void setTestSensors()
 275 {
 276     telemetryConfig_System.flysky_sensors[0] = 0x03;
 277     telemetryConfig_System.flysky_sensors[1] = 0x01;
 278     telemetryConfig_System.flysky_sensors[2] = 0x02;
 279     telemetryConfig_System.flysky_sensors[3] = 0x00;
 280 }
 281
 282 void serialTestResetPort()
 283 {
 284     portIsShared = false;
 285     openSerial_called = false;
 286     determinePortSharing_stub_retval = PORTSHARING_UNUSED;
 287     telemetryDetermineEnabledState_stub_retval = true;
 288
 289     serialTestResetBuffers();
 290 }
 291
 292
 293
 294 class IbusTelemteryInitUnitTest : public ::testing::Test
 295 {
 296 protected:
 297     virtual void SetUp()
 298     {
 299         serialTestResetPort();
 300         setTestSensors();
 301     }
 302 };
 303
 304
 305 TEST_F(IbusTelemteryInitUnitTest, Test_IbusInitNotEnabled)
 306 {
 307     findSerialPortConfig_stub_retval = NULL;
 308     telemetryDetermineEnabledState_stub_retval = false;
 309
 310     //given stuff in serial read
 311     serialReadStub.end++;
 312
 313     //when initializing and polling ibus
 314     initIbusTelemetry();
 315     checkIbusTelemetryState();
 316     handleIbusTelemetry();
 317
 318     //then nothing is read from serial port
 319     EXPECT_NE(serialReadStub.pos, serialReadStub.end);
 320     EXPECT_FALSE(openSerial_called);
 321 }
 322
 323
 324 TEST_F(IbusTelemteryInitUnitTest, Test_IbusInitEnabled)
 325 {
 326     findSerialPortConfig_stub_retval = &serialTestInstanceConfig;
 327
 328     //given stuff in serial read
 329     serialReadStub.end++;
 330
 331     //when initializing and polling ibus
 332     initIbusTelemetry();
 333     checkIbusTelemetryState();
 334     handleIbusTelemetry();
 335
 336     //then all is read from serial port
 337     EXPECT_EQ(serialReadStub.end, serialReadStub.pos);
 338     EXPECT_TRUE(openSerial_called);
 339 }
 340
 341
 342 TEST_F(IbusTelemteryInitUnitTest, Test_IbusInitSerialRxAndTelemetryEnabled)
 343 {
 344     findSerialPortConfig_stub_retval = &serialTestInstanceConfig;
 345
 346     //given stuff in serial read
 347     serialReadStub.end++;
 348     //and serial rx enabled too
 349     portIsShared = true;
 350
 351     //when initializing and polling ibus
 352     initIbusTelemetry();
 353     checkIbusTelemetryState();
 354     handleIbusTelemetry();
 355
 356     //then all is read from serial port
 357     EXPECT_NE(serialReadStub.pos, serialReadStub.end);
 358     EXPECT_FALSE(openSerial_called);
 359 }
 360
 361 class IbusTelemetryProtocolUnitTestBase : public ::testing::Test
 362 {
 363 protected:
 364     virtual void SetUp()
 365     {
 366         serialTestResetPort();
 367         telemetryConfigMutable()->report_cell_voltage = false;
 368         serialTestResetBuffers();
 369         initIbusTelemetry();
 370         checkIbusTelemetryState();
 371     }
 372
 373     void checkResponseToCommand(const char *rx, uint8_t rxCnt, const char *expectedTx, uint8_t expectedTxCnt)
 374     {
 375         serialTestResetBuffers();
 376
 377         memcpy(serialReadStub.buffer, rx, rxCnt);
 378         serialReadStub.end += rxCnt;
 379
 380         //when polling ibus
 381         for (int i = 0; i<10; i++) {
 382             handleIbusTelemetry();
 383         }
 384
 385         EXPECT_EQ(expectedTxCnt, serialWriteStub.pos);
 386         EXPECT_EQ(0, memcmp(serialWriteStub.buffer, expectedTx, expectedTxCnt));
 387     }
 388
 389     void setupBaseAddressOne(void)
 390     {
 391         checkResponseToCommand("\x04\x81\x7a\xff", 4, "\x04\x81\x7a\xff", 4);
 392         serialTestResetBuffers();
 393     }
 394
 395     void setupBaseAddressThree(void)
 396     {
 397         checkResponseToCommand("\x04\x83\x78\xff", 4, "\x04\x83\x78\xff", 4);
 398         serialTestResetBuffers();
 399     }
 400 };
 401
 402
 403
 404 class IbusTelemteryProtocolUnitTest : public ::IbusTelemetryProtocolUnitTestBase
 405 {
 406 protected:
 407     virtual void SetUp()
 408     {
 409         IbusTelemetryProtocolUnitTestBase::SetUp();
 410         setupBaseAddressOne();
 411     }
 412 };
 413
 414
 415 TEST_F(IbusTelemteryProtocolUnitTest, Test_IbusNoRespondToDiscoveryCrcErr)
 416 {
 417     //Given ibus command: Hello sensor at address 1, are you there (with bad crc)?
 418     //then we do not respond
 419     checkResponseToCommand("\x04\x81\x00\x00", 4, NULL, 0);
 420 }
 421
 422
 423 TEST_F(IbusTelemteryProtocolUnitTest, Test_IbusRespondToDiscovery)
 424 {
 425     //Given ibus command: Hello sensor at address 1, are you there?
 426     //then we respond with: Yes, i'm here, hello!
 427     checkResponseToCommand("\x04\x81\x7a\xff", 4, "\x04\x81\x7A\xFF", 4);
 428 }
 429
 430
 431 TEST_F(IbusTelemteryProtocolUnitTest, Test_IbusRespondToSensorTypeQueryVbatt)
 432 {
 433     //Given ibus command: Sensor at address 1, what type are you?
 434     //then we respond with: I'm a voltage sensor
 435     checkResponseToCommand("\x04\x91\x6A\xFF", 4, "\x06\x91\x03\x02\x63\xFF", 6);
 436 }
 437
 438
 439 TEST_F(IbusTelemteryProtocolUnitTest, Test_IbusRespondToSensorTypeQueryTemperature)
 440 {
 441     //Given ibus command: Sensor at address 1, what type are you?
 442     //then we respond with: I'm a thermometer
 443     checkResponseToCommand("\x04\x92\x69\xFF", 4, "\x06\x92\x01\x02\x64\xFF", 6);
 444 }
 445
 446
 447 TEST_F(IbusTelemteryProtocolUnitTest, Test_IbusRespondToSensorTypeQueryRpm)
 448 {
 449     //Given ibus command: Sensor at address 3, what type are you?
 450     //then we respond with: I'm a rpm sensor
 451     checkResponseToCommand("\x04\x93\x68\xFF", 4, "\x06\x93\x02\x02\x62\xFF", 6);
 452 }
 453
 454
 455 TEST_F(IbusTelemteryProtocolUnitTest, Test_IbusRespondToGetMeasurementVbattZero)
 456 {
 457     //Given ibus command: Sensor at address 1, please send your measurement
 458     //then we respond with: I'm reading 0 volts
 459     testBatteryVoltage = 0;
 460     checkResponseToCommand("\x04\xA1\x5a\xff", 4, "\x06\xA1\x00\x00\x58\xFF", 6);
 461 }
 462
 463 TEST_F(IbusTelemteryProtocolUnitTest, Test_IbusRespondToGetMeasurementVbattCellVoltage)
 464 {
 465     telemetryConfigMutable()->report_cell_voltage = true;
 466
 467     //Given ibus command: Sensor at address 1, please send your measurement
 468     //then we respond with: I'm reading 0.1 volts
 469     testBatteryCellCount =3;
 470     testBatteryVoltage = 300;
 471     checkResponseToCommand("\x04\xA1\x5a\xff", 4, "\x06\xA1\x64\x00\xf4\xFe", 6);
 472
 473     //Given ibus command: Sensor at address 1, please send your measurement
 474     //then we respond with: I'm reading 0.1 volts
 475     testBatteryCellCount =1;
 476     testBatteryVoltage = 100;
 477     checkResponseToCommand("\x04\xA1\x5a\xff", 4, "\x06\xA1\x64\x00\xf4\xFe", 6);
 478 }
 479
 480 TEST_F(IbusTelemteryProtocolUnitTest, Test_IbusRespondToGetMeasurementVbattPackVoltage)
 481 {
 482     telemetryConfigMutable()->report_cell_voltage = false;
 483
 484     //Given ibus command: Sensor at address 1, please send your measurement
 485     //then we respond with: I'm reading 0.1 volts
 486     testBatteryCellCount =3;
 487     testBatteryVoltage = 100;
 488     checkResponseToCommand("\x04\xA1\x5a\xff", 4, "\x06\xA1\x64\x00\xf4\xFe", 6);
 489
 490     //Given ibus command: Sensor at address 1, please send your measurement
 491     //then we respond with: I'm reading 0.1 volts
 492     testBatteryCellCount =1;
 493     testBatteryVoltage = 100;
 494     checkResponseToCommand("\x04\xA1\x5a\xff", 4, "\x06\xA1\x64\x00\xf4\xFe", 6);
 495 }
 496
 497
 498 TEST_F(IbusTelemteryProtocolUnitTest, Test_IbusRespondToGetMeasurementTemperature)
 499 {
 500     //Given ibus command: Sensor at address 2, please send your measurement
 501     //then we respond
 502     gyroTemperature = 50;
 503     checkResponseToCommand("\x04\xA2\x59\xff", 4, "\x06\xA2\x84\x03\xd0\xfe", 6);
 504
 505     //Given ibus command: Sensor at address 2, please send your measurement
 506     //then we respond
 507     gyroTemperature = 59;  //test integer rounding
 508     checkResponseToCommand("\x04\xA2\x59\xff", 4, "\x06\xA2\xde\x03\x76\xfe", 6);
 509
 510     //Given ibus command: Sensor at address 2, please send your measurement
 511     //then we respond
 512     gyroTemperature = 150;
 513     checkResponseToCommand("\x04\xA2\x59\xff", 4, "\x06\xA2\x6c\x07\xe4\xfe", 6);
 514 }
 515
 516
 517 TEST_F(IbusTelemteryProtocolUnitTest, Test_IbusRespondToGetMeasurementRpm)
 518 {
 519     //Given ibus command: Sensor at address 3, please send your measurement
 520     //then we respond with: I'm reading 0 rpm
 521     rcCommand[THROTTLE] = 0;
 522     checkResponseToCommand("\x04\xA3\x58\xff", 4, "\x06\xA3\x00\x00\x56\xFF", 6);
 523
 524     //Given ibus command: Sensor at address 3, please send your measurement
 525     //then we respond with: I'm reading 100 rpm
 526     rcCommand[THROTTLE] = 100;
 527     checkResponseToCommand("\x04\xA3\x58\xff", 4, "\x06\xA3\x64\x00\xf2\xFe", 6);
 528 }
 529
 530
 531
 532 class IbusTelemteryProtocolUnitTestDaisyChained : public ::IbusTelemetryProtocolUnitTestBase
 533 {
 534 protected:
 535     virtual void SetUp()
 536     {
 537         IbusTelemetryProtocolUnitTestBase::SetUp();
 538         setupBaseAddressThree();
 539     }
 540 };
 541
 542
 543 TEST_F(IbusTelemteryProtocolUnitTestDaisyChained, Test_IbusRespondToDiscoveryBaseAddressThree)
 544 {
 545     //Given ibus commands: Hello sensor at address 3, 4, 5 are you there?
 546     //then we respond with: Yes, we're here, hello!
 547     checkResponseToCommand("\x04\x83\x78\xff", 4, "\x04\x83\x78\xff", 4);
 548     checkResponseToCommand("\x04\x84\x77\xff", 4, "\x04\x84\x77\xff", 4);
 549     checkResponseToCommand("\x04\x85\x76\xff", 4, "\x04\x85\x76\xff", 4);
 550 }
 551
 552
 553 TEST_F(IbusTelemteryProtocolUnitTestDaisyChained, Test_IbusRespondToSensorTypeQueryWrongAddress)
 554 {
 555     //Given ibus commands: Sensor at address 1, 2, 6, what type are you?
 556     //then we do not respond
 557     checkResponseToCommand("\x04\x91\x6A\xFF", 4, "", 0);
 558     checkResponseToCommand("\x04\x92\x69\xFF", 4, "", 0);
 559
 560     checkResponseToCommand("\x04\x96\x65\xFF", 4, "", 0);
 561 }
 562
 563
 564 TEST_F(IbusTelemteryProtocolUnitTestDaisyChained, Test_IbusRespondToSensorTypeQueryVbattBaseThree)
 565 {
 566     //Given ibus commands: Sensor at address 3, 4, 5, what type are you?
 567     //then we respond with: I'm a voltage sensor
 568     checkResponseToCommand("\x04\x93\x68\xFF", 4, "\x06\x93\x03\x02\x61\xFF", 6);
 569     //then we respond with: I'm a thermometer
 570     checkResponseToCommand("\x04\x94\x67\xFF", 4, "\x06\x94\x01\x02\x62\xFF", 6);
 571     //then we respond with: I'm a rpm sensor
 572     checkResponseToCommand("\x04\x95\x66\xFF", 4, "\x06\x95\x02\x02\x60\xFF", 6);
 573 }
 574
 575
 576 TEST_F(IbusTelemteryProtocolUnitTestDaisyChained, Test_IbusRespondToGetMeasurementsBaseThree)
 577 {
 578     //Given ibus command: Sensor at address 3, please send your measurement
 579     //then we respond with: I'm reading 0.1 volts
 580     testBatteryCellCount = 1;
 581     testBatteryVoltage = 100;
 582     checkResponseToCommand("\x04\xA3\x58\xff", 4, "\x06\xA3\x64\x00\xf2\xfe", 6);
 583
 584     //Given ibus command: Sensor at address 4, please send your measurement
 585     //then we respond
 586     gyroTemperature = 150;
 587     checkResponseToCommand("\x04\xA4\x57\xff", 4, "\x06\xA4\x6c\x07\xe2\xfe", 6);
 588
 589     //Given ibus command: Sensor at address 5, please send your measurement
 590     //then we respond with: I'm reading 100 rpm
 591     rcCommand[THROTTLE] = 100;
 592     checkResponseToCommand("\x04\xA5\x56\xff", 4, "\x06\xA5\x64\x00\xf0\xFe", 6);
 593 }