1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-21 15:25:36 +03:00

CF/BF - Cleanup transponder config after Arctimer merge.

See https://github.com/cleanflight/cleanflight/pull/2580
This commit is contained in:
Hydra 2017-04-01 18:33:18 +01:00
parent 9f7f2f2d08
commit 0e6e19cb6b
5 changed files with 37 additions and 38 deletions

View file

@ -151,7 +151,7 @@ void transponderIrHardwareInit(ioTag_t ioTag, transponder_t *transponder)
DMA_ITConfig(dmaRef, DMA_IT_TC, ENABLE);
}
bool transponderIrInit(const TransponderProvider* transponderProvider)
bool transponderIrInit(const transponderProvider_e provider)
{
ioTag_t ioTag = IO_TAG_NONE;
for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
@ -165,14 +165,15 @@ bool transponderIrInit(const TransponderProvider* transponderProvider)
return false;
}
uint8_t transponderProviderLocal = *transponderProvider;
switch(transponderProviderLocal){
switch (provider) {
case ARCITIMER:
transponderIrInitArcitimer(&transponder);
break;
case ILAP:
transponderIrInitIlap(&transponder);
break;
default:
return false;
}
transponderIrHardwareInit(ioTag, &transponder);

View file

@ -79,16 +79,17 @@ typedef struct transponder_s {
const struct transponderVTable *vTable;
} transponder_t;
typedef enum TransponderProvider{
typedef enum {
NONE,
ARCITIMER,
ILAP
} TransponderProvider;
} transponderProvider_e;
struct transponderVTable {
void (*updateTransponderDMABuffer)(transponder_t *transponder, const uint8_t* transponderData);
};
bool transponderIrInit(const TransponderProvider* transponderProvider);
bool transponderIrInit(const transponderProvider_e provider);
void transponderIrDisable(void);
void transponderIrHardwareInit(ioTag_t ioTag, transponder_t *transponder);

View file

@ -1085,30 +1085,28 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
serializeSDCardSummaryReply(dst);
break;
case MSP_TRANSPONDER_CONFIG:
{
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 = 1; //transponder supported
uint8_t header = 0;
switch(transponderConfig()->provider){
case ILAP:
header |= 0x02;
break;
case ARCITIMER:
header |= 0x04;
break;
default:
break;
}
header |= (sizeof(transponderConfig()->data) << 4);
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);
sbufWriteU8(dst, header);
for (unsigned int i = 0; i < sizeof(transponderConfig()->data); i++) {
sbufWriteU8(dst, transponderConfig()->data[i]);
}
#else
sbufWriteU8(dst, 0); // Transponder not supported
sbufWriteU8(dst, 0 & TRANSPONDER_SUPPORTED_MASK);
#endif
break;
}
@ -1625,27 +1623,24 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
case MSP_SET_TRANSPONDER_CONFIG:
{
uint8_t tmp = sbufReadU8(src);
uint8_t bytesRemaining = dataSize - 1;
uint8_t type;
switch(tmp){
case 0x02:
type = ILAP;
break;
case 0x04:
type = ARCITIMER;
break;
}
uint8_t provider = (tmp & TRANSPONDER_PROVIDER_MASK) >> TRANSPONDER_PROVIDER_OFFSET;
uint8_t transponderDataSize = (tmp & TRANSPONDER_DATA_SIZE_MASK) >> TRANSPONDER_DATA_SIZE_OFFSET;
if(type != transponderConfig()->provider){
if(provider != transponderConfig()->provider) {
transponderStopRepeating();
}
transponderConfigMutable()->provider = type;
transponderConfigMutable()->provider = provider;
if (dataSize != sizeof(transponderConfig()->data) + 1) {
if (bytesRemaining != transponderDataSize || transponderDataSize > sizeof(transponderConfig()->data)) {
return MSP_RESULT_ERROR;
}
for (unsigned int i = 0; i < sizeof(transponderConfig()->data); i++) {
memset(transponderConfigMutable()->data, 0, sizeof(transponderConfig()->data));
for (unsigned int i = 0; i < transponderDataSize; i++) {
transponderConfigMutable()->data[i] = sbufReadU8(src);
}
transponderUpdateData();

View file

@ -40,8 +40,9 @@
PG_REGISTER_WITH_RESET_TEMPLATE(transponderConfig_t, transponderConfig, PG_TRANSPONDER_CONFIG, 0);
PG_RESET_TEMPLATE(transponderConfig_t, transponderConfig,
.provider = ILAP,
.reserved = 0,
.data = { 0x12, 0x34, 0x56, 0x78, 0x9A, 0xBC, 0x0, 0x0, 0x0 }, // Note, this is NOT a valid transponder code, it's just for testing production hardware
.provider = ILAP
);
static bool transponderInitialised = false;
@ -86,7 +87,7 @@ void transponderUpdate(timeUs_t currentTimeUs)
void transponderInit(void)
{
transponderInitialised = transponderIrInit(&(transponderConfig()->provider));
transponderInitialised = transponderIrInit(transponderConfig()->provider);
if (!transponderInitialised) {
return;
}

View file

@ -21,7 +21,8 @@
#include "config/parameter_group.h"
typedef struct transponderConfig_s {
TransponderProvider provider;
transponderProvider_e provider;
uint8_t reserved;
uint8_t data[9];
} transponderConfig_t;