diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index b70b08719f..f2ba387e92 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -36,6 +36,7 @@ #include "drivers/gyro_sync.h" #include "drivers/light_led.h" #include "drivers/time.h" +#include "drivers/transponder_ir.h" #include "sensors/acceleration.h" #include "sensors/barometer.h" diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 690e2222c0..f09bb23713 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -749,8 +749,24 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce break; case MSP_TRANSPONDER_CONFIG: + { #ifdef TRANSPONDER - sbufWriteU8(dst, 1); //Transponder supported + uint8_t header = 1; //transponder supported + + switch(transponderConfig()->provider){ + case ILAP: + header |= 0x02; + break; + case ARCITIMER: + header |= 0x04; + break; + default: + break; + } + + header |= (sizeof(transponderConfig()->data) << 4); + + sbufWriteU8(dst, header); for (unsigned int i = 0; i < sizeof(transponderConfig()->data); i++) { sbufWriteU8(dst, transponderConfig()->data[i]); } @@ -758,6 +774,7 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce sbufWriteU8(dst, 0); // Transponder not supported #endif break; + } case MSP_OSD_CONFIG: { #define OSD_FLAGS_OSD_FEATURE (1 << 0) @@ -1988,7 +2005,26 @@ static mspResult_e mspCommonProcessInCommand(uint8_t cmdMSP, sbuf_t *src) switch (cmdMSP) { #ifdef TRANSPONDER case MSP_SET_TRANSPONDER_CONFIG: - if (dataSize != sizeof(transponderConfig()->data)) { + { + uint8_t tmp = sbufReadU8(src); + + uint8_t type; + switch(tmp){ + case 0x02: + type = ILAP; + break; + case 0x04: + type = ARCITIMER; + break; + } + + if(type != transponderConfig()->provider){ + transponderStopRepeating(); + } + + transponderConfigMutable()->provider = type; + + if (dataSize != sizeof(transponderConfig()->data) + 1) { return MSP_RESULT_ERROR; } for (unsigned int i = 0; i < sizeof(transponderConfig()->data); i++) { @@ -1996,6 +2032,7 @@ static mspResult_e mspCommonProcessInCommand(uint8_t cmdMSP, sbuf_t *src) } transponderUpdateData(); break; + } #endif case MSP_SET_VOLTAGE_METER_CONFIG: {