1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-25 01:05:27 +03:00
This commit is contained in:
jflyper 2017-06-11 02:14:47 +09:00
parent fac694ce4b
commit c40178dda1
5 changed files with 53 additions and 76 deletions

View file

@ -749,25 +749,27 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce
break;
case MSP_TRANSPONDER_CONFIG: {
#define TRANSPONDER_SUPPORTED_MASK 0x01 // 00000001
#define TRANSPONDER_PROVIDER_MASK 0x0E // 00001110
#define TRANSPONDER_DATA_SIZE_MASK 0xF0 // 11110000
#define TRANSPONDER_PROVIDER_OFFSET 1
#define TRANSPONDER_DATA_SIZE_OFFSET 4
#ifdef TRANSPONDER
uint8_t header = 0;
// Backward compatibility to BFC 3.1.1 is lost for this message type
sbufWriteU8(dst, TRANSPONDER_PROVIDER_COUNT);
for (unsigned int i = 0; i < TRANSPONDER_PROVIDER_COUNT; i++) {
sbufWriteU8(dst, transponderRequirements[i].provider);
sbufWriteU8(dst, transponderRequirements[i].dataLength);
}
header |= 1 & TRANSPONDER_SUPPORTED_MASK;
header |= (transponderConfig()->provider << TRANSPONDER_PROVIDER_OFFSET) & TRANSPONDER_PROVIDER_MASK;
header |= ((sizeof(transponderConfig()->data) << TRANSPONDER_DATA_SIZE_OFFSET) & TRANSPONDER_DATA_SIZE_MASK);
uint8_t provider = transponderConfig()->provider;
sbufWriteU8(dst, provider);
sbufWriteU8(dst, header);
for (unsigned int i = 0; i < sizeof(transponderConfig()->data); i++) {
sbufWriteU8(dst, transponderConfig()->data[i]);
if (provider) {
uint8_t requirementIndex = provider - 1;
uint8_t providerDataLength = transponderRequirements[requirementIndex].dataLength;
for (unsigned int i = 0; i < providerDataLength; i++) {
sbufWriteU8(dst, transponderConfig()->data[i]);
}
}
#else
sbufWriteU8(dst, 0 & TRANSPONDER_SUPPORTED_MASK);
sbufWriteU8(dst, 0); // no providers
#endif
break;
}
@ -2000,30 +2002,36 @@ static mspResult_e mspCommonProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
switch (cmdMSP) {
#ifdef TRANSPONDER
case MSP_SET_TRANSPONDER_CONFIG:
{
uint8_t tmp = sbufReadU8(src);
case MSP_SET_TRANSPONDER_CONFIG: {
// Backward compatibility to BFC 3.1.1 is lost for this message type
uint8_t type;
switch(tmp){
case 0x02:
type = ILAP;
break;
case 0x04:
type = ARCITIMER;
break;
uint8_t provider = sbufReadU8(src);
uint8_t bytesRemaining = dataSize - 1;
if (provider > TRANSPONDER_PROVIDER_COUNT) {
return MSP_RESULT_ERROR;
}
if(type != transponderConfig()->provider){
const uint8_t requirementIndex = provider - 1;
const uint8_t transponderDataSize = transponderRequirements[requirementIndex].dataLength;
transponderConfigMutable()->provider = provider;
if (provider == TRANSPONDER_NONE) {
break;
}
if (bytesRemaining != transponderDataSize) {
return MSP_RESULT_ERROR;
}
if (provider != transponderConfig()->provider) {
transponderStopRepeating();
}
transponderConfigMutable()->provider = type;
memset(transponderConfigMutable()->data, 0, sizeof(transponderConfig()->data));
if (dataSize != sizeof(transponderConfig()->data) + 1) {
return MSP_RESULT_ERROR;
}
for (unsigned int i = 0; i < sizeof(transponderConfig()->data); i++) {
for (unsigned int i = 0; i < transponderDataSize; i++) {
transponderConfigMutable()->data[i] = sbufReadU8(src);
}
transponderUpdateData();