Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-24 00:35:39 +03:00
Code Issues Wiki Activity

Merge pull request #7182 from klutvott123/fport-sport-writeeeprom-fix

Browse source 
Skip smartport/fport telemetry requests after eeprom write
This commit is contained in:
Michael Keller 2018-12-11 01:28:16 +13:00 • committed by GitHub
commit 6972f3eb01
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
6 changed files with 35 additions and 17 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/telemetry/crsf.c
View file

@ -112,7 +112,7 @@ bool handleCrsfMspFrameBuffer(uint8_t payloadSize, mspResponseFnPtr responseFn)
int pos = 0;
while (true) {
const int mspFrameLength = mspRxBuffer.bytes[pos];
if (handleMspFrame(&mspRxBuffer.bytes[CRSF_MSP_LENGTH_OFFSET + pos], mspFrameLength)) {
if (handleMspFrame(&mspRxBuffer.bytes[CRSF_MSP_LENGTH_OFFSET + pos], mspFrameLength, NULL)) {
requestHandled |= sendMspReply(payloadSize, responseFn);
}
pos += CRSF_MSP_LENGTH_OFFSET + mspFrameLength;

10
src/main/telemetry/msp_shared.c
View file

@ -31,6 +31,7 @@
#include "common/utils.h"
#include "interface/msp.h"
#include "interface/msp_protocol.h"
#include "telemetry/crsf.h"
#include "telemetry/msp_shared.h"
@ -44,6 +45,8 @@
#define TELEMETRY_MSP_SEQ_MASK 0x0F
#define TELEMETRY_MSP_RES_ERROR (-10)
#define TELEMETRY_REQUEST_SKIPS_AFTER_EEPROMWRITE 5
enum {
TELEMETRY_MSP_VER_MISMATCH=0,
TELEMETRY_MSP_CRC_ERROR=1,
@ -98,7 +101,7 @@ void sendMspErrorResponse(uint8_t error, int16_t cmd)
sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer);
}
bool handleMspFrame(uint8_t *frameStart, int frameLength)
bool handleMspFrame(uint8_t *frameStart, int frameLength, uint8_t *skipsBeforeResponse)
{
static uint8_t mspStarted = 0;
static uint8_t lastSeq = 0;
@ -170,6 +173,11 @@ bool handleMspFrame(uint8_t *frameStart, int frameLength)
}
}
// Skip a few telemetry requests if command is MSP_EEPROM_WRITE
if (packet->cmd == MSP_EEPROM_WRITE && skipsBeforeResponse) {
*skipsBeforeResponse = TELEMETRY_REQUEST_SKIPS_AFTER_EEPROMWRITE;
}
mspStarted = 0;
sbufSwitchToReader(rxBuf, mspPackage.requestBuffer);
processMspPacket();

2
src/main/telemetry/msp_shared.h
View file

@ -46,5 +46,5 @@ typedef union mspTxBuffer_u {
} mspTxBuffer_t;
void initSharedMsp(void);
bool handleMspFrame(uint8_t *frameStart, int frameLength);
bool handleMspFrame(uint8_t *frameStart, int frameLength, uint8_t *skipsBeforeResponse);
bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn);

20
src/main/telemetry/smartport.c
View file

