Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 05:15:25 +03:00
Code Issues Wiki Activity

Corrected VTX vtables to not use static device handle

Browse source
This commit is contained in:
Martin Budden 2018-02-01 09:24:57 +00:00
parent f000d7bf10
commit e2683cd2dc
14 changed files with 259 additions and 281 deletions
Download patch file Download diff file Expand all files Collapse all files

86
src/main/drivers/vtx_common.c
View file

@ -30,114 +30,86 @@
#include "drivers/vtx_common.h" #include "drivers/vtx_common.h"
vtxDevice_t *vtxDevice = NULL; static vtxDevice_t *vtxDevice = NULL;
void vtxCommonInit(void) void vtxCommonInit(void)
{ {
} }
void vtxCommonRegisterDevice(vtxDevice_t *pDevice) void vtxCommonSetDevice(vtxDevice_t *pDevice)
{ {
vtxDevice = pDevice; vtxDevice = pDevice;
} }
bool vtxCommonDeviceRegistered(void) vtxDevice_t *vtxCommonDevice(void)
{ {
return vtxDevice; return vtxDevice;
} }
vtxDevType_e vtxCommonGetDeviceType(void) vtxDevType_e vtxCommonGetDeviceType(const vtxDevice_t *vtxDevice)
{ {
if (!vtxDevice || !vtxDevice->vTable->getDeviceType) { if (!vtxDevice) {
return VTXDEV_UNKNOWN; return VTXDEV_UNKNOWN;
} }
return vtxDevice->vTable->getDeviceType(vtxDevice);
return vtxDevice->vTable->getDeviceType();
} }
void vtxCommonProcess(timeUs_t currentTimeUs) { void vtxCommonProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
if (vtxDevice && vtxDevice->vTable->process) { {
vtxDevice->vTable->process(currentTimeUs); if (vtxDevice) {
vtxDevice->vTable->process(vtxDevice, currentTimeUs);
} }
} }
// band and channel are 1 origin // band and channel are 1 origin
void vtxCommonSetBandAndChannel(uint8_t band, uint8_t channel) void vtxCommonSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel)
{ {
if (vtxDevice && (band <= vtxDevice->capability.bandCount) && (channel <= vtxDevice->capability.channelCount)) { if (band <= vtxDevice->capability.bandCount && channel <= vtxDevice->capability.channelCount) {
if (vtxDevice->vTable->setBandAndChannel) { vtxDevice->vTable->setBandAndChannel(vtxDevice, band, channel);
vtxDevice->vTable->setBandAndChannel(band, channel);
}
} }
} }
// index is zero origin, zero = power off completely // index is zero origin, zero = power off completely
void vtxCommonSetPowerByIndex(uint8_t index) void vtxCommonSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index)
{ {
if (vtxDevice && (index < vtxDevice->capability.powerCount)) { if (index < vtxDevice->capability.powerCount) {
if (vtxDevice->vTable->setPowerByIndex) { vtxDevice->vTable->setPowerByIndex(vtxDevice, index);
vtxDevice->vTable->setPowerByIndex(index);
}
} }
} }
// on = 1, off = 0 // on = 1, off = 0
void vtxCommonSetPitMode(uint8_t onoff) void vtxCommonSetPitMode(vtxDevice_t *vtxDevice, uint8_t onOff)
{ {
if (vtxDevice && vtxDevice->vTable->setPitMode) { vtxDevice->vTable->setPitMode(vtxDevice, onOff);
vtxDevice->vTable->setPitMode(onoff);
}
} }
void vtxCommonSetFrequency(uint16_t freq) void vtxCommonSetFrequency(vtxDevice_t *vtxDevice, uint16_t frequency)
{ {
if (vtxDevice && vtxDevice->vTable->setFrequency) { vtxDevice->vTable->setFrequency(vtxDevice, frequency);
vtxDevice->vTable->setFrequency(freq);
}
} }
bool vtxCommonGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel) bool vtxCommonGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel)
{ {
if (vtxDevice && vtxDevice->vTable->getBandAndChannel) { return vtxDevice->vTable->getBandAndChannel(vtxDevice, pBand, pChannel);
return vtxDevice->vTable->getBandAndChannel(pBand, pChannel);
} else {
return false;
}
} }
bool vtxCommonGetPowerIndex(uint8_t *pIndex) bool vtxCommonGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
{ {
if (vtxDevice && vtxDevice->vTable->getPowerIndex) { return vtxDevice->vTable->getPowerIndex(vtxDevice, pIndex);
return vtxDevice->vTable->getPowerIndex(pIndex);
} else {
return false;
}
} }
bool vtxCommonGetPitMode(uint8_t *pOnOff) bool vtxCommonGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff)
{ {
if (vtxDevice && vtxDevice->vTable->getPitMode) { return vtxDevice->vTable->getPitMode(vtxDevice, pOnOff);
return vtxDevice->vTable->getPitMode(pOnOff);
} else {
return false;
}
} }
bool vtxCommonGetFrequency(uint16_t *pFreq) bool vtxCommonGetFrequency(const vtxDevice_t *vtxDevice, uint16_t *pFrequency)
{ {
if (vtxDevice && vtxDevice->vTable->getFrequency) { return vtxDevice->vTable->getFrequency(vtxDevice, pFrequency);
return vtxDevice->vTable->getFrequency(pFreq);
} else {
return false;
}
} }
bool vtxCommonGetDeviceCapability(vtxDeviceCapability_t *pDeviceCapability) bool vtxCommonGetDeviceCapability(const vtxDevice_t *vtxDevice, vtxDeviceCapability_t *pDeviceCapability)
{ {
if (!vtxDevice) {
return false;
}
memcpy(pDeviceCapability, &vtxDevice->capability, sizeof(vtxDeviceCapability_t)); memcpy(pDeviceCapability, &vtxDevice->capability, sizeof(vtxDeviceCapability_t));
return true; return true;
} }

71
src/main/drivers/vtx_common.h
View file

@ -110,30 +110,12 @@ typedef struct vtxDeviceCapability_s {
uint8_t bandCount; uint8_t bandCount;
uint8_t channelCount; uint8_t channelCount;
uint8_t powerCount; uint8_t powerCount;
uint8_t filler;
} vtxDeviceCapability_t; } vtxDeviceCapability_t;
// {set,get}BandAndChannel: band and channel are 1 origin struct vtxVTable_s;
// {set,get}PowerByIndex: 0 = Power OFF, 1 = device dependent
// {set,get}PitMode: 0 = OFF, 1 = ON
typedef struct vtxVTable_s {
void (*process)(timeUs_t currentTimeUs);
vtxDevType_e (*getDeviceType)(void);
bool (*isReady)(void);
void (*setBandAndChannel)(uint8_t band, uint8_t channel);
void (*setPowerByIndex)(uint8_t level);
void (*setPitMode)(uint8_t onoff);
void (*setFrequency)(uint16_t freq);
bool (*getBandAndChannel)(uint8_t *pBand, uint8_t *pChannel);
bool (*getPowerIndex)(uint8_t *pIndex);
bool (*getPitMode)(uint8_t *pOnOff);
bool (*getFrequency)(uint16_t *pFreq);
} vtxVTable_t;
typedef struct vtxDevice_s { typedef struct vtxDevice_s {
const vtxVTable_t * const vTable; const struct vtxVTable_s * const vTable;
vtxDeviceCapability_t capability; vtxDeviceCapability_t capability;
@ -142,6 +124,7 @@ typedef struct vtxDevice_s {
char **channelNames; // char *channelNames[channelCount] char **channelNames; // char *channelNames[channelCount]
char **powerNames; // char *powerNames[powerCount] char **powerNames; // char *powerNames[powerCount]
uint16_t frequency;
uint8_t band; // Band = 1, 1-based uint8_t band; // Band = 1, 1-based
uint8_t channel; // CH1 = 1, 1-based uint8_t channel; // CH1 = 1, 1-based
uint8_t powerIndex; // Lowest/Off = 0 uint8_t powerIndex; // Lowest/Off = 0
@ -149,24 +132,44 @@ typedef struct vtxDevice_s {
} vtxDevice_t; } vtxDevice_t;
// {set,get}BandAndChannel: band and channel are 1 origin
// {set,get}PowerByIndex: 0 = Power OFF, 1 = device dependent
// {set,get}PitMode: 0 = OFF, 1 = ON
typedef struct vtxVTable_s {
void (*process)(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs);
vtxDevType_e (*getDeviceType)(const vtxDevice_t *vtxDevice);
bool (*isReady)(const vtxDevice_t *vtxDevice);
void (*setBandAndChannel)(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel);
void (*setPowerByIndex)(vtxDevice_t *vtxDevice, uint8_t level);
void (*setPitMode)(vtxDevice_t *vtxDevice, uint8_t onoff);
void (*setFrequency)(vtxDevice_t *vtxDevice, uint16_t freq);
bool (*getBandAndChannel)(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel);
bool (*getPowerIndex)(const vtxDevice_t *vtxDevice, uint8_t *pIndex);
bool (*getPitMode)(const vtxDevice_t *vtxDevice, uint8_t *pOnOff);
bool (*getFrequency)(const vtxDevice_t *vtxDevice, uint16_t *pFreq);
} vtxVTable_t;
// 3.1.0 // 3.1.0
// PIT mode is defined as LOWEST POSSIBLE RF POWER. // PIT mode is defined as LOWEST POSSIBLE RF POWER.
// - It can be a dedicated mode, or lowest RF power possible. // - It can be a dedicated mode, or lowest RF power possible.
// - It is *NOT* RF on/off control ? // - It is *NOT* RF on/off control ?
void vtxCommonInit(void); void vtxCommonInit(void);
void vtxCommonRegisterDevice(vtxDevice_t *pDevice); void vtxCommonSetDevice(vtxDevice_t *vtxDevice);
bool vtxCommonDeviceRegistered(void); vtxDevice_t *vtxCommonDevice(void);
// VTable functions // VTable functions
void vtxCommonProcess(timeUs_t currentTimeUs); void vtxCommonProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs);
uint8_t vtxCommonGetDeviceType(void); uint8_t vtxCommonGetDeviceType(const vtxDevice_t *vtxDevice);
void vtxCommonSetBandAndChannel(uint8_t band, uint8_t channel); void vtxCommonSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel);
void vtxCommonSetPowerByIndex(uint8_t level); void vtxCommonSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t level);
void vtxCommonSetPitMode(uint8_t onoff); void vtxCommonSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff);
void vtxCommonSetFrequency(uint16_t freq); void vtxCommonSetFrequency(vtxDevice_t *vtxDevice, uint16_t freq);
bool vtxCommonGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel); bool vtxCommonGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel);
bool vtxCommonGetPowerIndex(uint8_t *pIndex); bool vtxCommonGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex);
bool vtxCommonGetPitMode(uint8_t *pOnOff); bool vtxCommonGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff);
bool vtxCommonGetFrequency(uint16_t *pFreq); bool vtxCommonGetFrequency(const vtxDevice_t *vtxDevice, uint16_t *pFreq);
bool vtxCommonGetDeviceCapability(vtxDeviceCapability_t *pDeviceCapability); bool vtxCommonGetDeviceCapability(const vtxDevice_t *vtxDevice, vtxDeviceCapability_t *pDeviceCapability);

2
src/main/fc/fc_init.c
View file

