src/test/unit/telemetry_crsf_msp_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 <string.h>
  21
  22 #include <limits.h>
  23 #include <algorithm>
  24
  25 extern "C" {
  26     #include <platform.h>
  27
  28     #include "build/debug.h"
  29     #include "build/atomic.h"
  30
  31     #include "common/crc.h"
  32     #include "common/utils.h"
  33     #include "common/printf.h"
  34     #include "common/gps_conversion.h"
  35     #include "common/streambuf.h"
  36     #include "common/typeconversion.h"
  37
  38     #include "pg/pg.h"
  39     #include "pg/pg_ids.h"
  40     #include "pg/rx.h"
  41
  42     #include "drivers/nvic.h"
  43     #include "drivers/serial.h"
  44     #include "drivers/system.h"
  45
  46     #include "fc/runtime_config.h"
  47     #include "config/config.h"
  48     #include "flight/imu.h"
  49
  50     #include "io/serial.h"
  51     #include "io/gps.h"
  52
  53     #include "msp/msp.h"
  54     #include "msp/msp_serial.h"
  55
  56     #include "rx/rx.h"
  57     #include "rx/crsf.h"
  58
  59     #include "sensors/battery.h"
  60     #include "sensors/sensors.h"
  61
  62     #include "telemetry/telemetry.h"
  63     #include "telemetry/msp_shared.h"
  64     #include "telemetry/smartport.h"
  65     #include "sensors/acceleration.h"
  66
  67     rssiSource_e rssiSource;
  68     bool handleMspFrame(uint8_t *frameStart, uint8_t frameLength, uint8_t *skipsBeforeResponse);
  69     bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn);
  70     uint8_t sbufReadU8(sbuf_t *src);
  71     int sbufBytesRemaining(sbuf_t *buf);
  72     void initSharedMsp();
  73     uint16_t testBatteryVoltage = 0;
  74
  75     int32_t testAmperage = 0;
  76     uint8_t mspTxData[64]; // max frame size
  77     sbuf_t mspTxDataBuf;
  78     uint8_t crsfFrameOut[CRSF_FRAME_SIZE_MAX];
  79     uint8_t payloadOutput[64];
  80     sbuf_t payloadOutputBuf;
  81     int32_t testmAhDrawn = 0;
  82
  83     PG_REGISTER(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0);
  84     PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0);
  85     PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0);
  86     PG_REGISTER(rxConfig_t, rxConfig, PG_RX_CONFIG, 0);
  87     PG_REGISTER(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG,0);
  88
  89     extern bool crsfFrameDone;
  90     extern crsfFrame_t crsfFrame;
  91     extern uint8_t requestBuffer[MSP_TLM_INBUF_SIZE];
  92     extern struct mspPacket_s requestPacket;
  93     extern struct mspPacket_s responsePacket;
  94
  95     uint32_t dummyTimeUs;
  96
  97 }
  98
  99 #include "unittest_macros.h"
 100 #include "gtest/gtest.h"
 101
 102 typedef struct crsfMspFrame_s {
 103     uint8_t deviceAddress;
 104     uint8_t frameLength;
 105     uint8_t type;
 106     uint8_t destination;
 107     uint8_t origin;
 108     uint8_t payload[CRSF_FRAME_RX_MSP_FRAME_SIZE + CRSF_FRAME_LENGTH_CRC];
 109 } crsfMspFrame_t;
 110
 111 const uint8_t crsfPidRequest[] = {
 112     0x00,0x0D,0x7A,0xC8,0xEA,0x30,0x00,0x70,0x70,0x00,0x00,0x00,0x00,0x69
 113 };
 114
 115 TEST(CrossFireMSPTest, RequestBufferTest)
 116 {
 117     atomic_BASEPRI = 0;
 118
 119     initSharedMsp();
 120     const crsfMspFrame_t *framePtr = (const crsfMspFrame_t*)crsfPidRequest;
 121     crsfFrame = *(const crsfFrame_t*)framePtr;
 122     crsfFrameDone = true;
 123     EXPECT_EQ(CRSF_ADDRESS_BROADCAST, crsfFrame.frame.deviceAddress);
 124     EXPECT_EQ(CRSF_FRAME_LENGTH_ADDRESS + CRSF_FRAME_LENGTH_EXT_TYPE_CRC + CRSF_FRAME_RX_MSP_FRAME_SIZE, crsfFrame.frame.frameLength);
 125     EXPECT_EQ(CRSF_FRAMETYPE_MSP_REQ, crsfFrame.frame.type);
 126     uint8_t *destination = (uint8_t *)&crsfFrame.frame.payload;
 127     uint8_t *origin = (uint8_t *)&crsfFrame.frame.payload + 1;
 128     uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
 129     uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_FRAME_SIZE + 2;
 130     EXPECT_EQ(0xC8, *destination);
 131     EXPECT_EQ(0xEA, *origin);
 132     EXPECT_EQ(0x30, *frameStart);
 133     EXPECT_EQ(0x69, *frameEnd);
 134 }
 135
 136 TEST(CrossFireMSPTest, ResponsePacketTest)
 137 {
 138     initSharedMsp();
 139     const crsfMspFrame_t *framePtr = (const crsfMspFrame_t*)crsfPidRequest;
 140     crsfFrame = *(const crsfFrame_t*)framePtr;
 141     crsfFrameDone = true;
 142     uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
 143     handleMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL);
 144     for (unsigned int ii=1; ii<30; ii++) {
 145         EXPECT_EQ(ii, sbufReadU8(&responsePacket.buf));
 146     }
 147 }
 148
 149 //                               |   crsf                     | msp
 150 //                               sync size type to   from stts size fn   0    1    2    3    4
 151 const uint8_t crsfPidWrite1[] = {0x00,0x0C,0x7A,0xC8,0xEA,0x31,0x1E,0xCA,0x29,0x28,0x1E,0x3A,0x32};
 152 const uint8_t crsfPidWrite2[] = {0x00,0x0C,0x7A,0xC8,0xEA,0x22,0x23,0x46,0x2D,0x14,0x32,0x00,0x00};
 153 const uint8_t crsfPidWrite3[] = {0x00,0x0C,0x7A,0xC8,0xEA,0x23,0x0F,0x00,0x00,0x22,0x0E,0x35,0x19};
 154 const uint8_t crsfPidWrite4[] = {0x00,0x0C,0x7A,0xC8,0xEA,0x24,0x21,0x53,0x32,0x32,0x4B,0x28,0x00};
 155 const uint8_t crsfPidWrite5[] = {0x00,0x0C,0x7A,0xC8,0xEA,0x25,0x00,0x37,0x37,0x4B,0xF8,0x00,0x00};
 156
 157 TEST(CrossFireMSPTest, WriteResponseTest)
 158 {
 159     initSharedMsp();
 160     const crsfMspFrame_t *framePtr1 = (const crsfMspFrame_t*)crsfPidWrite1;
 161     crsfFrame = *(const crsfFrame_t*)framePtr1;
 162     crsfFrameDone = true;
 163     uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
 164     bool pending1 = handleMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL);
 165     EXPECT_FALSE(pending1); // not done yet
 166     EXPECT_EQ(0x29, requestBuffer[0]);
 167     EXPECT_EQ(0x28, requestBuffer[1]);
 168     EXPECT_EQ(0x1E, requestBuffer[2]);
 169     EXPECT_EQ(0x3A, requestBuffer[3]);
 170     EXPECT_EQ(0x32, requestBuffer[4]);
 171
 172     const crsfMspFrame_t *framePtr2 = (const crsfMspFrame_t*)crsfPidWrite2;
 173     crsfFrame = *(const crsfFrame_t*)framePtr2;
 174     crsfFrameDone = true;
 175     uint8_t *frameStart2 = (uint8_t *)&crsfFrame.frame.payload + 2;
 176     bool pending2 = handleMspFrame(frameStart2, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL);
 177     EXPECT_FALSE(pending2); // not done yet
 178     EXPECT_EQ(0x23, requestBuffer[5]);
 179     EXPECT_EQ(0x46, requestBuffer[6]);
 180     EXPECT_EQ(0x2D, requestBuffer[7]);
 181     EXPECT_EQ(0x14, requestBuffer[8]);
 182     EXPECT_EQ(0x32, requestBuffer[9]);
 183     EXPECT_EQ(0x00, requestBuffer[10]);
 184     EXPECT_EQ(0x00, requestBuffer[11]);
 185
 186     const crsfMspFrame_t *framePtr3 = (const crsfMspFrame_t*)crsfPidWrite3;
 187     crsfFrame = *(const crsfFrame_t*)framePtr3;
 188     crsfFrameDone = true;
 189     uint8_t *frameStart3 = (uint8_t *)&crsfFrame.frame.payload + 2;
 190     bool pending3 = handleMspFrame(frameStart3, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL);
 191     EXPECT_FALSE(pending3); // not done yet
 192     EXPECT_EQ(0x0F, requestBuffer[12]);
 193     EXPECT_EQ(0x00, requestBuffer[13]);
 194     EXPECT_EQ(0x00, requestBuffer[14]);
 195     EXPECT_EQ(0x22, requestBuffer[15]);
 196     EXPECT_EQ(0x0E, requestBuffer[16]);
 197     EXPECT_EQ(0x35, requestBuffer[17]);
 198     EXPECT_EQ(0x19, requestBuffer[18]);
 199
 200     const crsfMspFrame_t *framePtr4 = (const crsfMspFrame_t*)crsfPidWrite4;
 201     crsfFrame = *(const crsfFrame_t*)framePtr4;
 202     crsfFrameDone = true;
 203     uint8_t *frameStart4 = (uint8_t *)&crsfFrame.frame.payload + 2;
 204     bool pending4 = handleMspFrame(frameStart4, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL);
 205     EXPECT_FALSE(pending4); // not done yet
 206     EXPECT_EQ(0x21, requestBuffer[19]);
 207     EXPECT_EQ(0x53, requestBuffer[20]);
 208     EXPECT_EQ(0x32, requestBuffer[21]);
 209     EXPECT_EQ(0x32, requestBuffer[22]);
 210     EXPECT_EQ(0x4B, requestBuffer[23]);
 211     EXPECT_EQ(0x28, requestBuffer[24]);
 212     EXPECT_EQ(0x00, requestBuffer[25]);
 213
 214     const crsfMspFrame_t *framePtr5 = (const crsfMspFrame_t*)crsfPidWrite5;
 215     crsfFrame = *(const crsfFrame_t*)framePtr5;
 216     crsfFrameDone = true;
 217     uint8_t *frameStart5 = (uint8_t *)&crsfFrame.frame.payload + 2;
 218     bool pending5 = handleMspFrame(frameStart5, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL);
 219     EXPECT_TRUE(pending5); // not done yet
 220     EXPECT_EQ(0x00, requestBuffer[26]);
 221     EXPECT_EQ(0x37, requestBuffer[27]);
 222     EXPECT_EQ(0x37, requestBuffer[28]);
 223     EXPECT_EQ(0x4B, requestBuffer[29]);
 224
 225 }
 226
 227 void testSendMspResponse(uint8_t *payload, const uint8_t ) {
 228     sbuf_t *plOut = sbufInit(&payloadOutputBuf, payloadOutput, payloadOutput + 64);
 229     sbufWriteData(plOut, payload, *payload + 64);
 230     sbufSwitchToReader(&payloadOutputBuf, payloadOutput);
 231 }
 232
 233 TEST(CrossFireMSPTest, SendMspReply) {
 234     initSharedMsp();
 235     const crsfMspFrame_t *framePtr = (const crsfMspFrame_t*)crsfPidRequest;
 236     crsfFrame = *(const crsfFrame_t*)framePtr;
 237     crsfFrameDone = true;
 238     uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
 239     bool handled = handleMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL);
 240     EXPECT_TRUE(handled);
 241     bool replyPending = sendMspReply(64, &testSendMspResponse);
 242     EXPECT_FALSE(replyPending);
 243     EXPECT_EQ(0x30, sbufReadU8(&payloadOutputBuf)); // status (MSPv1 + #0)
 244     EXPECT_EQ(0x1E, sbufReadU8(&payloadOutputBuf)); // payload size
 245     EXPECT_EQ(0x70, sbufReadU8(&payloadOutputBuf)); // function ID
 246     for (unsigned int ii=1; ii<=30; ii++) {
 247         EXPECT_EQ(ii, sbufReadU8(&payloadOutputBuf));
 248     }
 249 }
 250
 251 // STUBS
 252
 253 extern "C" {
 254
 255     gpsSolutionData_t gpsSol;
 256     attitudeEulerAngles_t attitude = { { 0, 0, 0 } };
 257     uint8_t responseBuffer[MSP_TLM_OUTBUF_SIZE];
 258
 259     uint32_t micros(void) {return dummyTimeUs;}
 260     uint32_t microsISR(void) {return micros();}
 261     serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, serialReceiveCallbackPtr, void *, uint32_t, portMode_e, portOptions_e) {return NULL;}
 262     const serialPortConfig_t *findSerialPortConfig(serialPortFunction_e ) {return NULL;}
 263     bool isBatteryVoltageConfigured(void) { return true; }
 264     uint16_t getBatteryVoltage(void) {
 265         return testBatteryVoltage;
 266     }
 267     uint16_t getLegacyBatteryVoltage(void) {
 268         return (testBatteryVoltage + 5) / 10;
 269     }
 270     uint16_t getBatteryAverageCellVoltage(void) {
 271         return 0;
 272     }
 273     bool isAmperageConfigured(void) { return true; }
 274     int32_t getAmperage(void) {
 275         return testAmperage;
 276     }
 277
 278     uint8_t calculateBatteryPercentageRemaining(void) {
 279         return 67;
 280     }
 281
 282     int32_t getEstimatedAltitudeCm(void) {
 283         return 0;
 284     }
 285
 286     bool featureIsEnabled(uint32_t) {return false;}
 287
 288     bool airmodeIsEnabled(void) {return true;}
 289
 290     mspDescriptor_t mspDescriptorAlloc(void) {return 0;}
 291
 292     mspResult_e mspFcProcessCommand(mspDescriptor_t srcDesc, mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn) {
 293
 294         UNUSED(srcDesc);
 295         UNUSED(mspPostProcessFn);
 296
 297         sbuf_t *dst = &reply->buf;
 298         const uint8_t cmdMSP = cmd->cmd;
 299         reply->cmd = cmd->cmd;
 300
 301         if (cmdMSP == 0x70) {
 302             for (unsigned int ii=1; ii<=30; ii++) {
 303                 sbufWriteU8(dst, ii);
 304             }
 305         } else if (cmdMSP == 0xCA) {
 306             return MSP_RESULT_ACK;
 307         }
 308
 309         return MSP_RESULT_ACK;
 310     }
 311
 312     void beeperConfirmationBeeps(uint8_t ) {}
 313
 314     int32_t getMAhDrawn(void) {
 315       return testmAhDrawn;
 316     }
 317
 318     bool telemetryIsSensorEnabled(sensor_e) {
 319         return true;
 320     }
 321
 322     timeUs_t rxFrameTimeUs(void) { return 0; }
 323 }