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