rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #1642 from martinbudden/bf_dataflash_read 

Fix dataflash read by only using serialWriteBuf in mspSerialEncode
This commit is contained in:
Martin Budden 2016-11-23 22:32:06 +01:00 committed by GitHub
parent af42f9d225 83472a6989
commit 181a12bb00
1 changed files with 9 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/main/msp/msp_serial.c
View File

@ -125,20 +125,21 @@ static int mspSerialEncode(mspPort_t *msp, mspPacket_t *packet)
serialBeginWrite(msp->port); serialBeginWrite(msp->port);
const int len = sbufBytesRemaining(&packet->buf); const int len = sbufBytesRemaining(&packet->buf);
const int mspLen = len < JUMBO_FRAME_SIZE_LIMIT ? len : JUMBO_FRAME_SIZE_LIMIT; const int mspLen = len < JUMBO_FRAME_SIZE_LIMIT ? len : JUMBO_FRAME_SIZE_LIMIT;
const uint8_t hdr[5] = {'$', 'M', packet->result == MSP_RESULT_ERROR ? '!' : '>', mspLen, packet->cmd}; uint8_t hdr[8] = {'$', 'M', packet->result == MSP_RESULT_ERROR ? '!' : '>', mspLen, packet->cmd};
serialWriteBuf(msp->port, hdr, sizeof(hdr)); int hdrLen = 5;
uint8_t checksum = mspSerialChecksumBuf(0, hdr + 3, 2); // checksum starts from len field #define CHECKSUM_STARTPOS 3 // checksum starts from mspLen field
if (len >= JUMBO_FRAME_SIZE_LIMIT) { if (len >= JUMBO_FRAME_SIZE_LIMIT) {
serialWrite(msp->port, len & 0xff); hdrLen += 2;
checksum ^= len & 0xff; hdr[5] = len & 0xff;
serialWrite(msp->port, (len >> 8) & 0xff); hdr[6] = (len >> 8) & 0xff;
checksum ^= (len >> 8) & 0xff;
} }
serialWriteBuf(msp->port, hdr, hdrLen);
uint8_t checksum = mspSerialChecksumBuf(0, hdr + CHECKSUM_STARTPOS, hdrLen - CHECKSUM_STARTPOS);
if (len > 0) { if (len > 0) {
serialWriteBuf(msp->port, sbufPtr(&packet->buf), len); serialWriteBuf(msp->port, sbufPtr(&packet->buf), len);
checksum = mspSerialChecksumBuf(checksum, sbufPtr(&packet->buf), len); checksum = mspSerialChecksumBuf(checksum, sbufPtr(&packet->buf), len);
} }
serialWrite(msp->port, checksum); serialWriteBuf(msp->port, &checksum, 1);
serialEndWrite(msp->port); serialEndWrite(msp->port);
return sizeof(hdr) + len + 1; // header, data, and checksum return sizeof(hdr) + len + 1; // header, data, and checksum
} }