mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-25 01:05:27 +03:00
Touch up
This commit is contained in:
parent
fac694ce4b
commit
c40178dda1
5 changed files with 53 additions and 76 deletions
|
@ -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();
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue