Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-20 14:55:21 +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; int pos = 0;
while (true) { while (true) {
const int mspFrameLength = mspRxBuffer.bytes[pos]; 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); requestHandled |= sendMspReply(payloadSize, responseFn);
} }
pos += CRSF_MSP_LENGTH_OFFSET + mspFrameLength; pos += CRSF_MSP_LENGTH_OFFSET + mspFrameLength;

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

@ -31,6 +31,7 @@
#include "common/utils.h" #include "common/utils.h"
#include "interface/msp.h" #include "interface/msp.h"
#include "interface/msp_protocol.h"
#include "telemetry/crsf.h" #include "telemetry/crsf.h"
#include "telemetry/msp_shared.h" #include "telemetry/msp_shared.h"
@ -44,6 +45,8 @@
#define TELEMETRY_MSP_SEQ_MASK 0x0F #define TELEMETRY_MSP_SEQ_MASK 0x0F
#define TELEMETRY_MSP_RES_ERROR (-10) #define TELEMETRY_MSP_RES_ERROR (-10)
#define TELEMETRY_REQUEST_SKIPS_AFTER_EEPROMWRITE 5
enum { enum {
TELEMETRY_MSP_VER_MISMATCH=0, TELEMETRY_MSP_VER_MISMATCH=0,
TELEMETRY_MSP_CRC_ERROR=1, TELEMETRY_MSP_CRC_ERROR=1,
@ -98,7 +101,7 @@ void sendMspErrorResponse(uint8_t error, int16_t cmd)
sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer); 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 mspStarted = 0;
static uint8_t lastSeq = 0; static uint8_t lastSeq = 0;
@ -169,7 +172,12 @@ bool handleMspFrame(uint8_t *frameStart, int frameLength)
return true; return true;
} }
} }
// 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; mspStarted = 0;
sbufSwitchToReader(rxBuf, mspPackage.requestBuffer); sbufSwitchToReader(rxBuf, mspPackage.requestBuffer);
processMspPacket(); processMspPacket();

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

@ -46,5 +46,5 @@ typedef union mspTxBuffer_u {
} mspTxBuffer_t; } mspTxBuffer_t;
void initSharedMsp(void); 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); bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn);

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

@ -59,7 +59,6 @@
#include "flight/pid.h" #include "flight/pid.h"
#include "interface/msp.h" #include "interface/msp.h"
#include "interface/msp_protocol.h"
#include "io/beeper.h" #include "io/beeper.h"
#include "io/motors.h" #include "io/motors.h"
@ -82,7 +81,6 @@
#include "telemetry/msp_shared.h" #include "telemetry/msp_shared.h"
#define SMARTPORT_MIN_TELEMETRY_RESPONSE_DELAY_US 500 #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 // these data identifiers are obtained from https://github.com/opentx/opentx/blob/master/radio/src/telemetry/frsky_hub.h
enum enum
@ -289,11 +287,6 @@ bool smartPortPayloadContainsMSP(const smartPortPayload_t *payload)
return payload->frameId == FSSP_MSPC_FRAME_SMARTPORT || payload->frameId == FSSP_MSPC_FRAME_FPORT; 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) void smartPortWriteFrameSerial(const smartPortPayload_t *payload, serialPort_t *port, uint16_t checksum)
{ {
uint8_t *data = (uint8_t *)payload; 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 // unless we start receiving other sensors' packets
// Pass only the payload: skip frameId // Pass only the payload: skip frameId
uint8_t *frameStart = (uint8_t *)&payload->valueId; 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 // Don't send MSP response if MSP command is MSP_EEPROM_WRITE
// CPU just got out of suspended state after writeEEPROM() // CPU just got out of suspended state after writeEEPROM()
// We don't know if the receiver is listening again // We don't know if the receiver is listening again
// Skip a few telemetry requests before sending response // Skip a few telemetry requests before sending response
if (cmdIsEepromWrite(payload) && smartPortMspReplyPending) { if (skipRequests) {
skipRequests = SMARTPORT_REQUEST_SKIPS_AFTER_EEPROMWRITE;
*clearToSend = false; *clearToSend = false;
} }
} }