Center ARMED message and stats (#12086)
[betaflight.git] / src / test / unit / telemetry_crsf_msp_unittest.cc
blob633a58e980c6d98f056f1456a6c29fd8517e971a
1 /*
2 * This file is part of Cleanflight.
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.
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.
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/>.
18 #include <stdint.h>
19 #include <stdbool.h>
20 #include <string.h>
22 #include <limits.h>
23 #include <algorithm>
25 extern "C" {
26 #include <platform.h>
28 #include "build/debug.h"
29 #include "build/atomic.h"
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"
38 #include "pg/pg.h"
39 #include "pg/pg_ids.h"
40 #include "pg/rx.h"
42 #include "drivers/nvic.h"
43 #include "drivers/serial.h"
44 #include "drivers/system.h"
46 #include "fc/runtime_config.h"
47 #include "config/config.h"
48 #include "flight/imu.h"
50 #include "io/serial.h"
51 #include "io/gps.h"
53 #include "msp/msp.h"
54 #include "msp/msp_serial.h"
56 #include "rx/rx.h"
57 #include "rx/crsf.h"
59 #include "sensors/battery.h"
60 #include "sensors/sensors.h"
62 #include "telemetry/telemetry.h"
63 #include "telemetry/msp_shared.h"
64 #include "telemetry/smartport.h"
65 #include "sensors/acceleration.h"
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;
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;
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);
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;
95 uint32_t dummyTimeUs;
99 #include "unittest_macros.h"
100 #include "gtest/gtest.h"
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;
111 const uint8_t crsfPidRequest[] = {
112 0x00,0x0D,0x7A,0xC8,0xEA,0x30,0x00,0x70,0x70,0x00,0x00,0x00,0x00,0x69
115 TEST(CrossFireMSPTest, RequestBufferTest)
117 atomic_BASEPRI = 0;
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);
136 TEST(CrossFireMSPTest, ResponsePacketTest)
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));
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};
157 TEST(CrossFireMSPTest, WriteResponseTest)
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]);
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]);
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]);
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]);
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]);
227 void testSendMspResponse(uint8_t *payload, const uint8_t )
229 sbuf_t *plOut = sbufInit(&payloadOutputBuf, payloadOutput, payloadOutput + 64);
230 sbufWriteData(plOut, payload, *payload + 64);
231 sbufSwitchToReader(&payloadOutputBuf, payloadOutput);
234 TEST(CrossFireMSPTest, SendMspReply)
236 initSharedMsp();
237 const crsfMspFrame_t *framePtr = (const crsfMspFrame_t*)crsfPidRequest;
238 crsfFrame = *(const crsfFrame_t*)framePtr;
239 crsfFrameDone = true;
240 uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
241 bool handled = handleMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL);
242 EXPECT_TRUE(handled);
243 bool replyPending = sendMspReply(64, &testSendMspResponse);
244 EXPECT_FALSE(replyPending);
245 EXPECT_EQ(0x30, sbufReadU8(&payloadOutputBuf)); // status (MSPv1 + #0)
246 EXPECT_EQ(0x1E, sbufReadU8(&payloadOutputBuf)); // payload size
247 EXPECT_EQ(0x70, sbufReadU8(&payloadOutputBuf)); // function ID
248 for (unsigned int ii=1; ii<=30; ii++) {
249 EXPECT_EQ(ii, sbufReadU8(&payloadOutputBuf));
253 // STUBS
255 extern "C" {
257 gpsSolutionData_t gpsSol;
258 attitudeEulerAngles_t attitude = { { 0, 0, 0 } };
259 uint8_t responseBuffer[MSP_TLM_OUTBUF_SIZE];
261 uint32_t micros(void) {return dummyTimeUs;}
262 uint32_t microsISR(void) {return micros();}
263 serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, serialReceiveCallbackPtr, void *, uint32_t, portMode_e, portOptions_e) {return NULL;}
264 const serialPortConfig_t *findSerialPortConfig(serialPortFunction_e ) {return NULL;}
265 bool isBatteryVoltageConfigured(void) { return true; }
266 uint16_t getBatteryVoltage(void) {
267 return testBatteryVoltage;
269 uint16_t getLegacyBatteryVoltage(void) {
270 return (testBatteryVoltage + 5) / 10;
272 uint16_t getBatteryAverageCellVoltage(void) {
273 return 0;
275 bool isAmperageConfigured(void) { return true; }
276 int32_t getAmperage(void) {
277 return testAmperage;
280 uint8_t calculateBatteryPercentageRemaining(void) {
281 return 67;
284 int32_t getEstimatedAltitudeCm(void) {
285 return 0;
288 bool featureIsEnabled(uint32_t) {return false;}
290 bool airmodeIsEnabled(void) {return true;}
292 mspDescriptor_t mspDescriptorAlloc(void) {return 0;}
294 mspResult_e mspFcProcessCommand(mspDescriptor_t srcDesc, mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn) {
296 UNUSED(srcDesc);
297 UNUSED(mspPostProcessFn);
299 sbuf_t *dst = &reply->buf;
300 const uint8_t cmdMSP = cmd->cmd;
301 reply->cmd = cmd->cmd;
303 if (cmdMSP == 0x70) {
304 for (unsigned int ii=1; ii<=30; ii++) {
305 sbufWriteU8(dst, ii);
307 } else if (cmdMSP == 0xCA) {
308 return MSP_RESULT_ACK;
311 return MSP_RESULT_ACK;
314 void beeperConfirmationBeeps(uint8_t ) {}
316 int32_t getMAhDrawn(void) {
317 return testmAhDrawn;
320 bool telemetryIsSensorEnabled(sensor_e) {
321 return true;
324 timeUs_t rxFrameTimeUs(void) { return 0; }