mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-23 16:25:31 +03:00
Corrected smartport regression issue with msp_shared
Corrected condition where an extra padded zero was added to outgoing msp buffers. Whitespace tidy and removal of unnecessary condition. Commented OSD and CMS from STM32F3DISCOVERY target.h Various improvements to msp_shared (removed byte loops, point arithmetic) Raised schedule priority of CRSF device and MSP calls Reworked buffers in msp_shared to enable writes. Moved msp buffers to msp_shared Added new custom frames and reworking msp implementation Implemented CRSF device info reply Adding crsf ping/pong device info functionality Changed Colibri FC address to Betaflight address Implementing MSP in CRSF Telemetry Decoupled msp functionality from smartport into msp_shared Moved USE_SERVOS to FLASH>128 per mikeller
This commit is contained in:
Curtis Bangert
parent
ab53b178ca
commit
863b24ae6a
17 changed files with 738 additions and 229 deletions
299
src/test/unit/telemetry_crsf_msp_unittest.cc
Normal file
299
src/test/unit/telemetry_crsf_msp_unittest.cc
Normal file
|
|
@ -0,0 +1,299 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* Cleanflight is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <string.h>
|
||||
|
||||
#include <limits.h>
|
||||
#include <algorithm>
|
||||
|
||||
extern "C" {
|
||||
#include <platform.h>
|
||||
|
||||
#include "build/debug.h"
|
||||
|
||||
#include "common/crc.h"
|
||||
#include "common/utils.h"
|
||||
#include "common/printf.h"
|
||||
#include "common/gps_conversion.h"
|
||||
#include "common/streambuf.h"
|
||||
#include "common/typeconversion.h"
|
||||
|
||||
#include "config/parameter_group.h"
|
||||
#include "config/parameter_group_ids.h"
|
||||
|
||||
#include "drivers/serial.h"
|
||||
#include "drivers/system.h"
|
||||
|
||||
#include "fc/runtime_config.h"
|
||||
#include "fc/config.h"
|
||||
#include "flight/imu.h"
|
||||
#include "fc/fc_msp.h"
|
||||
|
||||
#include "io/serial.h"
|
||||
#include "io/gps.h"
|
||||
|
||||
#include "msp/msp.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
#include "rx/crsf.h"
|
||||
|
||||
#include "sensors/battery.h"
|
||||
#include "sensors/sensors.h"
|
||||
|
||||
#include "telemetry/telemetry.h"
|
||||
#include "telemetry/msp_shared.h"
|
||||
#include "telemetry/smartport.h"
|
||||
|
||||
bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd);
|
||||
bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn);
|
||||
uint8_t sbufReadU8(sbuf_t *src);
|
||||
int sbufBytesRemaining(sbuf_t *buf);
|
||||
void initSharedMsp();
|
||||
uint16_t testBatteryVoltage = 0;
|
||||
int32_t testAmperage = 0;
|
||||
uint8_t mspTxData[64]; //max frame size
|
||||
sbuf_t mspTxDataBuf;
|
||||
uint8_t crsfFrameOut[CRSF_FRAME_SIZE_MAX];
|
||||
uint8_t payloadOutput[64];
|
||||
sbuf_t payloadOutputBuf;
|
||||
int32_t testmAhDrawn = 0;
|
||||
|
||||
PG_REGISTER(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0);
|
||||
PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0);
|
||||
PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0);
|
||||
|
||||
extern bool crsfFrameDone;
|
||||
extern crsfFrame_t crsfFrame;
|
||||
extern mspPackage_t mspPackage;
|
||||
extern uint8_t checksum;
|
||||
|
||||
uint32_t dummyTimeUs;
|
||||
|
||||
}
|
||||
|
||||
#include "unittest_macros.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
typedef struct crsfMspFrame_s {
|
||||
uint8_t deviceAddress;
|
||||
uint8_t frameLength;
|
||||
uint8_t type;
|
||||
uint8_t destination;
|
||||
uint8_t origin;
|
||||
uint8_t payload[CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_CRC];
|
||||
} crsfMspFrame_t;
|
||||
|
||||
const uint8_t crsfPidRequest[] = {
|
||||
0x00,0x0D,0x7A,0xC8,0xEA,0x30,0x00,0x70,0x70,0x00,0x00,0x00,0x00,0x69
|
||||
};
|
||||
|
||||
TEST(CrossFireMSPTest, RequestBufferTest)
|
||||
{
|
||||
initSharedMsp();
|
||||
const crsfMspFrame_t *framePtr = (const crsfMspFrame_t*)crsfPidRequest;
|
||||
crsfFrame = *(const crsfFrame_t*)framePtr;
|
||||
crsfFrameDone = true;
|
||||
EXPECT_EQ(CRSF_ADDRESS_BROADCAST, crsfFrame.frame.deviceAddress);
|
||||
EXPECT_EQ(CRSF_FRAME_LENGTH_ADDRESS + CRSF_FRAME_LENGTH_EXT_TYPE_CRC + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE, crsfFrame.frame.frameLength);
|
||||
EXPECT_EQ(CRSF_FRAMETYPE_MSP_REQ, crsfFrame.frame.type);
|
||||
uint8_t *destination = (uint8_t *)&crsfFrame.frame.payload;
|
||||
uint8_t *origin = (uint8_t *)&crsfFrame.frame.payload + 1;
|
||||
uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
|
||||
uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2;
|
||||
EXPECT_EQ(0xC8, *destination);
|
||||
EXPECT_EQ(0xEA, *origin);
|
||||
EXPECT_EQ(0x30, *frameStart);
|
||||
EXPECT_EQ(0x69, *frameEnd);
|
||||
}
|
||||
|
||||
TEST(CrossFireMSPTest, ResponsePacketTest)
|
||||
{
|
||||
initSharedMsp();
|
||||
const crsfMspFrame_t *framePtr = (const crsfMspFrame_t*)crsfPidRequest;
|
||||
crsfFrame = *(const crsfFrame_t*)framePtr;
|
||||
crsfFrameDone = true;
|
||||
uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
|
||||
uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2;
|
||||
handleMspFrame(frameStart, frameEnd);
|
||||
for (unsigned int ii=1; ii<30; ii++) {
|
||||
EXPECT_EQ(ii, sbufReadU8(&mspPackage.responsePacket->buf));
|
||||
}
|
||||
sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer);
|
||||
}
|
||||
|
||||
const uint8_t crsfPidWrite1[] = {0x00,0x0D,0x7C,0xC8,0xEA,0x31,0x1E,0xCA,0x29,0x28,0x1E,0x3A,0x32};
|
||||
const uint8_t crsfPidWrite2[] = {0x00,0x0D,0x7C,0xC8,0xEA,0x22,0x23,0x46,0x2D,0x14,0x32,0x00,0x00};
|
||||
const uint8_t crsfPidWrite3[] = {0x00,0x0D,0x7C,0xC8,0xEA,0x23,0x0F,0x00,0x00,0x22,0x0E,0x35,0x19};
|
||||
const uint8_t crsfPidWrite4[] = {0x00,0x0D,0x7C,0xC8,0xEA,0x24,0x21,0x53,0x32,0x32,0x4B,0x28,0x00};
|
||||
const uint8_t crsfPidWrite5[] = {0x00,0x0D,0x7C,0xC8,0xEA,0x25,0x00,0x37,0x37,0x4B,0xF8,0x00,0x00};
|
||||
|
||||
TEST(CrossFireMSPTest, WriteResponseTest)
|
||||
{
|
||||
initSharedMsp();
|
||||
const crsfMspFrame_t *framePtr1 = (const crsfMspFrame_t*)crsfPidWrite1;
|
||||
crsfFrame = *(const crsfFrame_t*)framePtr1;
|
||||
crsfFrameDone = true;
|
||||
uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
|
||||
uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2;
|
||||
bool pending1 = handleMspFrame(frameStart, frameEnd);
|
||||
EXPECT_FALSE(pending1); // not done yet*/
|
||||
EXPECT_EQ(0x29, mspPackage.requestBuffer[0]);
|
||||
EXPECT_EQ(0x28, mspPackage.requestBuffer[1]);
|
||||
EXPECT_EQ(0x1E, mspPackage.requestBuffer[2]);
|
||||
EXPECT_EQ(0x3A, mspPackage.requestBuffer[3]);
|
||||
EXPECT_EQ(0x32, mspPackage.requestBuffer[4]);
|
||||
|
||||
const crsfMspFrame_t *framePtr2 = (const crsfMspFrame_t*)crsfPidWrite2;
|
||||
crsfFrame = *(const crsfFrame_t*)framePtr2;
|
||||
crsfFrameDone = true;
|
||||
uint8_t *frameStart2 = (uint8_t *)&crsfFrame.frame.payload + 2;
|
||||
uint8_t *frameEnd2 = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2;
|
||||
bool pending2 = handleMspFrame(frameStart2, frameEnd2);
|
||||
EXPECT_FALSE(pending2); // not done yet
|
||||
EXPECT_EQ(0x23, mspPackage.requestBuffer[5]);
|
||||
EXPECT_EQ(0x46, mspPackage.requestBuffer[6]);
|
||||
EXPECT_EQ(0x2D, mspPackage.requestBuffer[7]);
|
||||
EXPECT_EQ(0x14, mspPackage.requestBuffer[8]);
|
||||
EXPECT_EQ(0x32, mspPackage.requestBuffer[9]);
|
||||
EXPECT_EQ(0x00, mspPackage.requestBuffer[10]);
|
||||
EXPECT_EQ(0x00, mspPackage.requestBuffer[11]);
|
||||
|
||||
const crsfMspFrame_t *framePtr3 = (const crsfMspFrame_t*)crsfPidWrite3;
|
||||
crsfFrame = *(const crsfFrame_t*)framePtr3;
|
||||
crsfFrameDone = true;
|
||||
uint8_t *frameStart3 = (uint8_t *)&crsfFrame.frame.payload + 2;
|
||||
uint8_t *frameEnd3 = frameStart3 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE;
|
||||
bool pending3 = handleMspFrame(frameStart3, frameEnd3);
|
||||
EXPECT_FALSE(pending3); // not done yet
|
||||
EXPECT_EQ(0x0F, mspPackage.requestBuffer[12]);
|
||||
EXPECT_EQ(0x00, mspPackage.requestBuffer[13]);
|
||||
EXPECT_EQ(0x00, mspPackage.requestBuffer[14]);
|
||||
EXPECT_EQ(0x22, mspPackage.requestBuffer[15]);
|
||||
EXPECT_EQ(0x0E, mspPackage.requestBuffer[16]);
|
||||
EXPECT_EQ(0x35, mspPackage.requestBuffer[17]);
|
||||
EXPECT_EQ(0x19, mspPackage.requestBuffer[18]);
|
||||
|
||||
const crsfMspFrame_t *framePtr4 = (const crsfMspFrame_t*)crsfPidWrite4;
|
||||
crsfFrame = *(const crsfFrame_t*)framePtr4;
|
||||
crsfFrameDone = true;
|
||||
uint8_t *frameStart4 = (uint8_t *)&crsfFrame.frame.payload + 2;
|
||||
uint8_t *frameEnd4 = frameStart4 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE;
|
||||
bool pending4 = handleMspFrame(frameStart4, frameEnd4);
|
||||
EXPECT_FALSE(pending4); // not done yet
|
||||
EXPECT_EQ(0x21, mspPackage.requestBuffer[19]);
|
||||
EXPECT_EQ(0x53, mspPackage.requestBuffer[20]);
|
||||
EXPECT_EQ(0x32, mspPackage.requestBuffer[21]);
|
||||
EXPECT_EQ(0x32, mspPackage.requestBuffer[22]);
|
||||
EXPECT_EQ(0x4B, mspPackage.requestBuffer[23]);
|
||||
EXPECT_EQ(0x28, mspPackage.requestBuffer[24]);
|
||||
EXPECT_EQ(0x00, mspPackage.requestBuffer[25]);
|
||||
//EXPECT_EQ(0xB3,checksum);
|
||||
|
||||
const crsfMspFrame_t *framePtr5 = (const crsfMspFrame_t*)crsfPidWrite5;
|
||||
crsfFrame = *(const crsfFrame_t*)framePtr5;
|
||||
crsfFrameDone = true;
|
||||
uint8_t *frameStart5 = (uint8_t *)&crsfFrame.frame.payload + 2;
|
||||
uint8_t *frameEnd5 = frameStart2 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE;
|
||||
bool pending5 = handleMspFrame(frameStart5, frameEnd5);
|
||||
EXPECT_TRUE(pending5); // not done yet
|
||||
EXPECT_EQ(0x00, mspPackage.requestBuffer[26]);
|
||||
EXPECT_EQ(0x37, mspPackage.requestBuffer[27]);
|
||||
EXPECT_EQ(0x37, mspPackage.requestBuffer[28]);
|
||||
EXPECT_EQ(0x4B, mspPackage.requestBuffer[29]);
|
||||
EXPECT_EQ(0xF8,checksum);
|
||||
|
||||
}
|
||||
|
||||
void testSendMspResponse(uint8_t *payload) {
|
||||
sbuf_t *plOut = sbufInit(&payloadOutputBuf, payloadOutput, payloadOutput + 64);
|
||||
sbufWriteData(plOut, payload, *payload + 64);
|
||||
sbufSwitchToReader(&payloadOutputBuf, payloadOutput);
|
||||
}
|
||||
|
||||
TEST(CrossFireMSPTest, SendMspReply) {
|
||||
initSharedMsp();
|
||||
const crsfMspFrame_t *framePtr = (const crsfMspFrame_t*)crsfPidRequest;
|
||||
crsfFrame = *(const crsfFrame_t*)framePtr;
|
||||
crsfFrameDone = true;
|
||||
uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
|
||||
uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2;
|
||||
bool handled = handleMspFrame(frameStart, frameEnd);
|
||||
EXPECT_TRUE(handled);
|
||||
bool replyPending = sendMspReply(64, &testSendMspResponse);
|
||||
EXPECT_FALSE(replyPending);
|
||||
EXPECT_EQ(0x10, sbufReadU8(&payloadOutputBuf));
|
||||
EXPECT_EQ(0x1E, sbufReadU8(&payloadOutputBuf));
|
||||
for (unsigned int ii=1; ii<=30; ii++) {
|
||||
EXPECT_EQ(ii, sbufReadU8(&payloadOutputBuf));
|
||||
}
|
||||
EXPECT_EQ(0x71, sbufReadU8(&payloadOutputBuf)); // CRC
|
||||
}
|
||||
|
||||
// STUBS
|
||||
|
||||
extern "C" {
|
||||
|
||||
gpsSolutionData_t gpsSol;
|
||||
attitudeEulerAngles_t attitude = { { 0, 0, 0 } };
|
||||
|
||||
uint32_t micros(void) {return dummyTimeUs;}
|
||||
serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, serialReceiveCallbackPtr, uint32_t, portMode_e, portOptions_e) {return NULL;}
|
||||
serialPortConfig_t *findSerialPortConfig(serialPortFunction_e ) {return NULL;}
|
||||
uint16_t getBatteryVoltage(void) {
|
||||
return testBatteryVoltage;
|
||||
}
|
||||
int32_t getAmperage(void) {
|
||||
return testAmperage;
|
||||
}
|
||||
|
||||
uint8_t calculateBatteryPercentageRemaining(void) {
|
||||
return 67;
|
||||
}
|
||||
|
||||
bool feature(uint32_t) {return false;}
|
||||
|
||||
bool isAirmodeActive(void) {return true;}
|
||||
|
||||
mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn) {
|
||||
|
||||
UNUSED(mspPostProcessFn);
|
||||
|
||||
sbuf_t *dst = &reply->buf;
|
||||
//sbuf_t *src = &cmd->buf;
|
||||
const uint8_t cmdMSP = cmd->cmd;
|
||||
reply->cmd = cmd->cmd;
|
||||
|
||||
if (cmdMSP == 0x70) {
|
||||
for (unsigned int ii=1; ii<=30; ii++) {
|
||||
sbufWriteU8(dst, ii);
|
||||
}
|
||||
} else if (cmdMSP == 0xCA) {
|
||||
return MSP_RESULT_ACK;
|
||||
}
|
||||
|
||||
return MSP_RESULT_ACK;
|
||||
}
|
||||
|
||||
void beeperConfirmationBeeps(uint8_t ) {}
|
||||
|
||||
int32_t getMAhDrawn(void) {
|
||||
return testmAhDrawn;
|
||||
}
|
||||
|
||||
}
|
||||
Loading…
Add table
Add a link
Reference in a new issue