/*
* 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 .
*/
/* Created by jflyper */
#include
#include
#include
#include
#include "platform.h"
#if defined(USE_VTX_COMMON)
#include "common/time.h"
#include "drivers/vtx_common.h"
#include "drivers/vtx_table.h"
static vtxDevice_t *vtxDevice = NULL;
static uint8_t selectedBand = 0;
static uint8_t selectedChannel = 0;
void vtxCommonInit(void)
{
}
void vtxCommonSetDevice(vtxDevice_t *pDevice)
{
vtxDevice = pDevice;
}
vtxDevice_t *vtxCommonDevice(void)
{
return vtxDevice;
}
vtxDevType_e vtxCommonGetDeviceType(const vtxDevice_t *vtxDevice)
{
if (!vtxDevice) {
return VTXDEV_UNKNOWN;
}
return vtxDevice->vTable->getDeviceType(vtxDevice);
}
bool vtxCommonDeviceIsReady(const vtxDevice_t *vtxDevice)
{
if (vtxDevice && vtxDevice->vTable->isReady) {
return vtxDevice->vTable->isReady(vtxDevice);
}
return false;
}
void vtxCommonProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
{
if (vtxDevice) {
vtxDevice->vTable->process(vtxDevice, currentTimeUs);
}
}
// band and channel are 1 origin
void vtxCommonSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel)
{
uint16_t freq = vtxCommonLookupFrequency(vtxDevice, band, channel);
if (freq != 0) {
selectedChannel = channel;
selectedBand = band;
if (vtxTableIsFactoryBand[band - 1]) {
vtxDevice->vTable->setBandAndChannel(vtxDevice, band, channel);
} else {
vtxDevice->vTable->setFrequency(vtxDevice, freq);
}
}
}
// index is one origin, zero = unknown power level
void vtxCommonSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index)
{
if (index <= vtxTablePowerLevels) {
vtxDevice->vTable->setPowerByIndex(vtxDevice, index);
}
}
// on = 1, off = 0
void vtxCommonSetPitMode(vtxDevice_t *vtxDevice, uint8_t onOff)
{
vtxDevice->vTable->setPitMode(vtxDevice, onOff);
}
void vtxCommonSetFrequency(vtxDevice_t *vtxDevice, uint16_t frequency)
{
selectedBand = 0;
selectedChannel = 0;
vtxDevice->vTable->setFrequency(vtxDevice, frequency);
}
bool vtxCommonGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel)
{
bool result = vtxDevice->vTable->getBandAndChannel(vtxDevice, pBand, pChannel);
if ((!result || (*pBand == 0 && *pChannel == 0)) && selectedBand != 0 && selectedChannel != 0
&& !vtxTableIsFactoryBand[selectedBand - 1]) {
uint16_t freq;
result = vtxCommonGetFrequency(vtxDevice, &freq);
if (result) {
vtxCommonLookupBandChan(vtxDevice, freq, pBand, pChannel);
}
}
return result;
}
bool vtxCommonGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
{
return vtxDevice->vTable->getPowerIndex(vtxDevice, pIndex);
}
bool vtxCommonGetFrequency(const vtxDevice_t *vtxDevice, uint16_t *pFrequency)
{
return vtxDevice->vTable->getFrequency(vtxDevice, pFrequency);
}
bool vtxCommonGetStatus(const vtxDevice_t *vtxDevice, unsigned *status)
{
return vtxDevice->vTable->getStatus(vtxDevice, status);
}
uint8_t vtxCommonGetVTXPowerLevels(const vtxDevice_t *vtxDevice, uint16_t *levels, uint16_t *powers)
{
return vtxDevice->vTable->getPowerLevels(vtxDevice, levels, powers);
}
const char *vtxCommonLookupBandName(const vtxDevice_t *vtxDevice, int band)
{
if (vtxDevice && band > 0 && band <= vtxTableBandCount) {
return vtxTableBandNames[band];
} else {
return "?";
}
}
char vtxCommonLookupBandLetter(const vtxDevice_t *vtxDevice, int band)
{
if (vtxDevice && band > 0 && band <= vtxTableBandCount) {
return vtxTableBandLetters[band];
} else {
return '?';
}
}
const char *vtxCommonLookupChannelName(const vtxDevice_t *vtxDevice, int channel)
{
if (vtxDevice && channel > 0 && channel <= vtxTableChannelCount) {
return vtxTableChannelNames[channel];
} else {
return "?";
}
}
//Converts frequency (in MHz) to band and channel values.
//If frequency not found in the vtxtable then band and channel will return 0
void vtxCommonLookupBandChan(const vtxDevice_t *vtxDevice, uint16_t freq, uint8_t *pBand, uint8_t *pChannel)
{
*pBand = 0;
*pChannel = 0;
if (vtxDevice) {
// Use reverse lookup order so that 5880Mhz
// get Raceband 7 instead of Fatshark 8.
for (int band = vtxTableBandCount - 1 ; band >= 0 ; band--) {
for (int channel = 0 ; channel < vtxTableChannelCount ; channel++) {
if (vtxTableFrequency[band][channel] == freq) {
*pBand = band + 1;
*pChannel = channel + 1;
return;
}
}
}
}
}
//Converts band and channel values to a frequency (in MHz) value.
// band: Band value (1 to 5).
// channel: Channel value (1 to 8).
// Returns frequency value (in MHz), or 0 if band/channel out of range.
uint16_t vtxCommonLookupFrequency(const vtxDevice_t *vtxDevice, int band, int channel)
{
if (vtxDevice) {
if (band > 0 && band <= vtxTableBandCount &&
channel > 0 && channel <= vtxTableChannelCount) {
return vtxTableFrequency[band - 1][channel - 1];
}
}
return 0;
}
const char *vtxCommonLookupPowerName(const vtxDevice_t *vtxDevice, int index)
{
if (vtxDevice && index > 0 && index <= vtxTablePowerLevels) {
return vtxTablePowerLabels[index];
} else {
return "?";
}
}
bool vtxCommonLookupPowerValue(const vtxDevice_t *vtxDevice, int index, uint16_t *pPowerValue)
{
if (vtxDevice && index > 0 && index <= vtxTablePowerLevels) {
*pPowerValue = vtxTablePowerValues[index - 1];
return true;
} else {
return false;
}
}
static void vtxCommonSerializeCustomDeviceStatus(const vtxDevice_t *vtxDevice, sbuf_t *dst)
{
const bool customDeviceStatusAvailable = vtxDevice && vtxDevice->vTable->serializeCustomDeviceStatus;
if (customDeviceStatusAvailable) {
vtxDevice->vTable->serializeCustomDeviceStatus(vtxDevice, dst);
} else {
sbufWriteU8(dst, 0);
}
}
static void vtxCommonSerializePowerLevels(const vtxDevice_t *vtxDevice, sbuf_t *dst)
{
uint16_t levels[VTX_TABLE_MAX_POWER_LEVELS];
uint16_t powers[VTX_TABLE_MAX_POWER_LEVELS];
const uint8_t powerLevelCount = vtxCommonGetVTXPowerLevels(vtxDevice, levels, powers);
sbufWriteU8(dst, powerLevelCount);
for (int i = 0; i < powerLevelCount; i++) {
sbufWriteU16(dst, levels[i]);
sbufWriteU16(dst, powers[i]);
}
}
void vtxCommonSerializeDeviceStatus(const vtxDevice_t *vtxDevice, sbuf_t *dst)
{
if (vtxDevice) {
const vtxDevType_e vtxType = vtxCommonGetDeviceType(vtxDevice);
const bool deviceReady = vtxCommonDeviceIsReady(vtxDevice);
uint8_t band = 0;
uint8_t channel = 0;
const bool bandAndChannelAvailable = vtxCommonGetBandAndChannel(vtxDevice, &band, &channel);
uint8_t powerIndex = 0;
const bool powerIndexAvailable = vtxCommonGetPowerIndex(vtxDevice, &powerIndex);
uint16_t frequency = 0;
const bool frequencyAvailable = vtxCommonGetFrequency(vtxDevice, &frequency);
unsigned vtxStatus = 0; // pitmode and/or locked
const bool vtxStatusAvailable = vtxCommonGetStatus(vtxDevice, &vtxStatus);
sbufWriteU8(dst, vtxType);
sbufWriteU8(dst, deviceReady);
sbufWriteU8(dst, bandAndChannelAvailable);
sbufWriteU8(dst, band);
sbufWriteU8(dst, channel);
sbufWriteU8(dst, powerIndexAvailable);
sbufWriteU8(dst, powerIndex);
sbufWriteU8(dst, frequencyAvailable);
sbufWriteU16(dst, frequency);
sbufWriteU8(dst, vtxStatusAvailable);
sbufWriteU32(dst, vtxStatus);
vtxCommonSerializePowerLevels(vtxDevice, dst);
vtxCommonSerializeCustomDeviceStatus(vtxDevice, dst);
}
}
#endif