Merge pull request #2480 from martinbudden/bf_transponder_config
Added transponder config
This commit is contained in:
commit
0d0a6169f6
|
@ -56,6 +56,7 @@
|
||||||
#include "io/osd.h"
|
#include "io/osd.h"
|
||||||
#include "io/serial.h"
|
#include "io/serial.h"
|
||||||
#include "io/servos.h"
|
#include "io/servos.h"
|
||||||
|
#include "io/transponder_ir.h"
|
||||||
#include "io/vtx.h"
|
#include "io/vtx.h"
|
||||||
|
|
||||||
#include "rx/rx.h"
|
#include "rx/rx.h"
|
||||||
|
@ -122,6 +123,7 @@
|
||||||
#define displayPortProfileOled(x) (&masterConfig.displayPortProfileOled)
|
#define displayPortProfileOled(x) (&masterConfig.displayPortProfileOled)
|
||||||
#define vtxConfig(x) (&masterConfig.vtxConfig)
|
#define vtxConfig(x) (&masterConfig.vtxConfig)
|
||||||
#define beeperConfig(x) (&masterConfig.beeperConfig)
|
#define beeperConfig(x) (&masterConfig.beeperConfig)
|
||||||
|
#define transponderConfig(x) (&masterConfig.transponderConfig)
|
||||||
|
|
||||||
|
|
||||||
#define featureConfigMutable(x) (&masterConfig.featureConfig)
|
#define featureConfigMutable(x) (&masterConfig.featureConfig)
|
||||||
|
@ -173,6 +175,7 @@
|
||||||
#define displayPortProfileOledMutable(x) (&masterConfig.displayPortProfileOled)
|
#define displayPortProfileOledMutable(x) (&masterConfig.displayPortProfileOled)
|
||||||
#define vtxConfigMutable(x) (&masterConfig.vtxConfig)
|
#define vtxConfigMutable(x) (&masterConfig.vtxConfig)
|
||||||
#define beeperConfigMutable(x) (&masterConfig.beeperConfig)
|
#define beeperConfigMutable(x) (&masterConfig.beeperConfig)
|
||||||
|
#define transponderConfigMutable(x) (&masterConfig.transponderConfig)
|
||||||
|
|
||||||
#define servoParams(x) (&servoProfile()->servoConf[x])
|
#define servoParams(x) (&servoProfile()->servoConf[x])
|
||||||
#define adjustmentRanges(x) (&adjustmentProfile()->adjustmentRanges[x])
|
#define adjustmentRanges(x) (&adjustmentProfile()->adjustmentRanges[x])
|
||||||
|
@ -278,7 +281,7 @@ typedef struct master_s {
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef TRANSPONDER
|
#ifdef TRANSPONDER
|
||||||
uint8_t transponderData[6];
|
transponderConfig_t transponderConfig;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef OSD
|
#ifdef OSD
|
||||||
|
|
|
@ -1019,7 +1019,7 @@ void createDefaultConfig(master_t *config)
|
||||||
#ifdef TRANSPONDER
|
#ifdef TRANSPONDER
|
||||||
static const uint8_t defaultTransponderData[6] = { 0x12, 0x34, 0x56, 0x78, 0x9A, 0xBC }; // Note, this is NOT a valid transponder code, it's just for testing production hardware
|
static const uint8_t defaultTransponderData[6] = { 0x12, 0x34, 0x56, 0x78, 0x9A, 0xBC }; // Note, this is NOT a valid transponder code, it's just for testing production hardware
|
||||||
|
|
||||||
memcpy(config->transponderData, &defaultTransponderData, sizeof(defaultTransponderData));
|
memcpy(config->transponderConfig.data, &defaultTransponderData, sizeof(defaultTransponderData));
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef BLACKBOX
|
#ifdef BLACKBOX
|
||||||
|
|
|
@ -481,7 +481,7 @@ void init(void)
|
||||||
|
|
||||||
#ifdef TRANSPONDER
|
#ifdef TRANSPONDER
|
||||||
if (feature(FEATURE_TRANSPONDER)) {
|
if (feature(FEATURE_TRANSPONDER)) {
|
||||||
transponderInit(masterConfig.transponderData);
|
transponderInit();
|
||||||
transponderStartRepeating();
|
transponderStartRepeating();
|
||||||
systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED;
|
systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED;
|
||||||
}
|
}
|
||||||
|
|
|
@ -1062,8 +1062,8 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
|
||||||
case MSP_TRANSPONDER_CONFIG:
|
case MSP_TRANSPONDER_CONFIG:
|
||||||
#ifdef TRANSPONDER
|
#ifdef TRANSPONDER
|
||||||
sbufWriteU8(dst, 1); //Transponder supported
|
sbufWriteU8(dst, 1); //Transponder supported
|
||||||
for (unsigned int i = 0; i < sizeof(masterConfig.transponderData); i++) {
|
for (unsigned int i = 0; i < sizeof(transponderConfig()->data); i++) {
|
||||||
sbufWriteU8(dst, masterConfig.transponderData[i]);
|
sbufWriteU8(dst, transponderConfig()->data[i]);
|
||||||
}
|
}
|
||||||
#else
|
#else
|
||||||
sbufWriteU8(dst, 0); // Transponder not supported
|
sbufWriteU8(dst, 0); // Transponder not supported
|
||||||
|
@ -1601,13 +1601,13 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
||||||
|
|
||||||
#ifdef TRANSPONDER
|
#ifdef TRANSPONDER
|
||||||
case MSP_SET_TRANSPONDER_CONFIG:
|
case MSP_SET_TRANSPONDER_CONFIG:
|
||||||
if (dataSize != sizeof(masterConfig.transponderData)) {
|
if (dataSize != sizeof(transponderConfig()->data)) {
|
||||||
return MSP_RESULT_ERROR;
|
return MSP_RESULT_ERROR;
|
||||||
}
|
}
|
||||||
for (unsigned int i = 0; i < sizeof(masterConfig.transponderData); i++) {
|
for (unsigned int i = 0; i < sizeof(transponderConfig()->data); i++) {
|
||||||
masterConfig.transponderData[i] = sbufReadU8(src);
|
transponderConfigMutable()->data[i] = sbufReadU8(src);
|
||||||
}
|
}
|
||||||
transponderUpdateData(masterConfig.transponderData);
|
transponderUpdateData();
|
||||||
break;
|
break;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
|
@ -23,7 +23,10 @@
|
||||||
|
|
||||||
#include <platform.h>
|
#include <platform.h>
|
||||||
|
|
||||||
#include <build/build_config.h>
|
#include "build/build_config.h"
|
||||||
|
|
||||||
|
#include "config/parameter_group.h"
|
||||||
|
#include "config/parameter_group_ids.h"
|
||||||
|
|
||||||
#include "drivers/transponder_ir.h"
|
#include "drivers/transponder_ir.h"
|
||||||
#include "drivers/system.h"
|
#include "drivers/system.h"
|
||||||
|
@ -73,14 +76,14 @@ void transponderUpdate(timeUs_t currentTimeUs)
|
||||||
transponderIrTransmit();
|
transponderIrTransmit();
|
||||||
}
|
}
|
||||||
|
|
||||||
void transponderInit(uint8_t* transponderData)
|
void transponderInit(void)
|
||||||
{
|
{
|
||||||
transponderInitialised = transponderIrInit();
|
transponderInitialised = transponderIrInit();
|
||||||
if (!transponderInitialised) {
|
if (!transponderInitialised) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
transponderIrUpdateData(transponderData);
|
transponderIrUpdateData(transponderConfig()->data);
|
||||||
}
|
}
|
||||||
|
|
||||||
void transponderStopRepeating(void)
|
void transponderStopRepeating(void)
|
||||||
|
@ -97,13 +100,13 @@ void transponderStartRepeating(void)
|
||||||
transponderRepeat = true;
|
transponderRepeat = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
void transponderUpdateData(uint8_t* transponderData)
|
void transponderUpdateData(void)
|
||||||
{
|
{
|
||||||
if (!transponderInitialised) {
|
if (!transponderInitialised) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
transponderIrUpdateData(transponderData);
|
transponderIrUpdateData(transponderConfig()->data);
|
||||||
}
|
}
|
||||||
|
|
||||||
void transponderTransmitOnce(void) {
|
void transponderTransmitOnce(void) {
|
||||||
|
|
|
@ -18,11 +18,18 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "common/time.h"
|
#include "common/time.h"
|
||||||
|
#include "config/parameter_group.h"
|
||||||
|
|
||||||
void transponderInit(uint8_t* transponderCode);
|
typedef struct transponderConfig_s {
|
||||||
|
uint8_t data[6];
|
||||||
|
} transponderConfig_t;
|
||||||
|
|
||||||
|
PG_DECLARE(transponderConfig_t, transponderConfig);
|
||||||
|
|
||||||
|
void transponderInit(void);
|
||||||
|
|
||||||
void transponderUpdate(timeUs_t currentTimeUs);
|
void transponderUpdate(timeUs_t currentTimeUs);
|
||||||
void transponderUpdateData(uint8_t* transponderData);
|
void transponderUpdateData(void);
|
||||||
void transponderTransmitOnce(void);
|
void transponderTransmitOnce(void);
|
||||||
void transponderStartRepeating(void);
|
void transponderStartRepeating(void);
|
||||||
void transponderStopRepeating(void);
|
void transponderStopRepeating(void);
|
||||||
|
|
Loading…
Reference in New Issue