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

Move MSP_EEPROM_WRITE check to handleMspFrame

Browse source
This commit is contained in:
Hans Christian Olaussen 2018-12-06 19:22:59 +01:00
parent bfabf48ca7
commit 4fb82f8d34
4 changed files with 14 additions and 14 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 = 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);

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

@ -59,7 +59,6 @@
#include "flight/pid.h"
#include "interface/msp.h"
#include "interface/msp_protocol.h"
#include "io/beeper.h"
#include "io/motors.h"
@ -82,7 +81,6 @@
#include "telemetry/msp_shared.h"
#define SMARTPORT_MIN_TELEMETRY_RESPONSE_DELAY_US 500
#define SMARTPORT_REQUEST_SKIPS_AFTER_EEPROMWRITE 5
// these data identifiers are obtained from https://github.com/opentx/opentx/blob/master/radio/src/telemetry/frsky_hub.h
enum
@ -289,11 +287,6 @@ bool smartPortPayloadContainsMSP(const smartPortPayload_t *payload)
return payload->frameId == FSSP_MSPC_FRAME_SMARTPORT || payload->frameId == FSSP_MSPC_FRAME_FPORT;
}
bool cmdIsEepromWrite(const smartPortPayload_t *payload)
{
return ((payload->valueId >> 8)&0xFF) == 0 && (payload->data&0xFF) == MSP_EEPROM_WRITE && ((payload->data >> 8)&0xFF) == MSP_EEPROM_WRITE;
}
void smartPortWriteFrameSerial(const smartPortPayload_t *payload, serialPort_t *port, uint16_t checksum)
{
uint8_t *data = (uint8_t *)payload;
@ -512,14 +505,13 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
// 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);
smartPortMspReplyPending = handleMspFrame(frameStart, SMARTPORT_MSP_PAYLOAD_SIZE, &skipRequests);
// Don't send MSP response if MSP command is MSP_EEPROM_WRITE
// 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 (cmdIsEepromWrite(payload) && smartPortMspReplyPending) {
skipRequests = SMARTPORT_REQUEST_SKIPS_AFTER_EEPROMWRITE;
if (skipRequests) {
*clearToSend = false;
}
}