mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-21 23:35:34 +03:00
Move MSP_EEPROM_WRITE check to handleMspFrame
This commit is contained in:
parent
bfabf48ca7
commit
4fb82f8d34
4 changed files with 14 additions and 14 deletions
|
@ -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;
|
||||
|
|
|
@ -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();
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue