src/test/unit/serial_msp_unittest.cc.txt

   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 #include <math.h>
  21
  22 extern "C" {
  23     #include <platform.h>
  24     #include "build_config.h"
  25     #include "version.h"
  26     #include "debug.h"
  27     #include "common/axis.h"
  28     #include "common/color.h"
  29     #include "common/maths.h"
  30
  31     #include "drivers/system.h"
  32     #include "drivers/sensor.h"
  33     #include "drivers/accgyro.h"
  34     #include "drivers/compass.h"
  35     #include "drivers/serial.h"
  36     #include "drivers/bus_i2c.h"
  37     #include "drivers/timer.h"
  38     #include "drivers/pwm_rx.h"
  39     #include "drivers/buf_writer.h"
  40
  41     #include "rx/rx.h"
  42
  43     #include "io/rc_controls.h"
  44     #include "io/gps.h"
  45     #include "io/ledstrip.h"
  46     #include "io/serial_msp.h"
  47     #include "io/escservo.h"
  48
  49     #include "telemetry/telemetry.h"
  50
  51     #include "sensors/sensors.h"
  52     #include "sensors/boardalignment.h"
  53     #include "sensors/sensors.h"
  54     #include "sensors/battery.h"
  55     #include "sensors/sonar.h"
  56     #include "sensors/acceleration.h"
  57     #include "sensors/barometer.h"
  58     #include "sensors/compass.h"
  59     #include "sensors/gyro.h"
  60
  61     #include "flight/mixer.h"
  62     #include "flight/pid.h"
  63     #include "flight/navigation.h"
  64     #include "flight/imu.h"
  65     #include "flight/failsafe.h"
  66
  67     #include "config/runtime_config.h"
  68     #include "config/config.h"
  69     #include "config/config_profile.h"
  70     #include "config/config_master.h"
  71 }
  72
  73 #include "unittest_macros.h"
  74 #include "gtest/gtest.h"
  75
  76
  77 extern "C" {
  78     void setCurrentPort(mspPort_t *port);
  79     void mspProcessReceivedCommand();
  80     extern mspPort_t *currentPort;
  81     extern bufWriter_t *writer;
  82     extern mspPort_t mspPorts[];
  83     profile_t *currentProfile;
  84 }
  85
  86 profile_t profile;
  87
  88 typedef struct mspHeader_s {
  89     uint8_t dollar;
  90     uint8_t m;
  91     uint8_t direction;
  92     uint8_t size;
  93     uint8_t type;
  94 } mspHeader_t;
  95
  96 typedef struct mspResonse_s {
  97     mspHeader_t header;
  98     uint8_t payload[];
  99 } mspResponse_t;
 100
 101 #define SERIAL_BUFFER_SIZE 256
 102 typedef union mspBuffer_u {
 103     mspResponse_t mspResponse;
 104     uint8_t buf[SERIAL_BUFFER_SIZE];
 105 } mspBuffer_t;
 106
 107 static mspBuffer_t serialBuffer;
 108
 109 static int serialWritePos = 0;
 110 static int serialReadPos = 0;
 111
 112 uint8_t buf[sizeof(bufWriter_t) + SERIAL_BUFFER_SIZE];
 113
 114 void serialWrite(serialPort_t *instance, uint8_t ch)
 115 {
 116     UNUSED(instance);
 117     serialBuffer.buf[serialWritePos] = ch;
 118     ++serialWritePos;
 119 }
 120
 121 void serialWriteBufShim(void *instance, uint8_t *data, int count)
 122 {
 123     for (uint8_t *p = data; count > 0; count--, p++) {
 124         serialWrite((serialPort_t *)instance, *p);
 125     }
 126 }
 127
 128 void serialBeginWrite(serialPort_t *instance)
 129 {
 130     UNUSED(instance);
 131 }
 132
 133 void serialEndWrite(serialPort_t *instance)
 134 {
 135     UNUSED(instance);
 136 }
 137
 138 uint8_t serialRxBytesWaiting(serialPort_t *instance)
 139 {
 140     UNUSED(instance);
 141     if (serialWritePos > serialReadPos) {
 142         return serialWritePos - serialReadPos;
 143     } else {
 144         return 0;
 145     }
 146 }
 147
 148 uint8_t serialRead(serialPort_t *instance)
 149 {
 150     UNUSED(instance);
 151     const uint8_t ch = serialBuffer.buf[serialReadPos];
 152     ++serialReadPos;
 153     if (currentPort->indRX == MSP_PORT_INBUF_SIZE) {
 154         currentPort->indRX = 0;
 155     }
 156     currentPort->inBuf[currentPort->indRX] = ch;
 157     ++currentPort->indRX;
 158     return ch;
 159 }
 160
 161 bool isSerialTransmitBufferEmpty(serialPort_t *instance)
 162 {
 163     UNUSED(instance);
 164     return true;
 165 }
 166
 167 class SerialMspUnitTest : public ::testing::Test {
 168 protected:
 169     virtual void SetUp() {
 170         memset(serialBuffer.buf, 0, sizeof(serialBuffer));
 171         setCurrentPort(&mspPorts[0]);
 172         writer = bufWriterInit(buf, sizeof(buf), (bufWrite_t)serialWriteBufShim, &mspPorts[0]);
 173     }
 174 };
 175
 176
 177 TEST_F(SerialMspUnitTest, TestMspProcessReceivedCommand)
 178 {
 179     // check the MSP_API_VERSION is written out correctly
 180     serialWritePos = 0;
 181     serialReadPos = 0;
 182     currentPort->cmdMSP = MSP_API_VERSION;
 183     mspProcessReceivedCommand();
 184
 185     EXPECT_EQ('$', serialBuffer.mspResponse.header.dollar);
 186     EXPECT_EQ('M', serialBuffer.mspResponse.header.m);
 187     EXPECT_EQ('>', serialBuffer.mspResponse.header.direction);
 188     EXPECT_EQ(3, serialBuffer.mspResponse.header.size);
 189     EXPECT_EQ(MSP_API_VERSION, serialBuffer.mspResponse.header.type);
 190     EXPECT_EQ(MSP_PROTOCOL_VERSION, serialBuffer.mspResponse.payload[0]);
 191     EXPECT_EQ(API_VERSION_MAJOR, serialBuffer.mspResponse.payload[1]);
 192     EXPECT_EQ(API_VERSION_MINOR, serialBuffer.mspResponse.payload[2]);
 193     int checksum = 3 ^ MSP_API_VERSION;
 194     checksum ^= MSP_PROTOCOL_VERSION ^ API_VERSION_MAJOR ^ API_VERSION_MINOR;
 195     EXPECT_EQ(checksum, serialBuffer.mspResponse.payload[3]);// checksum
 196
 197     // check the MSP_FC_VARIANT is written out correctly
 198     serialWritePos = 0;
 199     serialReadPos = 0;
 200     currentPort->cmdMSP = MSP_FC_VARIANT;
 201     mspProcessReceivedCommand();
 202
 203     EXPECT_EQ('$', serialBuffer.mspResponse.header.dollar);
 204     EXPECT_EQ('M', serialBuffer.mspResponse.header.m);
 205     EXPECT_EQ('>', serialBuffer.mspResponse.header.direction);
 206     EXPECT_EQ(FLIGHT_CONTROLLER_IDENTIFIER_LENGTH, serialBuffer.mspResponse.header.size);
 207     EXPECT_EQ(MSP_FC_VARIANT, serialBuffer.mspResponse.header.type);
 208     EXPECT_EQ('C', serialBuffer.mspResponse.payload[0]);
 209     EXPECT_EQ('L', serialBuffer.mspResponse.payload[1]);
 210     EXPECT_EQ('F', serialBuffer.mspResponse.payload[2]);
 211     EXPECT_EQ('L', serialBuffer.mspResponse.payload[3]);
 212     checksum = FLIGHT_CONTROLLER_IDENTIFIER_LENGTH ^ MSP_FC_VARIANT;
 213     checksum ^= 'C'^ 'L' ^ 'F' ^ 'L';
 214     EXPECT_EQ(checksum, serialBuffer.mspResponse.payload[4]);
 215
 216     // check the MSP_FC_VERSION is written out correctly
 217     serialWritePos = 0;
 218     serialReadPos = 0;
 219     currentPort->cmdMSP = MSP_FC_VERSION;
 220     mspProcessReceivedCommand();
 221     EXPECT_EQ('$', serialBuffer.mspResponse.header.dollar);
 222     EXPECT_EQ('M', serialBuffer.mspResponse.header.m);
 223     EXPECT_EQ('>', serialBuffer.mspResponse.header.direction);
 224     EXPECT_EQ(FLIGHT_CONTROLLER_VERSION_LENGTH, serialBuffer.mspResponse.header.size);
 225     EXPECT_EQ(MSP_FC_VERSION, serialBuffer.mspResponse.header.type);
 226     EXPECT_EQ(FC_VERSION_MAJOR, serialBuffer.mspResponse.payload[0]);
 227     EXPECT_EQ(FC_VERSION_MINOR, serialBuffer.mspResponse.payload[1]);
 228     EXPECT_EQ(FC_VERSION_PATCH_LEVEL, serialBuffer.mspResponse.payload[2]);
 229     checksum = FLIGHT_CONTROLLER_VERSION_LENGTH ^ MSP_FC_VERSION;
 230     checksum ^= FC_VERSION_MAJOR ^ FC_VERSION_MINOR ^ FC_VERSION_PATCH_LEVEL;
 231     EXPECT_EQ(checksum, serialBuffer.mspResponse.payload[3]);
 232 }
 233
 234
 235 // STUBS
 236 extern "C" {
 237 // from acceleration.c
 238 uint16_t acc_1G = 256;          // this is the 1G measured acceleration.
 239 void accSetCalibrationCycles(uint16_t calibrationCyclesRequired) {UNUSED(calibrationCyclesRequired);}
 240 // from altitudehold.c
 241 int32_t AltHold;
 242 int32_t vario = 0;                      // variometer in cm/s
 243 int32_t altitudeHoldGetEstimatedAltitude(void) {return 0;}
 244 // from battery.c
 245 uint16_t vbat = 0;                   // battery voltage in 0.1V steps (filtered)
 246 int32_t amperage = 0;               // amperage read by current sensor in centiampere (1/100th A)
 247 int32_t mAhDrawn = 0;               // milliampere hours drawn from the battery since start
 248 // from compass.c
 249 int32_t magADC[XYZ_AXIS_COUNT];
 250 // from config.c
 251 master_t masterConfig;
 252 controlRateConfig_t *currentControlRateProfile;
 253 void resetPidProfile(pidProfile_t *pidProfile) {UNUSED(pidProfile);}
 254 void handleOneshotFeatureChangeOnRestart(void) {}
 255 void readEEPROM(void) {}
 256 void resetEEPROM(void) {}
 257 void writeEEPROM(void) {}
 258 bool feature(uint32_t mask) {UNUSED(mask);return false;}
 259 void featureSet(uint32_t mask) {UNUSED(mask);}
 260 void featureClearAll() {}
 261 uint32_t featureMask(void) {return 0;}
 262 // from debug.c
 263 int32_t debug[DEBUG32_VALUE_COUNT];
 264 // from gps.c
 265 #define GPS_SV_MAXSATS   16
 266 int32_t GPS_coord[2];               // LAT/LON
 267 uint8_t GPS_numSat;
 268 uint8_t GPS_update = 0;             // it's a binary toggle to distinct a GPS position update
 269 uint16_t GPS_altitude;              // altitude in 0.1m
 270 uint16_t GPS_speed;                 // speed in 0.1m/s
 271 uint16_t GPS_ground_course = 0;     // degrees * 10
 272 uint8_t GPS_numCh;                          // Number of channels
 273 uint8_t GPS_svinfo_chn[GPS_SV_MAXSATS];     // Channel number
 274 uint8_t GPS_svinfo_svid[GPS_SV_MAXSATS];    // Satellite ID
 275 uint8_t GPS_svinfo_quality[GPS_SV_MAXSATS]; // Bitfield Qualtity
 276 uint8_t GPS_svinfo_cno[GPS_SV_MAXSATS];     // Carrier to Noise Ratio (Signal Strength)
 277 // from gyro.c
 278 int32_t gyroADC[XYZ_AXIS_COUNT];
 279 // form imu.c
 280 attitudeEulerAngles_t attitude = { { 0, 0, 0 } };     // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
 281 int16_t accSmooth[XYZ_AXIS_COUNT];
 282 // from ledstrip.c
 283 void reevalulateLedConfig(void) {}
 284 // from mixer.c
 285 int16_t motor[MAX_SUPPORTED_MOTORS];
 286 int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
 287 int16_t servo[MAX_SUPPORTED_SERVOS];
 288 void stopMotors(void) {}
 289 void loadCustomServoMixer(void) {}
 290 // from msp.c
 291 void rxMspFrameReceive(uint16_t *frame, int channelCount) {UNUSED(frame);UNUSED(channelCount);}
 292 // from mw.c
 293 uint16_t cycleTime = 0;         // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
 294 // from navigation.c
 295 int32_t GPS_home[2];
 296 int32_t GPS_hold[2];
 297 uint32_t GPS_distanceToHome;        // distance to home point in meters
 298 int16_t GPS_directionToHome;        // direction to home or hol point in degrees
 299 navigationMode_e nav_mode = NAV_MODE_NONE;    // Navigation mode
 300 void GPS_set_next_wp(int32_t *lat, int32_t *lon) {UNUSED(lat);UNUSED(lon);}
 301 // from pid.c
 302 void pidSetController(pidControllerType_e type) {UNUSED(type);}
 303 // from rc_controls.c
 304 uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e
 305 void useRcControlsConfig(void *modeActivationConditions, void *escAndServoConfigToUse, void *pidProfileToUse) {
 306     UNUSED(modeActivationConditions);UNUSED(escAndServoConfigToUse);UNUSED(pidProfileToUse);}
 307 // from runtime_config.c
 308 uint8_t armingFlags = 0;
 309 uint8_t stateFlags = 0;
 310 uint16_t flightModeFlags = 0;
 311 uint16_t disableFlightMode(flightModeFlags_e mask) {UNUSED(mask);return 0;}
 312 bool sensors(uint32_t mask) {UNUSED(mask);return 0;}
 313 // from rx.c
 314 uint16_t rssi = 0;                  // range: [0;1023]
 315 int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];     // interval [1000;2000]
 316 rxRuntimeConfig_t rxRuntimeConfig;
 317 // from system.c
 318 void delay(uint32_t ms) {UNUSED(ms);}
 319 // from system_stm32fN0x.c
 320 void systemReset(void) {}
 321 void systemResetToBootloader(void) {}
 322 // from scheduler.c
 323 uint16_t averageSystemLoadPercent = 0;
 324 // from transponder_ir.c
 325 void transponderUpdateData(uint8_t*) {}
 326 }