@ -704,7 +704,7 @@ void init(void)
#ifdef USE_VTX_RTC6705 #ifdef USE_VTX_RTC6705
#ifdef VTX_RTC6705_OPTIONAL #ifdef VTX_RTC6705_OPTIONAL
if (!vtxCommonDeviceRegistered()) // external VTX takes precedence when configured. if (!vtxCommonDevice()) // external VTX takes precedence when configured.
#endif #endif
{ {
vtxRTC6705Init(); vtxRTC6705Init();

17
src/main/interface/msp.c
View file

@ -1211,11 +1211,11 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
#if defined(USE_VTX_COMMON) #if defined(USE_VTX_COMMON)
case MSP_VTX_CONFIG: case MSP_VTX_CONFIG:
{ {
uint8_t deviceType = vtxCommonGetDeviceType(); const vtxDevice_t *vtxDevice = vtxCommonDevice();
if (deviceType != VTXDEV_UNKNOWN) { if (vtxDevice) {
uint8_t pitmode=0; uint8_t pitmode=0;
vtxCommonGetPitMode(&pitmode); vtxCommonGetPitMode(vtxDevice, &pitmode);
sbufWriteU8(dst, deviceType); sbufWriteU8(dst, vtxCommonGetDeviceType(vtxDevice));
sbufWriteU8(dst, vtxSettingsConfig()->band); sbufWriteU8(dst, vtxSettingsConfig()->band);
sbufWriteU8(dst, vtxSettingsConfig()->channel); sbufWriteU8(dst, vtxSettingsConfig()->channel);
sbufWriteU8(dst, vtxSettingsConfig()->power); sbufWriteU8(dst, vtxSettingsConfig()->power);
@ -1728,8 +1728,9 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
#ifdef USE_VTX_COMMON #ifdef USE_VTX_COMMON
case MSP_SET_VTX_CONFIG: case MSP_SET_VTX_CONFIG:
{ {
if (vtxCommonDeviceRegistered()) { vtxDevice_t *vtxDevice = vtxCommonDevice();
if (vtxCommonGetDeviceType() != VTXDEV_UNKNOWN) { if (vtxDevice) {
if (vtxCommonGetDeviceType(vtxDevice) != VTXDEV_UNKNOWN) {
uint16_t newFrequency = sbufReadU16(src); uint16_t newFrequency = sbufReadU16(src);
if (newFrequency <= VTXCOMMON_MSP_BANDCHAN_CHKVAL) { //value is band and channel if (newFrequency <= VTXCOMMON_MSP_BANDCHAN_CHKVAL) { //value is band and channel
const uint8_t newBand = (newFrequency / 8) + 1; const uint8_t newBand = (newFrequency / 8) + 1;
@ -1747,9 +1748,9 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
// Delegate pitmode to vtx directly // Delegate pitmode to vtx directly
const uint8_t newPitmode = sbufReadU8(src); const uint8_t newPitmode = sbufReadU8(src);
uint8_t currentPitmode = 0; uint8_t currentPitmode = 0;
vtxCommonGetPitMode(&currentPitmode); vtxCommonGetPitMode(vtxDevice, &currentPitmode);
if (currentPitmode != newPitmode) { if (currentPitmode != newPitmode) {
vtxCommonSetPitMode(newPitmode); vtxCommonSetPitMode(vtxDevice, newPitmode);
} }
} }
} }

9
src/main/io/ledstrip.c
View file

@ -588,7 +588,8 @@ static void applyLedVtxLayer(bool updateNow, timeUs_t *timer)
static uint16_t lastCheck = 0; static uint16_t lastCheck = 0;
static bool blink = false; static bool blink = false;
if (!vtxCommonDeviceRegistered()) { const vtxDevice_t *vtxDevice = vtxCommonDevice();
if (!vtxDevice) {
return; return;
} }
@ -597,9 +598,9 @@ static void applyLedVtxLayer(bool updateNow, timeUs_t *timer)
if (updateNow) { if (updateNow) {
// keep counter running, so it stays in sync with vtx // keep counter running, so it stays in sync with vtx
vtxCommonGetBandAndChannel(&band, &channel); vtxCommonGetBandAndChannel(vtxDevice, &band, &channel);
vtxCommonGetPowerIndex(&power); vtxCommonGetPowerIndex(vtxDevice, &power);
vtxCommonGetPitMode(&pit); vtxCommonGetPitMode(vtxDevice, &pit);
frequency = vtx58frequencyTable[band - 1][channel - 1]; //subtracting 1 from band and channel so that correct frequency is returned. frequency = vtx58frequencyTable[band - 1][channel - 1]; //subtracting 1 from band and channel so that correct frequency is returned.
//might not be correct for tramp but should fix smart audio. //might not be correct for tramp but should fix smart audio.

5
src/main/io/osd.c
View file

@ -549,8 +549,9 @@ static bool osdDrawSingleElement(uint8_t item)
const char vtxBandLetter = vtx58BandLetter[vtxSettingsConfig()->band]; const char vtxBandLetter = vtx58BandLetter[vtxSettingsConfig()->band];
const char *vtxChannelName = vtx58ChannelNames[vtxSettingsConfig()->channel]; const char *vtxChannelName = vtx58ChannelNames[vtxSettingsConfig()->channel];
uint8_t vtxPower = vtxSettingsConfig()->power; uint8_t vtxPower = vtxSettingsConfig()->power;
if (vtxSettingsConfig()->lowPowerDisarm) { const vtxDevice_t *vtxDevice = vtxCommonDevice();
vtxCommonGetPowerIndex(&vtxPower); if (vtxDevice && vtxSettingsConfig()->lowPowerDisarm) {
vtxCommonGetPowerIndex(vtxDevice, &vtxPower);
} }
tfp_sprintf(buff, "%c:%s:%2d", vtxBandLetter, vtxChannelName, vtxPower); tfp_sprintf(buff, "%c:%s:%2d", vtxBandLetter, vtxChannelName, vtxPower);
break; break;

26
src/main/io/spektrum_vtx_control.c
View file

@ -101,7 +101,8 @@ uint8_t convertSpektrumVtxPowerIndex(uint8_t sPower)
{ {
uint8_t devicePower = 0; uint8_t devicePower = 0;
switch (vtxCommonGetDeviceType()) { const vtxDevice_t *vtxDevice = vtxCommonDevice();
switch (vtxCommonGetDeviceType(vtxDevice)) {
#ifdef USE_VTX_RTC6705 #ifdef USE_VTX_RTC6705
case VTXDEV_RTC6705: case VTXDEV_RTC6705:
devicePower = vtxRTC6705Pi[sPower]; devicePower = vtxRTC6705Pi[sPower];
@ -174,8 +175,8 @@ void spektrumVtxControl(void)
}; };
vtxSettingsConfig_t newSettings = prevSettings; vtxSettingsConfig_t newSettings = prevSettings;
if (vtxCommonDeviceRegistered()) { vtxDevice_t *vtxDevice = vtxCommonDevice();
if (vtxDevice) {
#ifdef USE_VTX_COMMON_FREQ_API #ifdef USE_VTX_COMMON_FREQ_API
uint16_t freq = SpektrumVtxfrequencyTable[vtx.band][vtx.channel]; uint16_t freq = SpektrumVtxfrequencyTable[vtx.band][vtx.channel];
if (prevSettings.freq != freq) { if (prevSettings.freq != freq) {
@ -183,34 +184,31 @@ void spektrumVtxControl(void)
newSettings.channel = vtx.channel; newSettings.channel = vtx.channel;
newSettings.freq = freq; newSettings.freq = freq;
} }
#else #else
// Convert to the internal Common Band index // Convert to the internal Common Band index
uint8_t band = spek2commonBand[vtx.band]; const uint8_t band = spek2commonBand[vtx.band];
uint8_t channel = vtx.channel +1; // 0 based to 1 based const uint8_t channel = vtx.channel +1; // 0 based to 1 based
if ((prevSettings.band != band) || (prevSettings.channel != channel)) { if ((prevSettings.band != band) || (prevSettings.channel != channel)) {
newSettings.band = band; newSettings.band = band;
newSettings.channel = channel; newSettings.channel = channel;
newSettings.freq = vtx58_Bandchan2Freq(band, channel); newSettings.freq = vtx58_Bandchan2Freq(band, channel);
} }
#endif #endif
// Seems to be no unified internal VTX API standard for power levels/indexes, VTX device brand specific.
// Seems to be no unified internal VTX API std for power levels/indexes, VTX device brand specific. const uint8_t power = convertSpektrumVtxPowerIndex(vtx.power);
uint8_t power = convertSpektrumVtxPowerIndex(vtx.power);
if (prevSettings.power != power) { if (prevSettings.power != power) {
newSettings.power = power; newSettings.power = power;
} }
// Everyone seems to agree on what PIT ON/OFF means // Everyone seems to agree on what PIT ON/OFF means
uint8_t currentPitMode = 0; uint8_t currentPitMode = 0;
vtxCommonGetPitMode(&currentPitMode); vtxCommonGetPitMode(vtxDevice, &currentPitMode);
if (currentPitMode != vtx.pitMode) { if (currentPitMode != vtx.pitMode) {
vtxCommonSetPitMode(vtx.pitMode); vtxCommonSetPitMode(vtxDevice, vtx.pitMode);
} }
} }
if(memcmp(&prevSettings,&newSettings,sizeof(vtxSettingsConfig_t))) { if (memcmp(&prevSettings,&newSettings,sizeof(vtxSettingsConfig_t))) {
vtxSettingsConfigMutable()->band = newSettings.band; vtxSettingsConfigMutable()->band = newSettings.band;
vtxSettingsConfigMutable()->channel = newSettings.channel; vtxSettingsConfigMutable()->channel = newSettings.channel;
vtxSettingsConfigMutable()->power = newSettings.power; vtxSettingsConfigMutable()->power = newSettings.power;
vtxSettingsConfigMutable()->freq = newSettings.freq; vtxSettingsConfigMutable()->freq = newSettings.freq;

72
src/main/io/vtx.c
View file

@ -22,17 +22,14 @@
#if defined(USE_VTX_COMMON) #if defined(USE_VTX_COMMON)
#include "common/time.h"
#include "common/maths.h" #include "common/maths.h"
#include "common/time.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "drivers/vtx_common.h" #include "drivers/vtx_common.h"
#include "fc/config.h" #include "fc/config.h"
#include "fc/runtime_config.h"
#include "fc/rc_modes.h" #include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "io/vtx.h" #include "io/vtx.h"
#include "io/vtx_string.h" #include "io/vtx_string.h"
@ -40,6 +37,9 @@
#include "interface/cli.h" #include "interface/cli.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
PG_REGISTER_WITH_RESET_TEMPLATE(vtxSettingsConfig_t, vtxSettingsConfig, PG_VTX_SETTINGS_CONFIG, 0); PG_REGISTER_WITH_RESET_TEMPLATE(vtxSettingsConfig_t, vtxSettingsConfig, PG_VTX_SETTINGS_CONFIG, 0);
@ -87,7 +87,8 @@ void vtxInit(void)
} }
} }
static vtxSettingsConfig_t vtxGetSettings(void) { static vtxSettingsConfig_t vtxGetSettings(void)
{
vtxSettingsConfig_t settings = { vtxSettingsConfig_t settings = {
.band = vtxSettingsConfig()->band, .band = vtxSettingsConfig()->band,
.channel = vtxSettingsConfig()->channel, .channel = vtxSettingsConfig()->channel,
@ -112,14 +113,15 @@ static vtxSettingsConfig_t vtxGetSettings(void) {
return settings; return settings;
} }
static bool vtxProcessBandAndChannel(void) { static bool vtxProcessBandAndChannel(vtxDevice_t *vtxDevice)
{
if(!ARMING_FLAG(ARMED)) { if(!ARMING_FLAG(ARMED)) {
uint8_t vtxBand; uint8_t vtxBand;
uint8_t vtxChan; uint8_t vtxChan;
if (vtxCommonGetBandAndChannel(&vtxBand, &vtxChan)) { if (vtxCommonGetBandAndChannel(vtxDevice, &vtxBand, &vtxChan)) {
const vtxSettingsConfig_t settings = vtxGetSettings(); const vtxSettingsConfig_t settings = vtxGetSettings();
if (vtxBand != settings.band || vtxChan != settings.channel) { if (vtxBand != settings.band || vtxChan != settings.channel) {
vtxCommonSetBandAndChannel(settings.band, settings.channel); vtxCommonSetBandAndChannel(vtxDevice, settings.band, settings.channel);
return true; return true;
} }
} }
@ -128,13 +130,14 @@ static bool vtxProcessBandAndChannel(void) {
} }
#if defined(VTX_SETTINGS_FREQCMD) #if defined(VTX_SETTINGS_FREQCMD)
static bool vtxProcessFrequency(void) { static bool vtxProcessFrequency(vtxDevice_t *vtxDevice)
{
if(!ARMING_FLAG(ARMED)) { if(!ARMING_FLAG(ARMED)) {
uint16_t vtxFreq; uint16_t vtxFreq;
if (vtxCommonGetFrequency(&vtxFreq)) { if (vtxCommonGetFrequency(vtxDevice, &vtxFreq)) {
const vtxSettingsConfig_t settings = vtxGetSettings(); const vtxSettingsConfig_t settings = vtxGetSettings();
if (vtxFreq != settings.freq) { if (vtxFreq != settings.freq) {
vtxCommonSetFrequency(settings.freq); vtxCommonSetFrequency(vtxDevice, settings.freq);
return true; return true;
} }
} }
@ -143,21 +146,23 @@ static bool vtxProcessFrequency(void) {
} }
#endif #endif
static bool vtxProcessPower(void) { static bool vtxProcessPower(vtxDevice_t *vtxDevice)
{
uint8_t vtxPower; uint8_t vtxPower;
if (vtxCommonGetPowerIndex(&vtxPower)) { if (vtxCommonGetPowerIndex(vtxDevice, &vtxPower)) {
const vtxSettingsConfig_t settings = vtxGetSettings(); const vtxSettingsConfig_t settings = vtxGetSettings();
if (vtxPower != settings.power) { if (vtxPower != settings.power) {
vtxCommonSetPowerByIndex(settings.power); vtxCommonSetPowerByIndex(vtxDevice, settings.power);
return true; return true;
} }
} }
return false; return false;
} }
static bool vtxProcessPitMode(void) { static bool vtxProcessPitMode(vtxDevice_t *vtxDevice)
{
uint8_t pitOnOff; uint8_t pitOnOff;
if (!ARMING_FLAG(ARMED) && vtxCommonGetPitMode(&pitOnOff)) { if (!ARMING_FLAG(ARMED) && vtxCommonGetPitMode(vtxDevice, &pitOnOff)) {
if (IS_RC_MODE_ACTIVE(BOXVTXPITMODE)) { if (IS_RC_MODE_ACTIVE(BOXVTXPITMODE)) {
#if defined(VTX_SETTINGS_FREQCMD) #if defined(VTX_SETTINGS_FREQCMD)
if (vtxSettingsConfig()->pitModeFreq) { if (vtxSettingsConfig()->pitModeFreq) {
@ -166,12 +171,12 @@ static bool vtxProcessPitMode(void) {
#endif #endif
if (isModeActivationConditionPresent(BOXVTXPITMODE)) { if (isModeActivationConditionPresent(BOXVTXPITMODE)) {
if (!pitOnOff) { if (!pitOnOff) {
vtxCommonSetPitMode(true); vtxCommonSetPitMode(vtxDevice, true);
return true; return true;
} }
} else { } else {
if (pitOnOff) { if (pitOnOff) {
vtxCommonSetPitMode(false); vtxCommonSetPitMode(vtxDevice, false);
return true; return true;
} }
} }
@ -180,22 +185,22 @@ static bool vtxProcessPitMode(void) {
return false; return false;
} }
static bool vtxProcessStateUpdate(void) static bool vtxProcessStateUpdate(vtxDevice_t *vtxDevice)
{ {
const vtxSettingsConfig_t vtxSettingsState = vtxGetSettings(); const vtxSettingsConfig_t vtxSettingsState = vtxGetSettings();
vtxSettingsConfig_t vtxState = vtxSettingsState; vtxSettingsConfig_t vtxState = vtxSettingsState;
if (vtxSettingsState.band) { if (vtxSettingsState.band) {
vtxCommonGetBandAndChannel(&vtxState.band, &vtxState.channel); vtxCommonGetBandAndChannel(vtxDevice, &vtxState.band, &vtxState.channel);
#if defined(VTX_SETTINGS_FREQCMD) #if defined(VTX_SETTINGS_FREQCMD)
} else { } else {
vtxCommonGetFrequency(&vtxState.freq); vtxCommonGetFrequency(vtxDevice, &vtxState.freq);
#endif #endif
} }
vtxCommonGetPowerIndex(&vtxState.power); vtxCommonGetPowerIndex(vtxDevice, &vtxState.power);
return (bool) memcmp(&vtxSettingsState, &vtxState, sizeof(vtxSettingsConfig_t)); return (bool)memcmp(&vtxSettingsState, &vtxState, sizeof(vtxSettingsConfig_t));
} }
void vtxUpdate(timeUs_t currentTimeUs) void vtxUpdate(timeUs_t currentTimeUs)
@ -206,36 +211,37 @@ void vtxUpdate(timeUs_t currentTimeUs)
return; return;
} }
// Check input sources for config updates vtxDevice_t *vtxDevice = vtxCommonDevice();
vtxControlInputPoll(); if (vtxDevice) {
// Check input sources for config updates
vtxControlInputPoll();
if (vtxCommonDeviceRegistered()) {
bool vtxUpdatePending = false; bool vtxUpdatePending = false;
switch (currentSchedule) { switch (currentSchedule) {
case VTX_PARAM_POWER: case VTX_PARAM_POWER:
vtxUpdatePending = vtxProcessPower(); vtxUpdatePending = vtxProcessPower(vtxDevice);
break; break;
case VTX_PARAM_BANDCHAN: case VTX_PARAM_BANDCHAN:
if (vtxGetSettings().band) { if (vtxGetSettings().band) {
vtxUpdatePending = vtxProcessBandAndChannel(); vtxUpdatePending = vtxProcessBandAndChannel(vtxDevice);
#if defined(VTX_SETTINGS_FREQCMD) #if defined(VTX_SETTINGS_FREQCMD)
} else { } else {
vtxUpdatePending = vtxProcessFrequency(); vtxUpdatePending = vtxProcessFrequency(vtxDevice);
#endif #endif
} }
break; break;
case VTX_PARAM_PITMODE: case VTX_PARAM_PITMODE:
vtxUpdatePending = vtxProcessPitMode(); vtxUpdatePending = vtxProcessPitMode(vtxDevice);
break; break;
case VTX_PARAM_CONFIRM: case VTX_PARAM_CONFIRM:
vtxUpdatePending = vtxProcessStateUpdate(); vtxUpdatePending = vtxProcessStateUpdate(vtxDevice);
break; break;
default: default:
break; break;
} }
currentSchedule = (currentSchedule + 1) % VTX_PARAM_COUNT; currentSchedule = (currentSchedule + 1) % VTX_PARAM_COUNT;
if (!ARMING_FLAG(ARMED) || vtxUpdatePending) { if (!ARMING_FLAG(ARMED) || vtxUpdatePending) {
vtxCommonProcess(currentTimeUs); vtxCommonProcess(vtxDevice, currentTimeUs);
} }
} }
} }

31
src/main/io/vtx_control.c
View file

@ -26,8 +26,6 @@
#include "common/maths.h" #include "common/maths.h"
#include "config/config_eeprom.h" #include "config/config_eeprom.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "drivers/buttons.h" #include "drivers/buttons.h"
#include "drivers/light_led.h" #include "drivers/light_led.h"
@ -38,11 +36,12 @@
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "io/osd.h" #include "io/osd.h"
#include "io/vtx_control.h"
#include "io/vtx.h"
#include "io/spektrum_vtx_control.h" #include "io/spektrum_vtx_control.h"
#include "io/vtx.h"
#include "io/vtx_control.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 1); PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 1);
@ -54,6 +53,7 @@ PG_RESET_TEMPLATE(vtxConfig_t, vtxConfig,
static uint8_t locked = 0; static uint8_t locked = 0;
void vtxControlInit(void) void vtxControlInit(void)
{ {
// NOTHING TO DO // NOTHING TO DO
@ -74,7 +74,7 @@ static void vtxUpdateBandAndChannel(uint8_t bandStep, uint8_t channelStep)
locked = 1; locked = 1;
} }
if (!locked && vtxCommonDeviceRegistered()) { if (!locked && vtxCommonDevice()) {
vtxSettingsConfigMutable()->band += bandStep; vtxSettingsConfigMutable()->band += bandStep;
vtxSettingsConfigMutable()->channel += channelStep; vtxSettingsConfigMutable()->channel += channelStep;
} }
@ -106,7 +106,7 @@ void vtxUpdateActivatedChannel(void)
locked = 1; locked = 1;
} }
if (!locked && vtxCommonDeviceRegistered()) { if (!locked && vtxCommonDevice()) {
static uint8_t lastIndex = -1; static uint8_t lastIndex = -1;
for (uint8_t index = 0; index < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; index++) { for (uint8_t index = 0; index < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; index++) {
@ -126,11 +126,12 @@ void vtxUpdateActivatedChannel(void)
void vtxCycleBandOrChannel(const uint8_t bandStep, const uint8_t channelStep) void vtxCycleBandOrChannel(const uint8_t bandStep, const uint8_t channelStep)
{ {
if (vtxCommonDeviceRegistered()) { const vtxDevice_t *vtxDevice = vtxCommonDevice();
if (vtxDevice) {
uint8_t band = 0, channel = 0; uint8_t band = 0, channel = 0;
vtxDeviceCapability_t capability; vtxDeviceCapability_t capability;
bool haveAllNeededInfo = vtxCommonGetBandAndChannel(&band, &channel) && vtxCommonGetDeviceCapability(&capability); const bool haveAllNeededInfo = vtxCommonGetBandAndChannel(vtxDevice, &band, &channel) && vtxCommonGetDeviceCapability(vtxDevice, &capability);
if (!haveAllNeededInfo) { if (!haveAllNeededInfo) {
return; return;
} }
@ -156,11 +157,11 @@ void vtxCycleBandOrChannel(const uint8_t bandStep, const uint8_t channelStep)
void vtxCyclePower(const uint8_t powerStep) void vtxCyclePower(const uint8_t powerStep)
{ {
if (vtxCommonDeviceRegistered()) { const vtxDevice_t *vtxDevice = vtxCommonDevice();
if (vtxDevice) {
uint8_t power = 0; uint8_t power = 0;
vtxDeviceCapability_t capability; vtxDeviceCapability_t capability;
const bool haveAllNeededInfo = vtxCommonGetPowerIndex(vtxDevice, &power) && vtxCommonGetDeviceCapability(vtxDevice, &capability);
bool haveAllNeededInfo = vtxCommonGetPowerIndex(&power) && vtxCommonGetDeviceCapability(&capability);
if (!haveAllNeededInfo) { if (!haveAllNeededInfo) {
return; return;
} }
@ -196,15 +197,15 @@ void handleVTXControlButton(void)
{ {
#if defined(USE_VTX_RTC6705) && defined(BUTTON_A_PIN) #if defined(USE_VTX_RTC6705) && defined(BUTTON_A_PIN)
bool buttonWasPressed = false; bool buttonWasPressed = false;
uint32_t start = millis(); const timeMs_t start = millis();
uint32_t ledToggleAt = start; timeMs_t ledToggleAt = start;
bool ledEnabled = false; bool ledEnabled = false;
uint8_t flashesDone = 0; uint8_t flashesDone = 0;
uint8_t actionCounter = 0; uint8_t actionCounter = 0;
bool buttonHeld; bool buttonHeld;
while ((buttonHeld = buttonAPressed())) { while ((buttonHeld = buttonAPressed())) {
uint32_t end = millis(); const timeMs_t end = millis();
int32_t diff = cmp32(end, start); int32_t diff = cmp32(end, start);
if (diff > 25 && diff <= 1000) { if (diff > 25 && diff <= 1000) {

107
src/main/io/vtx_rtc6705.c
View file

@ -24,33 +24,19 @@
#if defined(USE_VTX_RTC6705) && defined(USE_VTX_CONTROL) #if defined(USE_VTX_RTC6705) && defined(USE_VTX_CONTROL)
#include "build/build_config.h"
#include "build/debug.h"
#include "cms/cms.h"
#include "cms/cms_types.h"
#include "common/maths.h" #include "common/maths.h"
#include "common/utils.h" #include "common/utils.h"
#include "config/feature.h" #include "config/feature.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "drivers/max7456.h" #include "drivers/max7456.h"
#include "drivers/system.h"
#include "drivers/time.h" #include "drivers/time.h"
#include "drivers/vtx_rtc6705.h" #include "drivers/vtx_rtc6705.h"
#include "drivers/vtx_common.h"
#include "fc/config.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "io/vtx.h"
#include "io/vtx_rtc6705.h" #include "io/vtx_rtc6705.h"
#include "io/vtx_string.h" #include "io/vtx_string.h"
#if defined(USE_CMS) || defined(USE_VTX_COMMON) #if defined(USE_CMS) || defined(USE_VTX_COMMON)
const char * const rtc6705PowerNames[] = { const char * const rtc6705PowerNames[] = {
"OFF", "MIN", "MAX" "OFF", "MIN", "MAX"
@ -70,21 +56,16 @@ static vtxDevice_t vtxRTC6705 = {
}; };
#endif #endif
static void vtxRTC6705SetBandAndChannel(uint8_t band, uint8_t channel); static void vtxRTC6705SetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel);
static void vtxRTC6705SetFrequency(vtxDevice_t *vtxDevice, uint16_t frequency);
bool vtxRTC6705Init(void) bool vtxRTC6705Init(void)
{ {
vtxCommonRegisterDevice(&vtxRTC6705); vtxCommonSetDevice(&vtxRTC6705);
return true; return true;
} }
void vtxRTC6705Configure(void)
{
rtc6705SetRFPower(vtxRTC6705.powerIndex);
vtxRTC6705SetBandAndChannel(vtxRTC6705.band, vtxRTC6705.channel);
}
bool vtxRTC6705CanUpdate(void) bool vtxRTC6705CanUpdate(void)
{ {
#if defined(MAX7456_SPI_INSTANCE) && defined(RTC6705_SPI_INSTANCE) && defined(SPI_SHARED_MAX7456_AND_RTC6705) #if defined(MAX7456_SPI_INSTANCE) && defined(RTC6705_SPI_INSTANCE) && defined(SPI_SHARED_MAX7456_AND_RTC6705)
@ -96,7 +77,13 @@ bool vtxRTC6705CanUpdate(void)
} }
#ifdef RTC6705_POWER_PIN #ifdef RTC6705_POWER_PIN
static void vtxRTC6705EnableAndConfigure(void) static void vtxRTC6705Configure(vtxDevice_t *vtxDevice)
{
rtc6705SetRFPower(vtxDevice->powerIndex);
vtxRTC6705SetBandAndChannel(vtxDevice, vtxDevice->band, vtxDevice->channel);
}
static void vtxRTC6705EnableAndConfigure(vtxDevice_t *vtxDevice)
{ {
while (!vtxRTC6705CanUpdate()); while (!vtxRTC6705CanUpdate());
@ -104,51 +91,53 @@ static void vtxRTC6705EnableAndConfigure(void)
delay(VTX_RTC6705_BOOT_DELAY); delay(VTX_RTC6705_BOOT_DELAY);
vtxRTC6705Configure(); vtxRTC6705Configure(vtxDevice);
} }
#endif #endif
void vtxRTC6705Process(timeUs_t now) static void vtxRTC6705Process(vtxDevice_t *vtxDevice, timeUs_t now)
{ {
UNUSED(vtxDevice);
UNUSED(now); UNUSED(now);
} }
#ifdef USE_VTX_COMMON #ifdef USE_VTX_COMMON
// Interface to common VTX API // Interface to common VTX API
vtxDevType_e vtxRTC6705GetDeviceType(void) static vtxDevType_e vtxRTC6705GetDeviceType(const vtxDevice_t *vtxDevice)
{ {
UNUSED(vtxDevice);
return VTXDEV_RTC6705; return VTXDEV_RTC6705;
} }
bool vtxRTC6705IsReady(void) static bool vtxRTC6705IsReady(const vtxDevice_t *vtxDevice)
{ {
return true; return vtxDevice != NULL;
} }
static void vtxRTC6705SetBandAndChannel(uint8_t band, uint8_t channel) static void vtxRTC6705SetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel)
{ {
while (!vtxRTC6705CanUpdate()); while (!vtxRTC6705CanUpdate());
if (band >= 1 && band <= VTX_SETTINGS_BAND_COUNT && channel >= 1 && channel <= VTX_SETTINGS_CHANNEL_COUNT) { if (band >= 1 && band <= VTX_SETTINGS_BAND_COUNT && channel >= 1 && channel <= VTX_SETTINGS_CHANNEL_COUNT) {
if (vtxRTC6705.powerIndex > 0) { if (vtxDevice->powerIndex > 0) {
vtxRTC6705.band = band; vtxDevice->band = band;
vtxRTC6705.channel = channel; vtxDevice->channel = channel;
rtc6705SetFrequency(vtx58frequencyTable[vtxRTC6705.band-1][ vtxRTC6705.channel-1]); vtxRTC6705SetFrequency(vtxDevice, vtx58frequencyTable[band-1][channel-1]);
} }
} }
} }
void vtxRTC6705SetPowerByIndex(uint8_t index) static void vtxRTC6705SetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index)
{ {
while (!vtxRTC6705CanUpdate()); while (!vtxRTC6705CanUpdate());
#ifdef RTC6705_POWER_PIN #ifdef RTC6705_POWER_PIN
if (index == 0) { if (index == 0) {
// power device off // power device off
if (vtxRTC6705.powerIndex > 0) { if (vtxDevice->powerIndex > 0) {
// on, power it off // on, power it off
vtxRTC6705.powerIndex = index; vtxDevice->powerIndex = index;
rtc6705Disable(); rtc6705Disable();
return; return;
} else { } else {
@ -156,58 +145,62 @@ void vtxRTC6705SetPowerByIndex(uint8_t index)
} }
} else { } else {
// change rf power and maybe turn the device on first // change rf power and maybe turn the device on first
if (vtxRTC6705.powerIndex == 0) { if (vtxDevice->powerIndex == 0) {
// if it's powered down, power it up, wait and configure channel, band and power. // if it's powered down, power it up, wait and configure channel, band and power.
vtxRTC6705.powerIndex = index; vtxDevice->powerIndex = index;
vtxRTC6705EnableAndConfigure(); vtxRTC6705EnableAndConfigure(vtxDevice);
return; return;
} else { } else {
// if it's powered up, just set the rf power // if it's powered up, just set the rf power
vtxRTC6705.powerIndex = index; vtxDevice->powerIndex = index;
rtc6705SetRFPower(index); rtc6705SetRFPower(index);
} }
} }
#else #else
vtxRTC6705.powerIndex = MAX(index, VTX_RTC6705_MIN_POWER); vtxDevice->powerIndex = MAX(index, VTX_RTC6705_MIN_POWER);
rtc6705SetRFPower(index); rtc6705SetRFPower(index);
#endif #endif
} }
void vtxRTC6705SetPitMode(uint8_t onoff) static void vtxRTC6705SetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
{ {
UNUSED(vtxDevice);
UNUSED(onoff); UNUSED(onoff);
return;
} }
void vtxRTC6705SetFreq(uint16_t freq) static void vtxRTC6705SetFrequency(vtxDevice_t *vtxDevice, uint16_t frequency)
{ {
UNUSED(freq); if (frequency >= VTX_RTC6705_FREQ_MIN && frequency <= VTX_RTC6705_FREQ_MAX) {
return; frequency = constrain(frequency, VTX_RTC6705_FREQ_MIN, VTX_RTC6705_FREQ_MAX);
vtxDevice->frequency = frequency;
rtc6705SetFrequency(frequency);
}
} }
bool vtxRTC6705GetBandAndChannel(uint8_t *pBand, uint8_t *pChannel) static bool vtxRTC6705GetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel)
{ {
*pBand = vtxRTC6705.band; *pBand = vtxDevice->band;
*pChannel = vtxRTC6705.channel; *pChannel = vtxDevice->channel;
return true; return true;
} }
bool vtxRTC6705GetPowerIndex(uint8_t *pIndex) static bool vtxRTC6705GetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
{ {
*pIndex = vtxRTC6705.powerIndex; *pIndex = vtxDevice->powerIndex;
return true; return true;
} }
bool vtxRTC6705GetPitMode(uint8_t *pOnOff) static bool vtxRTC6705GetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff)
{ {
UNUSED(vtxDevice);
UNUSED(pOnOff); UNUSED(pOnOff);
return false; return false;
} }
bool vtxRTC6705GetFreq(uint16_t *pFreq) static bool vtxRTC6705GetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFrequency)
{ {
UNUSED(pFreq); *pFrequency = vtxDevice->frequency;
return false; return true;
} }
static vtxVTable_t rtc6705VTable = { static vtxVTable_t rtc6705VTable = {
@ -217,7 +210,7 @@ static vtxVTable_t rtc6705VTable = {
.setBandAndChannel = vtxRTC6705SetBandAndChannel, .setBandAndChannel = vtxRTC6705SetBandAndChannel,
.setPowerByIndex = vtxRTC6705SetPowerByIndex, .setPowerByIndex = vtxRTC6705SetPowerByIndex,
.setPitMode = vtxRTC6705SetPitMode, .setPitMode = vtxRTC6705SetPitMode,
.setFrequency = vtxRTC6705SetFreq, .setFrequency = vtxRTC6705SetFrequency,
.getBandAndChannel = vtxRTC6705GetBandAndChannel, .getBandAndChannel = vtxRTC6705GetBandAndChannel,
.getPowerIndex = vtxRTC6705GetPowerIndex, .getPowerIndex = vtxRTC6705GetPowerIndex,
.getPitMode = vtxRTC6705GetPitMode, .getPitMode = vtxRTC6705GetPitMode,

1
src/main/io/vtx_rtc6705.h
View file

@ -24,6 +24,5 @@
extern const char * const rtc6705PowerNames[]; extern const char * const rtc6705PowerNames[];
void vtxRTC6705Configure(void);
bool vtxRTC6705CanUpdate(void); bool vtxRTC6705CanUpdate(void);
bool vtxRTC6705Init(void); bool vtxRTC6705Init(void);

55
src/main/io/vtx_smartaudio.c
View file

@ -26,35 +26,25 @@
#if defined(USE_VTX_SMARTAUDIO) && defined(USE_VTX_CONTROL) #if defined(USE_VTX_SMARTAUDIO) && defined(USE_VTX_CONTROL)
#include "build/build_config.h"
#include "build/debug.h" #include "build/debug.h"
#include "cms/cms.h" #include "cms/cms.h"
#include "cms/cms_types.h"
#include "cms/cms_menu_vtx_smartaudio.h" #include "cms/cms_menu_vtx_smartaudio.h"
#include "common/maths.h" #include "common/maths.h"
#include "common/printf.h" #include "common/printf.h"
#include "common/utils.h" #include "common/utils.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "drivers/serial.h"
#include "drivers/time.h" #include "drivers/time.h"
#include "drivers/vtx_common.h" #include "drivers/vtx_common.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "flight/pid.h"
#include "io/serial.h" #include "io/serial.h"
#include "io/vtx_control.h"
#include "io/vtx.h" #include "io/vtx.h"
#include "io/vtx_control.h"
#include "io/vtx_smartaudio.h" #include "io/vtx_smartaudio.h"
#include "io/vtx_string.h" #include "io/vtx_string.h"
// Timing parameters // Timing parameters
// Note that vtxSAProcess() is normally called at 200ms interval // Note that vtxSAProcess() is normally called at 200ms interval
#define SMARTAUDIO_CMD_TIMEOUT 120 // Time until the command is considered lost #define SMARTAUDIO_CMD_TIMEOUT 120 // Time until the command is considered lost
@ -693,7 +683,7 @@ bool vtxSmartAudioInit(void)
return false; return false;
} }
vtxCommonRegisterDevice(&vtxSmartAudio); vtxCommonSetDevice(&vtxSmartAudio);
return true; return true;
} }
@ -703,8 +693,9 @@ bool vtxSmartAudioInit(void)
#define SA_INITPHASE_WAIT_PITFREQ 2 // SA_FREQ_GETPIT sent and waiting for reply. #define SA_INITPHASE_WAIT_PITFREQ 2 // SA_FREQ_GETPIT sent and waiting for reply.
#define SA_INITPHASE_DONE 3 #define SA_INITPHASE_DONE 3
void vtxSAProcess(timeUs_t currentTimeUs) static void vtxSAProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
{ {
UNUSED(vtxDevice);
UNUSED(currentTimeUs); UNUSED(currentTimeUs);
static char initPhase = SA_INITPHASE_START; static char initPhase = SA_INITPHASE_START;
@ -777,25 +768,28 @@ void vtxSAProcess(timeUs_t currentTimeUs)
#ifdef USE_VTX_COMMON #ifdef USE_VTX_COMMON
// Interface to common VTX API // Interface to common VTX API
vtxDevType_e vtxSAGetDeviceType(void) vtxDevType_e vtxSAGetDeviceType(const vtxDevice_t *vtxDevice)
{ {
UNUSED(vtxDevice);
return VTXDEV_SMARTAUDIO; return VTXDEV_SMARTAUDIO;
} }
bool vtxSAIsReady(void) static bool vtxSAIsReady(const vtxDevice_t *vtxDevice)
{ {
return !(saDevice.version == 0); return vtxDevice!=NULL && !(saDevice.version == 0);
} }
void vtxSASetBandAndChannel(uint8_t band, uint8_t channel) static void vtxSASetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel)
{ {
UNUSED(vtxDevice);
if (saValidateBandAndChannel(band, channel)) { if (saValidateBandAndChannel(band, channel)) {
saSetBandAndChannel(band - 1, channel - 1); saSetBandAndChannel(band - 1, channel - 1);
} }
} }
void vtxSASetPowerByIndex(uint8_t index) static void vtxSASetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index)
{ {
UNUSED(vtxDevice);
if (index == 0) { if (index == 0) {
// SmartAudio doesn't support power off. // SmartAudio doesn't support power off.
return; return;
@ -804,9 +798,9 @@ void vtxSASetPowerByIndex(uint8_t index)
saSetPowerByIndex(index - 1); saSetPowerByIndex(index - 1);
} }
void vtxSASetPitMode(uint8_t onoff) static void vtxSASetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
{ {
if (!(vtxSAIsReady() && (saDevice.version == 2))) { if (!(vtxSAIsReady(vtxDevice) && (saDevice.version == 2))) {
return; return;
} }
@ -830,17 +824,18 @@ void vtxSASetPitMode(uint8_t onoff)
return; return;
} }
void vtxSASetFreq(uint16_t freq) static void vtxSASetFreq(vtxDevice_t *vtxDevice, uint16_t freq)
{ {
UNUSED(vtxDevice);
if (saValidateFreq(freq)) { if (saValidateFreq(freq)) {
saSetMode(0); //need to be in FREE mode to set freq saSetMode(0); //need to be in FREE mode to set freq
saSetFreq(freq); saSetFreq(freq);
} }
} }
bool vtxSAGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel) static bool vtxSAGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel)
{ {
if (!vtxSAIsReady()) { if (!vtxSAIsReady(vtxDevice)) {
return false; return false;
} }
@ -851,9 +846,9 @@ bool vtxSAGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel)
return true; return true;
} }
bool vtxSAGetPowerIndex(uint8_t *pIndex) static bool vtxSAGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
{ {
if (!vtxSAIsReady()) { if (!vtxSAIsReady(vtxDevice)) {
return false; return false;
} }
@ -861,9 +856,9 @@ bool vtxSAGetPowerIndex(uint8_t *pIndex)
return true; return true;
} }
bool vtxSAGetPitMode(uint8_t *pOnOff) static bool vtxSAGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff)
{ {
if (!(vtxSAIsReady() && (saDevice.version == 2))) { if (!(vtxSAIsReady(vtxDevice) && (saDevice.version == 2))) {
return false; return false;
} }
@ -871,9 +866,9 @@ bool vtxSAGetPitMode(uint8_t *pOnOff)
return true; return true;
} }
bool vtxSAGetFreq(uint16_t *pFreq) static bool vtxSAGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
{ {
if (!vtxSAIsReady()) { if (!vtxSAIsReady(vtxDevice)) {
return false; return false;
} }

41
src/main/io/vtx_tramp.c
View file

@ -366,8 +366,10 @@ void trampQueryS(void)
trampQuery('s'); trampQuery('s');
} }
void vtxTrampProcess(timeUs_t currentTimeUs) static void vtxTrampProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
{ {
UNUSED(vtxDevice);
static timeUs_t lastQueryTimeUs = 0; static timeUs_t lastQueryTimeUs = 0;
static bool initSettingsDoneFlag = false; static bool initSettingsDoneFlag = false;
@ -489,45 +491,50 @@ void vtxTrampProcess(timeUs_t currentTimeUs)
// Interface to common VTX API // Interface to common VTX API
vtxDevType_e vtxTrampGetDeviceType(void) static vtxDevType_e vtxTrampGetDeviceType(const vtxDevice_t *vtxDevice)
{ {
UNUSED(vtxDevice);
return VTXDEV_TRAMP; return VTXDEV_TRAMP;
} }
bool vtxTrampIsReady(void) static bool vtxTrampIsReady(const vtxDevice_t *vtxDevice)
{ {
return trampStatus > TRAMP_STATUS_OFFLINE; return vtxDevice!=NULL && trampStatus > TRAMP_STATUS_OFFLINE;
} }
void vtxTrampSetBandAndChannel(uint8_t band, uint8_t channel) static void vtxTrampSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel)
{ {
UNUSED(vtxDevice);
if (trampValidateBandAndChannel(band, channel)) { if (trampValidateBandAndChannel(band, channel)) {
trampSetBandAndChannel(band, channel); trampSetBandAndChannel(band, channel);
trampCommitChanges(); trampCommitChanges();
} }
} }
void vtxTrampSetPowerByIndex(uint8_t index) static void vtxTrampSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index)
{ {
UNUSED(vtxDevice);
trampDevSetPowerByIndex(index); trampDevSetPowerByIndex(index);
} }
void vtxTrampSetPitMode(uint8_t onoff) static void vtxTrampSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
{ {
UNUSED(vtxDevice);
trampSetPitMode(onoff); trampSetPitMode(onoff);
} }
void vtxTrampSetFreq(uint16_t freq) static void vtxTrampSetFreq(vtxDevice_t *vtxDevice, uint16_t freq)
{ {
UNUSED(vtxDevice);
if (trampValidateFreq(freq)) { if (trampValidateFreq(freq)) {
trampSetFreq(freq); trampSetFreq(freq);
trampCommitChanges(); trampCommitChanges();
} }
} }
bool vtxTrampGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel) static bool vtxTrampGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel)
{ {
if (!vtxTrampIsReady()) { if (!vtxTrampIsReady(vtxDevice)) {
return false; return false;
} }
@ -537,9 +544,9 @@ bool vtxTrampGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel)
return true; return true;
} }
bool vtxTrampGetPowerIndex(uint8_t *pIndex) static bool vtxTrampGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
{ {
if (!vtxTrampIsReady()) { if (!vtxTrampIsReady(vtxDevice)) {
return false; return false;
} }
@ -555,9 +562,9 @@ bool vtxTrampGetPowerIndex(uint8_t *pIndex)
return true; return true;
} }
bool vtxTrampGetPitMode(uint8_t *pOnOff) static bool vtxTrampGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff)
{ {
if (!vtxTrampIsReady()) { if (!vtxTrampIsReady(vtxDevice)) {
return false; return false;
} }
@ -565,9 +572,9 @@ bool vtxTrampGetPitMode(uint8_t *pOnOff)
return true; return true;
} }
bool vtxTrampGetFreq(uint16_t *pFreq) static bool vtxTrampGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
{ {
if (!vtxTrampIsReady()) { if (!vtxTrampIsReady(vtxDevice)) {
return false; return false;
} }
@ -611,7 +618,7 @@ bool vtxTrampInit(void)
} }
#if defined(USE_VTX_COMMON) #if defined(USE_VTX_COMMON)
vtxCommonRegisterDevice(&vtxTramp); vtxCommonSetDevice(&vtxTramp);
#endif #endif
return true; return true;

17
src/main/telemetry/srxl.c
View file

@ -267,14 +267,15 @@ bool srxlFrameText(sbuf_t *dst, timeUs_t currentTimeUs)
static uint8_t vtxDeviceType; static uint8_t vtxDeviceType;
void collectVtxTmData(spektrumVtx_t * vtx) static void collectVtxTmData(spektrumVtx_t * vtx)
{ {
vtxDeviceType = vtxCommonGetDeviceType(); const vtxDevice_t *vtxDevice = vtxCommonDevice();
vtxDeviceType = vtxCommonGetDeviceType(vtxDevice);
// Collect all data from VTX, if VTX is ready // Collect all data from VTX, if VTX is ready
if ( !(vtxCommonGetBandAndChannel(&(vtx->band), &(vtx->channel)) && if (vtxDevice == NULL || !(vtxCommonGetBandAndChannel(vtxDevice, &vtx->band, &vtx->channel) &&
vtxCommonGetPitMode(&(vtx->pitMode)) && vtxCommonGetPitMode(vtxDevice, &vtx->pitMode) &&
vtxCommonGetPowerIndex(&(vtx->power))) ) vtxCommonGetPowerIndex(vtxDevice, &vtx->power)) )
{ {
vtx->band = 0; vtx->band = 0;
vtx->channel = 0; vtx->channel = 0;
@ -291,7 +292,7 @@ void collectVtxTmData(spektrumVtx_t * vtx)
} }
// Reverse lookup, device power index to Spektrum power range index. // Reverse lookup, device power index to Spektrum power range index.
void convertVtxPower(spektrumVtx_t * vtx) static void convertVtxPower(spektrumVtx_t * vtx)
{ {
uint8_t const * powerIndexTable = NULL; uint8_t const * powerIndexTable = NULL;
@ -333,7 +334,7 @@ void convertVtxPower(spektrumVtx_t * vtx)
} }
} }
void convertVtxTmData(spektrumVtx_t * vtx) static void convertVtxTmData(spektrumVtx_t * vtx)
{ {
// Convert from internal band indexes to Spektrum indexes // Convert from internal band indexes to Spektrum indexes
for (int i = 0; i < SPEKTRUM_VTX_BAND_COUNT; i++) { for (int i = 0; i < SPEKTRUM_VTX_BAND_COUNT; i++) {
@ -368,7 +369,7 @@ typedef struct
#define STRU_TELE_VTX_RESERVE_COUNT 7 #define STRU_TELE_VTX_RESERVE_COUNT 7
#define VTX_KEEPALIVE_TIME_OUT 2000000 // uS #define VTX_KEEPALIVE_TIME_OUT 2000000 // uS
bool srxlFrameVTX(sbuf_t *dst, timeUs_t currentTimeUs) static bool srxlFrameVTX(sbuf_t *dst, timeUs_t currentTimeUs)
{ {
static timeUs_t lastTimeSentVtx = 0; static timeUs_t lastTimeSentVtx = 0;
static spektrumVtx_t vtxSent; static spektrumVtx_t vtxSent;