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

Add MSP_MULTIPLE_MSP

Browse source
This commit is contained in:
Alex Wolf 2018-08-03 11:24:38 -04:00
parent d0a9a0fd93
commit 7b6555f001
2 changed files with 37 additions and 2 deletions
Download patch file Download diff file Expand all files Collapse all files

34
src/main/interface/msp.c
View file

@ -1448,6 +1448,40 @@ static mspResult_e mspFcProcessOutCommandWithArg(uint8_t cmdMSP, sbuf_t *src, sb
*mspPostProcessFn = mspRebootFn;
}
break;
case MSP_MULTIPLE_MSP:
{
uint8_t maxMSPs = 0;
if (sbufBytesRemaining(src) == 0) {
return MSP_RESULT_ERROR;
}
int bytesRemaining = sbufBytesRemaining(dst) - 1; // need to keep one byte for checksum
mspPacket_t packetIn, packetOut;
sbufInit(&packetIn.buf, src->end, src->end);
uint8_t* resetInputPtr = src->ptr;
while (sbufBytesRemaining(src) && bytesRemaining > 0) {
uint8_t newMSP = sbufReadU8(src);
sbufInit(&packetOut.buf, dst->ptr, dst->end);
packetIn.cmd = newMSP;
mspFcProcessCommand(&packetIn, &packetOut, NULL);
uint8_t mspSize = sbufPtr(&packetOut.buf) - dst->ptr;
mspSize++; // need to add length information for each MSP
bytesRemaining -= mspSize;
if (bytesRemaining >= 0) {
maxMSPs++;
}
}
src->ptr = resetInputPtr;
sbufInit(&packetOut.buf, dst->ptr, dst->end);
for (int i = 0; i < maxMSPs; i++) {
uint8_t* sizePtr = sbufPtr(&packetOut.buf);
sbufWriteU8(&packetOut.buf, 0); // dummy
packetIn.cmd = sbufReadU8(src);
mspFcProcessCommand(&packetIn, &packetOut, NULL);
(*sizePtr) = sbufPtr(&packetOut.buf) - (sizePtr + 1);
}
dst->ptr = packetOut.buf.ptr;
}
break;
default:
return MSP_RESULT_CMD_UNKNOWN;

1
src/main/interface/msp_protocol.h
View file

@ -321,6 +321,7 @@
#define MSP_UID 160 //out message Unique device ID
#define MSP_GPSSVINFO 164 //out message get Signal Strength (only U-Blox)
#define MSP_GPSSTATISTICS 166 //out message get GPS debugging data
#define MSP_MULTIPLE_MSP 230 //out message request multiple MSPs in one request - limit is the TX buffer; returns each MSP in the order they were requested starting with length of MSP; MSPs with input arguments are not supported
#define MSP_ACC_TRIM 240 //out message get acc angle trim values
#define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values
#define MSP_SERVO_MIX_RULES 241 //out message Returns servo mixer configuration