1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-23 08:15:30 +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 2017-08-25 09:22:02 -04:00
parent ab53b178ca
commit 863b24ae6a
17 changed files with 738 additions and 229 deletions

View file

@ -172,7 +172,10 @@ rc_controls_unittest_SRC := \
rx_crsf_unittest_SRC := \
$(USER_DIR)/rx/crsf.c \
$(USER_DIR)/common/crc.c \
$(USER_DIR)/common/streambuf.c
$(USER_DIR)/common/printf.c \
$(USER_DIR)/common/typeconversion.c \
$(USER_DIR)/common/streambuf.c \
$(USER_DIR)/drivers/serial.c
rx_ibus_unittest_SRC := \
@ -207,8 +210,24 @@ telemetry_crsf_unittest_SRC := \
$(USER_DIR)/common/maths.c \
$(USER_DIR)/common/streambuf.c \
$(USER_DIR)/common/gps_conversion.c \
$(USER_DIR)/common/printf.c \
$(USER_DIR)/common/typeconversion.c \
$(USER_DIR)/fc/runtime_config.c
telemetry_crsf_msp_unittest_SRC := \
$(USER_DIR)/rx/crsf.c \
$(USER_DIR)/common/crc.c \
$(USER_DIR)/common/streambuf.c \
$(USER_DIR)/common/printf.c \
$(USER_DIR)/common/streambuf.c \
$(USER_DIR)/drivers/serial.c \
$(USER_DIR)/common/typeconversion.c \
$(USER_DIR)/telemetry/crsf.c \
$(USER_DIR)/common/gps_conversion.c \
$(USER_DIR)/telemetry/msp_shared.c \
$(USER_DIR)/fc/runtime_config.c
telemetry_crsf_unittest_DEFINES := \
FLASH_SIZE=128 \
STM32F10X_MD \
@ -254,6 +273,9 @@ huffman_unittest_SRC := \
huffman_unittest_DEFINES := \
USE_HUFFMAN
ringbuffer_unittest_SRC := \
$(USER_DIR)/common/ringbuffer.c
# Please tweak the following variable definitions as needed by your
# project, except GTEST_HEADERS, which you can use in your own targets
# but shouldn't modify.

View file

@ -29,11 +29,14 @@ extern "C" {
#include "common/crc.h"
#include "common/utils.h"
#include "drivers/serial.h"
#include "io/serial.h"
#include "rx/rx.h"
#include "rx/crsf.h"
#include "telemetry/msp_shared.h"
void crsfDataReceive(uint16_t c);
uint8_t crsfFrameCRC(void);
uint8_t crsfFrameStatus(void);
@ -277,7 +280,9 @@ int16_t debug[DEBUG16_VALUE_COUNT];
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;}
void serialWriteBuf(serialPort_t *, const uint8_t *, int) {}
bool telemetryCheckRxPortShared(const serialPortConfig_t *) {return false;}
serialPort_t *telemetrySharedPort = NULL;
void scheduleDeviceInfoResponse(void) {};
void scheduleMspResponse(mspPackage_t *package) { UNUSED(package); };
bool handleMspFrame(uint8_t *, uint8_t *) { return false; }
}

View 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;
}
}

View file

@ -31,6 +31,8 @@ extern "C" {
#include "common/filter.h"
#include "common/gps_conversion.h"
#include "common/maths.h"
#include "common/printf.h"
#include "common/typeconversion.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
@ -38,6 +40,7 @@ extern "C" {
#include "drivers/serial.h"
#include "drivers/system.h"
#include "fc/config.h"
#include "fc/runtime_config.h"
#include "flight/pid.h"
@ -53,6 +56,7 @@ extern "C" {
#include "telemetry/crsf.h"
#include "telemetry/telemetry.h"
#include "telemetry/msp_shared.h"
bool airMode;
@ -63,6 +67,7 @@ extern "C" {
serialPort_t *telemetrySharedPort;
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);
}
#include "unittest_macros.h"
@ -293,6 +298,7 @@ void serialWriteBuf(serialPort_t *, const uint8_t *, int) {}
void serialSetMode(serialPort_t *, portMode_e) {}
serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, serialReceiveCallbackPtr, uint32_t, portMode_e, portOptions_e) {return NULL;}
void closeSerialPort(serialPort_t *) {}
bool isSerialTransmitBufferEmpty(const serialPort_t *) { return true; }
serialPortConfig_t *findSerialPortConfig(serialPortFunction_e) {return NULL;}
@ -323,4 +329,7 @@ int32_t getMAhDrawn(void){
return testmAhDrawn;
}
bool sendMspReply(uint8_t, mspResponseFnPtr) { return false; }
bool handleMspFrame(uint8_t *, uint8_t *) { return false; }
}