mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-23 16:25:31 +03:00
234 lines
8.5 KiB
C
234 lines
8.5 KiB
C
/*
|
|
* This file is part of Cleanflight and Betaflight.
|
|
*
|
|
* Cleanflight and Betaflight are free software. You can redistribute
|
|
* this software and/or modify this software under the terms of the
|
|
* GNU General Public License as published by the Free Software
|
|
* Foundation, either version 3 of the License, or (at your option)
|
|
* any later version.
|
|
*
|
|
* Cleanflight and Betaflight are distributed in the hope that they
|
|
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
|
|
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
|
* See the GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with this software.
|
|
*
|
|
* If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include "platform.h"
|
|
#if defined(USE_SPEKTRUM_VTX_CONTROL) && defined(USE_VTX_COMMON)
|
|
|
|
#include <string.h>
|
|
|
|
#include "fc/config.h"
|
|
#include "drivers/vtx_common.h"
|
|
#include "io/vtx.h"
|
|
#include "io/vtx_string.h"
|
|
|
|
#include "io/spektrum_vtx_control.h"
|
|
|
|
// We can not use the common set/get-frequncy API.
|
|
// Some VTX devices do not support it.
|
|
//#define USE_VTX_COMMON_FREQ_API
|
|
|
|
#ifdef USE_VTX_COMMON_FREQ_API
|
|
const uint16_t SpektrumVtxfrequencyTable[SPEKTRUM_VTX_BAND_COUNT][SPEKTRUM_VTX_CHAN_COUNT] =
|
|
{
|
|
{ 5740, 5760, 5780, 5800, 5820, 5840, 5860, 5880 }, // FatShark
|
|
{ 5658, 5695, 5732, 5769, 5806, 5843, 5880, 5917 }, // RaceBand
|
|
{ 5705, 5685, 5665, 5645, 5885, 5905, 5925, 5945 }, // Boscam E
|
|
{ 5733, 5752, 5771, 5790, 5809, 5828, 5847, 5866 }, // Boscam B
|
|
{ 5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725 }, // Boscam A
|
|
};
|
|
#else
|
|
// Translation table, Spektrum bands to BF internal vtx_common bands
|
|
const uint8_t spek2commonBand[SPEKTRUM_VTX_BAND_COUNT]= {
|
|
VTX_COMMON_BAND_FS,
|
|
VTX_COMMON_BAND_RACE,
|
|
VTX_COMMON_BAND_E,
|
|
VTX_COMMON_BAND_B,
|
|
VTX_COMMON_BAND_A,
|
|
};
|
|
#endif
|
|
|
|
// RF Power Index translation tables. No generic power API available.....
|
|
|
|
#ifdef USE_VTX_TRAMP
|
|
// Tramp "---", 25, 200, 400. 600 mW
|
|
const uint8_t vtxTrampPi[SPEKTRUM_VTX_POWER_COUNT] = {
|
|
// Spektrum Spec Tx menu Tx sends To VTX Watt
|
|
VTX_TRAMP_POWER_OFF, // Off INHIBIT 0 0 -
|
|
VTX_TRAMP_POWER_OFF, // 1 - 14mW - - - -
|
|
VTX_TRAMP_POWER_25, // 15 - 25mW 15 - 25mW 2 1 25mW
|
|
VTX_TRAMP_POWER_100, // 26 - 99mW 26 - 99mW 3 2 100mW Slightly outside range
|
|
VTX_TRAMP_POWER_200, // 100 - 299mW 100 - 200mW 4 3 200mW
|
|
VTX_TRAMP_POWER_400, // 300 - 600mW 300 - 600mW 5 4 400mW
|
|
VTX_TRAMP_POWER_600, // 601 - max 601+ mW 6 5 600mW Slightly outside range
|
|
VTX_TRAMP_POWER_200 // Manual - - - -
|
|
};
|
|
#endif // USE_VTX_TRAMP
|
|
|
|
#ifdef USE_VTX_RTC6705
|
|
// RTC6705 "---", 25 or 200 mW
|
|
const uint8_t vtxRTC6705Pi[SPEKTRUM_VTX_POWER_COUNT] = {
|
|
VTX_6705_POWER_OFF, // Off
|
|
VTX_6705_POWER_OFF, // 1 - 14mW
|
|
VTX_6705_POWER_25, // 15 - 25mW
|
|
VTX_6705_POWER_25, // 26 - 99mW
|
|
VTX_6705_POWER_200, // 100 - 299mW
|
|
VTX_6705_POWER_200, // 300 - 600mW
|
|
VTX_6705_POWER_200, // 601 - max
|
|
VTX_6705_POWER_200 // Manual
|
|
};
|
|
#endif //USE_VTX_RTC6705
|
|
|
|
#ifdef USE_VTX_SMARTAUDIO
|
|
// SmartAudio "---", 25, 200, 500. 800 mW
|
|
const uint8_t vtxSaPi[SPEKTRUM_VTX_POWER_COUNT] = {
|
|
VTX_SA_POWER_OFF, // Off
|
|
VTX_SA_POWER_OFF, // 1 - 14mW
|
|
VTX_SA_POWER_25, // 15 - 25mW
|
|
VTX_SA_POWER_25, // 26 - 99mW
|
|
VTX_SA_POWER_200, // 100 - 299mW
|
|
VTX_SA_POWER_500, // 300 - 600mW
|
|
VTX_SA_POWER_800, // 601 - max
|
|
VTX_SA_POWER_200 // Manual
|
|
};
|
|
#endif // USE_VTX_SMARTAUDIO
|
|
|
|
uint8_t convertSpektrumVtxPowerIndex(uint8_t sPower)
|
|
{
|
|
uint8_t devicePower = 0;
|
|
|
|
const vtxDevice_t *vtxDevice = vtxCommonDevice();
|
|
switch (vtxCommonGetDeviceType(vtxDevice)) {
|
|
#ifdef USE_VTX_RTC6705
|
|
case VTXDEV_RTC6705:
|
|
devicePower = vtxRTC6705Pi[sPower];
|
|
break;
|
|
#endif // USE_VTX_RTC6705
|
|
|
|
#ifdef USE_VTX_SMARTAUDIO
|
|
case VTXDEV_SMARTAUDIO:
|
|
devicePower = vtxSaPi[sPower];
|
|
break;
|
|
#endif // USE_VTX_SMARTAUDIO
|
|
|
|
#ifdef USE_VTX_TRAMP
|
|
case VTXDEV_TRAMP:
|
|
devicePower = vtxTrampPi[sPower];
|
|
break;
|
|
#endif // USE_VTX_TRAMP
|
|
|
|
case VTXDEV_UNKNOWN:
|
|
case VTXDEV_UNSUPPORTED:
|
|
default:
|
|
break;
|
|
|
|
}
|
|
return devicePower;
|
|
}
|
|
|
|
#ifdef USE_SPEKTRUM_REGION_CODES
|
|
// Just a global SpektrumRegion for now, To save VTX ctrl input to VTX tm output.
|
|
// Would need a PG item to survive power cycle. Not really used so let it be as is.
|
|
uint8_t SpektrumRegion = SPEKTRUM_VTX_REGION_NONE;
|
|
#endif
|
|
|
|
// Mark an inital invalid VTX ctrl frame to force first VTX settings cheange to actually come from Tx/Rx.
|
|
static uint32_t vtxControl_ipc = ~(SPEKTRUM_VTX_CONTROL_FRAME);
|
|
|
|
// ############ RX task ######################
|
|
void spektrumHandleVtxControl(uint32_t vtxCntrl)
|
|
{
|
|
vtxControl_ipc = vtxCntrl;
|
|
}
|
|
// ###########################################
|
|
|
|
|
|
// ############ VTX_CONTROL task #############
|
|
void spektrumVtxControl(void)
|
|
{
|
|
static uint32_t prevVtxControl =0;
|
|
uint32_t vtxControl;
|
|
|
|
// Check for invalid VTX ctrl frames
|
|
if ((vtxControl_ipc & SPEKTRUM_VTX_CONTROL_FRAME_MASK) != SPEKTRUM_VTX_CONTROL_FRAME) return;
|
|
|
|
vtxControl = vtxControl_ipc;
|
|
vtxControl_ipc = 0;
|
|
|
|
if (prevVtxControl == vtxControl) return;
|
|
prevVtxControl = vtxControl;
|
|
|
|
spektrumVtx_t vtx = {
|
|
.pitMode = (vtxControl & SPEKTRUM_VTX_PIT_MODE_MASK) >> SPEKTRUM_VTX_PIT_MODE_SHIFT,
|
|
.region = (vtxControl & SPEKTRUM_VTX_REGION_MASK) >> SPEKTRUM_VTX_REGION_SHIFT,
|
|
.power = (vtxControl & SPEKTRUM_VTX_POWER_MASK) >> SPEKTRUM_VTX_POWER_SHIFT,
|
|
.band = (vtxControl & SPEKTRUM_VTX_BAND_MASK) >> SPEKTRUM_VTX_BAND_SHIFT,
|
|
.channel = (vtxControl & SPEKTRUM_VTX_CHANNEL_MASK) >> SPEKTRUM_VTX_CHANNEL_SHIFT,
|
|
#ifdef USE_SPEKTRUM_REGION_CODES
|
|
.region = (vtxControl & SPEKTRUM_VTX_REGION_MASK) >> SPEKTRUM_VTX_REGION_SHIFT;
|
|
#endif
|
|
};
|
|
|
|
const vtxSettingsConfig_t prevSettings = {
|
|
.band = vtxSettingsConfig()->band,
|
|
.channel = vtxSettingsConfig()->channel,
|
|
.freq = vtxSettingsConfig()->freq,
|
|
.power = vtxSettingsConfig()->power,
|
|
.lowPowerDisarm = vtxSettingsConfig()->lowPowerDisarm,
|
|
};
|
|
vtxSettingsConfig_t newSettings = prevSettings;
|
|
|
|
vtxDevice_t *vtxDevice = vtxCommonDevice();
|
|
if (vtxDevice) {
|
|
#ifdef USE_VTX_COMMON_FREQ_API
|
|
uint16_t freq = SpektrumVtxfrequencyTable[vtx.band][vtx.channel];
|
|
if (prevSettings.freq != freq) {
|
|
newSettings.band = VTX_COMMON_BAND_USER;
|
|
newSettings.channel = vtx.channel;
|
|
newSettings.freq = freq;
|
|
}
|
|
#else
|
|
// Convert to the internal Common Band index
|
|
const uint8_t band = spek2commonBand[vtx.band];
|
|
const uint8_t channel = vtx.channel +1; // 0 based to 1 based
|
|
if ((prevSettings.band != band) || (prevSettings.channel != channel)) {
|
|
newSettings.band = band;
|
|
newSettings.channel = channel;
|
|
newSettings.freq = vtx58_Bandchan2Freq(band, channel);
|
|
}
|
|
#endif
|
|
// Seems to be no unified internal VTX API standard for power levels/indexes, VTX device brand specific.
|
|
const uint8_t power = convertSpektrumVtxPowerIndex(vtx.power);
|
|
if (prevSettings.power != power) {
|
|
newSettings.power = power;
|
|
}
|
|
// Everyone seems to agree on what PIT ON/OFF means
|
|
uint8_t currentPitMode = 0;
|
|
if (vtxCommonGetPitMode(vtxDevice, ¤tPitMode)) {
|
|
if (currentPitMode != vtx.pitMode) {
|
|
vtxCommonSetPitMode(vtxDevice, vtx.pitMode);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (memcmp(&prevSettings,&newSettings,sizeof(vtxSettingsConfig_t))) {
|
|
vtxSettingsConfigMutable()->band = newSettings.band;
|
|
vtxSettingsConfigMutable()->channel = newSettings.channel;
|
|
vtxSettingsConfigMutable()->power = newSettings.power;
|
|
vtxSettingsConfigMutable()->freq = newSettings.freq;
|
|
saveConfigAndNotify();
|
|
}
|
|
#ifdef USE_SPEKTRUM_REGION_CODES
|
|
// Save region code
|
|
SpektrumRegion = vtx.region;
|
|
#endif
|
|
|
|
}
|
|
|
|
#endif // USE_SPEKTRUM_VTX_CONTROL && USE_VTX_COMMON
|