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/>.
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"
39 #include "pg/pg_ids.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"
54 #include "msp/msp_serial.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
);
73 uint16_t testBatteryVoltage
= 0;
75 int32_t testAmperage
= 0;
76 uint8_t mspTxData
[64]; // max frame size
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
;
99 #include "unittest_macros.h"
100 #include "gtest/gtest.h"
102 typedef struct crsfMspFrame_s
{
103 uint8_t deviceAddress
;
108 uint8_t payload
[CRSF_FRAME_RX_MSP_FRAME_SIZE
+ CRSF_FRAME_LENGTH_CRC
];
111 const uint8_t crsfPidRequest
[] = {
112 0x00,0x0D,0x7A,0xC8,0xEA,0x30,0x00,0x70,0x70,0x00,0x00,0x00,0x00,0x69
115 TEST(CrossFireMSPTest
, RequestBufferTest
)
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
)
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
));
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
)
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
)
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
));
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) {
275 bool isAmperageConfigured(void) { return true; }
276 int32_t getAmperage(void) {
280 uint8_t calculateBatteryPercentageRemaining(void) {
284 int32_t getEstimatedAltitudeCm(void) {
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
) {
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) {
320 bool telemetryIsSensorEnabled(sensor_e
) {
324 timeUs_t
rxFrameTimeUs(void) { return 0; }