@ -287,7 +287,6 @@ bool smartPortPayloadContainsMSP(const smartPortPayload_t *payload)
return payload->frameId == FSSP_MSPC_FRAME_SMARTPORT || payload->frameId == FSSP_MSPC_FRAME_FPORT;
}
void smartPortWriteFrameSerial(const smartPortPayload_t *payload, serialPort_t *port, uint16_t checksum)
{
uint8_t *data = (uint8_t *)payload;
@ -493,24 +492,35 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
static uint8_t smartPortIdCycleCnt = 0;
static uint8_t t1Cnt = 0;
static uint8_t t2Cnt = 0;
static uint8_t skipRequests = 0;
#ifdef USE_ESC_SENSOR_TELEMETRY
static uint8_t smartPortIdOffset = 0;
#endif
#if defined(USE_MSP_OVER_TELEMETRY)
if (payload && smartPortPayloadContainsMSP(payload)) {
if (skipRequests) {
skipRequests--;
} else if (payload && smartPortPayloadContainsMSP(payload)) {
// Do not check the physical ID here again
// unless we start receiving other sensors' packets
// Pass only the payload: skip frameId
uint8_t *frameStart = (uint8_t *)&payload->valueId;
smartPortMspReplyPending = handleMspFrame(frameStart, SMARTPORT_MSP_PAYLOAD_SIZE);
uint8_t *frameStart = (uint8_t *)&payload->valueId;
smartPortMspReplyPending = handleMspFrame(frameStart, SMARTPORT_MSP_PAYLOAD_SIZE, &skipRequests);
// Don't send MSP response after write to eeprom
// CPU just got out of suspended state after writeEEPROM()
// We don't know if the receiver is listening again
// Skip a few telemetry requests before sending response
if (skipRequests) {
*clearToSend = false;
}
}
#else
UNUSED(payload);
#endif
bool doRun = true;
while (doRun && *clearToSend) {
while (doRun && *clearToSend && !skipRequests) {
// Ensure we won't get stuck in the loop if there happens to be nothing available to send in a timely manner - dump the slot if we loop in there for too long.
if (requestTimeout) {
if (cmpTimeUs(micros(), *requestTimeout) >= 0) {

16
src/test/unit/telemetry_crsf_msp_unittest.cc
View file

@ -63,7 +63,7 @@ extern "C" {
#include "telemetry/smartport.h"
#include "sensors/acceleration.h"
bool handleMspFrame(uint8_t *frameStart, int frameLength);
bool handleMspFrame(uint8_t *frameStart, int frameLength, uint8_t *skipsBeforeResponse);
bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn);
uint8_t sbufReadU8(sbuf_t *src);
int sbufBytesRemaining(sbuf_t *buf);
@ -137,7 +137,7 @@ TEST(CrossFireMSPTest, ResponsePacketTest)
crsfFrame = *(const crsfFrame_t*)framePtr;
crsfFrameDone = true;
uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
handleMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE);
handleMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL);
for (unsigned int ii=1; ii<30; ii++) {
EXPECT_EQ(ii, sbufReadU8(&mspPackage.responsePacket->buf));
}
@ -157,7 +157,7 @@ TEST(CrossFireMSPTest, WriteResponseTest)
crsfFrame = *(const crsfFrame_t*)framePtr1;
crsfFrameDone = true;
uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
bool pending1 = handleMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE);
bool pending1 = handleMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL);
EXPECT_FALSE(pending1); // not done yet*/
EXPECT_EQ(0x29, mspPackage.requestBuffer[0]);
EXPECT_EQ(0x28, mspPackage.requestBuffer[1]);
@ -169,7 +169,7 @@ TEST(CrossFireMSPTest, WriteResponseTest)
crsfFrame = *(const crsfFrame_t*)framePtr2;
crsfFrameDone = true;
uint8_t *frameStart2 = (uint8_t *)&crsfFrame.frame.payload + 2;
bool pending2 = handleMspFrame(frameStart2, CRSF_FRAME_RX_MSP_FRAME_SIZE);
bool pending2 = handleMspFrame(frameStart2, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL);
EXPECT_FALSE(pending2); // not done yet
EXPECT_EQ(0x23, mspPackage.requestBuffer[5]);
EXPECT_EQ(0x46, mspPackage.requestBuffer[6]);
@ -183,7 +183,7 @@ TEST(CrossFireMSPTest, WriteResponseTest)
crsfFrame = *(const crsfFrame_t*)framePtr3;
crsfFrameDone = true;
uint8_t *frameStart3 = (uint8_t *)&crsfFrame.frame.payload + 2;
bool pending3 = handleMspFrame(frameStart3, CRSF_FRAME_RX_MSP_FRAME_SIZE);
bool pending3 = handleMspFrame(frameStart3, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL);
EXPECT_FALSE(pending3); // not done yet
EXPECT_EQ(0x0F, mspPackage.requestBuffer[12]);
EXPECT_EQ(0x00, mspPackage.requestBuffer[13]);
@ -197,7 +197,7 @@ TEST(CrossFireMSPTest, WriteResponseTest)
crsfFrame = *(const crsfFrame_t*)framePtr4;
crsfFrameDone = true;
uint8_t *frameStart4 = (uint8_t *)&crsfFrame.frame.payload + 2;
bool pending4 = handleMspFrame(frameStart4, CRSF_FRAME_RX_MSP_FRAME_SIZE);
bool pending4 = handleMspFrame(frameStart4, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL);
EXPECT_FALSE(pending4); // not done yet
EXPECT_EQ(0x21, mspPackage.requestBuffer[19]);
EXPECT_EQ(0x53, mspPackage.requestBuffer[20]);
@ -212,7 +212,7 @@ TEST(CrossFireMSPTest, WriteResponseTest)
crsfFrame = *(const crsfFrame_t*)framePtr5;
crsfFrameDone = true;
uint8_t *frameStart5 = (uint8_t *)&crsfFrame.frame.payload + 2;
bool pending5 = handleMspFrame(frameStart5, CRSF_FRAME_RX_MSP_FRAME_SIZE);
bool pending5 = handleMspFrame(frameStart5, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL);
EXPECT_TRUE(pending5); // not done yet
EXPECT_EQ(0x00, mspPackage.requestBuffer[26]);
EXPECT_EQ(0x37, mspPackage.requestBuffer[27]);
@ -234,7 +234,7 @@ TEST(CrossFireMSPTest, SendMspReply) {
crsfFrame = *(const crsfFrame_t*)framePtr;
crsfFrameDone = true;
uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
bool handled = handleMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE);
bool handled = handleMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL);
EXPECT_TRUE(handled);
bool replyPending = sendMspReply(64, &testSendMspResponse);
EXPECT_FALSE(replyPending);

2
src/test/unit/telemetry_crsf_unittest.cc
View file

@ -344,7 +344,7 @@ int32_t getMAhDrawn(void){
}
bool sendMspReply(uint8_t, mspResponseFnPtr) { return false; }
bool handleMspFrame(uint8_t *, int) { return false; }
bool handleMspFrame(uint8_t *, int, uint8_t *) { return false; }
void crsfScheduleMspResponse(void) {};
bool isBatteryVoltageConfigured(void) { return true; }
bool isAmperageConfigured(void) { return true; }