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

Split out MSP_DATAFLASH_READ command into separate function

Browse source
This commit is contained in:
Martin Budden 2016-10-18 21:18:21 +01:00
parent e78b544139
commit 1edb0fe33d
2 changed files with 27 additions and 27 deletions
Download patch file Download diff file Expand all files Collapse all files

52
src/main/fc/fc_msp.c
View file

@ -515,14 +515,9 @@ static uint32_t packFlightModeFlags(void)
return junk;
}
static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn)
static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn)
{
uint32_t i;
#ifdef USE_FLASHFS
const unsigned int dataSize = sbufBytesRemaining(src);
#else
UNUSED(src);
#endif
#ifdef GPS
uint8_t wp_no;
int32_t lat = 0, lon = 0;
@ -991,25 +986,6 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, sbuf_t *src, msp
serializeDataflashSummaryReply(dst);
break;
#ifdef USE_FLASHFS
case MSP_DATAFLASH_READ:
{
uint32_t readAddress = sbufReadU32(src);
uint16_t readLength;
bool useLegacyFormat;
if (dataSize >= sizeof(uint32_t) + sizeof(uint16_t)) {
readLength = sbufReadU16(src);
useLegacyFormat = false;
} else {
readLength = 128;
useLegacyFormat = true;
}
serializeDataflashReadReply(dst, readAddress, readLength, useLegacyFormat);
}
break;
#endif
case MSP_BLACKBOX_CONFIG:
#ifdef BLACKBOX
sbufWriteU8(dst, 1); //Blackbox supported
@ -1150,6 +1126,25 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, sbuf_t *src, msp
return true;
}
#ifdef USE_FLASHFS
static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src)
{
const unsigned int dataSize = sbufBytesRemaining(src);
const uint32_t readAddress = sbufReadU32(src);
uint16_t readLength;
bool useLegacyFormat;
if (dataSize >= sizeof(uint32_t) + sizeof(uint16_t)) {
readLength = sbufReadU16(src);
useLegacyFormat = false;
} else {
readLength = 128;
useLegacyFormat = true;
}
serializeDataflashReadReply(dst, readAddress, readLength, useLegacyFormat);
}
#endif
static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
{
uint32_t i;
@ -1755,8 +1750,13 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro
// initialize reply by default
reply->cmd = cmd->cmd;
if (mspFcProcessOutCommand(cmdMSP, dst, src, mspPostProcessFn)) {
if (mspFcProcessOutCommand(cmdMSP, dst, mspPostProcessFn)) {
ret = MSP_RESULT_ACK;
#ifdef USE_FLASHFS
} else if (cmdMSP == MSP_DATAFLASH_READ) {
mspFcDataFlashReadCommand(dst, src);
ret = MSP_RESULT_ACK;
#endif
} else {
ret = mspFcProcessInCommand(cmdMSP, src);
}

2
src/main/msp/msp_serial.c
View file

@ -74,7 +74,7 @@ void mspSerialReleasePortIfAllocated(serialPort_t *serialPort)
}
}
static bool mspSerialProcessReceivedData(mspPort_t * mspPort, uint8_t c)
static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
{
if (mspPort->c_state == MSP_IDLE) {
if (c == '$') {