src/main/drivers/vtx_common.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 /* Created by jflyper */
  22
  23 #include <stdbool.h>
  24 #include <stdint.h>
  25 #include <ctype.h>
  26 #include <string.h>
  27
  28 #include "platform.h"
  29
  30 #if defined(USE_VTX_COMMON)
  31
  32 #include "common/time.h"
  33 #include "drivers/vtx_common.h"
  34 #include "drivers/vtx_table.h"
  35
  36 static vtxDevice_t *vtxDevice = NULL;
  37 static uint8_t selectedBand = 0;
  38 static uint8_t selectedChannel = 0;
  39
  40 void vtxCommonInit(void)
  41 {
  42 }
  43
  44 void vtxCommonSetDevice(vtxDevice_t *pDevice)
  45 {
  46     vtxDevice = pDevice;
  47 }
  48
  49 vtxDevice_t *vtxCommonDevice(void)
  50 {
  51     return vtxDevice;
  52 }
  53
  54 vtxDevType_e vtxCommonGetDeviceType(const vtxDevice_t *vtxDevice)
  55 {
  56     if (!vtxDevice) {
  57         return VTXDEV_UNKNOWN;
  58     }
  59     return vtxDevice->vTable->getDeviceType(vtxDevice);
  60 }
  61
  62 bool vtxCommonDeviceIsReady(const vtxDevice_t *vtxDevice)
  63 {
  64     if (vtxDevice && vtxDevice->vTable->isReady) {
  65         return vtxDevice->vTable->isReady(vtxDevice);
  66     }
  67
  68     return false;
  69 }
  70
  71 void vtxCommonProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
  72 {
  73     if (vtxDevice) {
  74         vtxDevice->vTable->process(vtxDevice, currentTimeUs);
  75     }
  76 }
  77
  78 // band and channel are 1 origin
  79 void vtxCommonSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel)
  80 {
  81     uint16_t freq = vtxCommonLookupFrequency(vtxDevice, band, channel);
  82     if (freq != 0) {
  83         selectedChannel = channel;
  84         selectedBand = band;
  85         if (vtxTableIsFactoryBand[band - 1]) {
  86             vtxDevice->vTable->setBandAndChannel(vtxDevice, band, channel);
  87         } else {
  88             vtxDevice->vTable->setFrequency(vtxDevice, freq);
  89         }
  90     }
  91 }
  92
  93 // index is one origin, zero = unknown power level
  94 void vtxCommonSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index)
  95 {
  96     if (index <= vtxTablePowerLevels) {
  97         vtxDevice->vTable->setPowerByIndex(vtxDevice, index);
  98     }
  99 }
 100
 101 // on = 1, off = 0
 102 void vtxCommonSetPitMode(vtxDevice_t *vtxDevice, uint8_t onOff)
 103 {
 104     vtxDevice->vTable->setPitMode(vtxDevice, onOff);
 105 }
 106
 107 void vtxCommonSetFrequency(vtxDevice_t *vtxDevice, uint16_t frequency)
 108 {
 109     selectedBand = 0;
 110     selectedChannel = 0;
 111     vtxDevice->vTable->setFrequency(vtxDevice, frequency);
 112 }
 113
 114 bool vtxCommonGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel)
 115 {
 116     bool result = vtxDevice->vTable->getBandAndChannel(vtxDevice, pBand, pChannel);
 117     if ((!result || (*pBand == 0 && *pChannel == 0)) && selectedBand != 0 && selectedChannel != 0
 118         && !vtxTableIsFactoryBand[selectedBand - 1]) {
 119         uint16_t freq;
 120         result = vtxCommonGetFrequency(vtxDevice, &freq);
 121         if (result) {
 122             vtxCommonLookupBandChan(vtxDevice, freq, pBand, pChannel);
 123         }
 124     }
 125     return result;
 126 }
 127
 128 bool vtxCommonGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
 129 {
 130     return vtxDevice->vTable->getPowerIndex(vtxDevice, pIndex);
 131 }
 132
 133 bool vtxCommonGetFrequency(const vtxDevice_t *vtxDevice, uint16_t *pFrequency)
 134 {
 135     return vtxDevice->vTable->getFrequency(vtxDevice, pFrequency);
 136 }
 137
 138 bool vtxCommonGetStatus(const vtxDevice_t *vtxDevice, unsigned *status)
 139 {
 140     return vtxDevice->vTable->getStatus(vtxDevice, status);
 141 }
 142
 143 uint8_t vtxCommonGetVTXPowerLevels(const vtxDevice_t *vtxDevice, uint16_t *levels, uint16_t *powers)
 144 {
 145     return vtxDevice->vTable->getPowerLevels(vtxDevice, levels, powers);
 146 }
 147
 148 const char *vtxCommonLookupBandName(const vtxDevice_t *vtxDevice, int band)
 149 {
 150     if (vtxDevice && band > 0 && band <= vtxTableBandCount) {
 151         return vtxTableBandNames[band];
 152     } else {
 153         return "?";
 154     }
 155 }
 156
 157 char vtxCommonLookupBandLetter(const vtxDevice_t *vtxDevice, int band)
 158 {
 159     if (vtxDevice && band > 0 && band <= vtxTableBandCount) {
 160         return vtxTableBandLetters[band];
 161     } else {
 162         return '?';
 163     }
 164 }
 165
 166 const char *vtxCommonLookupChannelName(const vtxDevice_t *vtxDevice, int channel)
 167 {
 168     if (vtxDevice && channel > 0 && channel <= vtxTableChannelCount) {
 169         return vtxTableChannelNames[channel];
 170     } else {
 171         return "?";
 172     }
 173 }
 174
 175 //Converts frequency (in MHz) to band and channel values.
 176 //If frequency not found in the vtxtable then band and channel will return 0
 177 void vtxCommonLookupBandChan(const vtxDevice_t *vtxDevice, uint16_t freq, uint8_t *pBand, uint8_t *pChannel)
 178 {
 179     *pBand = 0;
 180     *pChannel = 0;
 181     if (vtxDevice) {
 182         // Use reverse lookup order so that 5880Mhz
 183         // get Raceband 7 instead of Fatshark 8.
 184         for (int band = vtxTableBandCount - 1 ; band >= 0 ; band--) {
 185             for (int channel = 0 ; channel < vtxTableChannelCount ; channel++) {
 186                 if (vtxTableFrequency[band][channel] == freq) {
 187                     *pBand = band + 1;
 188                     *pChannel = channel + 1;
 189                     return;
 190                 }
 191             }
 192         }
 193     }
 194 }
 195
 196 //Converts band and channel values to a frequency (in MHz) value.
 197 // band:  Band value (1 to 5).
 198 // channel:  Channel value (1 to 8).
 199 // Returns frequency value (in MHz), or 0 if band/channel out of range.
 200 uint16_t vtxCommonLookupFrequency(const vtxDevice_t *vtxDevice, int band, int channel)
 201 {
 202     if (vtxDevice) {
 203         if (band > 0 && band <= vtxTableBandCount &&
 204             channel > 0 && channel <= vtxTableChannelCount) {
 205             return vtxTableFrequency[band - 1][channel - 1];
 206         }
 207     }
 208
 209     return 0;
 210 }
 211
 212 const char *vtxCommonLookupPowerName(const vtxDevice_t *vtxDevice, int index)
 213 {
 214     if (vtxDevice && index > 0 && index <= vtxTablePowerLevels) {
 215         return vtxTablePowerLabels[index];
 216     } else {
 217         return "?";
 218     }
 219 }
 220
 221 bool vtxCommonLookupPowerValue(const vtxDevice_t *vtxDevice, int index, uint16_t *pPowerValue)
 222 {
 223     if (vtxDevice && index > 0 && index <= vtxTablePowerLevels) {
 224         *pPowerValue = vtxTablePowerValues[index - 1];
 225         return true;
 226     } else {
 227         return false;
 228     }
 229 }
 230
 231 static void vtxCommonSerializeCustomDeviceStatus(const vtxDevice_t *vtxDevice, sbuf_t *dst)
 232 {
 233     const bool customDeviceStatusAvailable = vtxDevice && vtxDevice->vTable->serializeCustomDeviceStatus;
 234
 235     if (customDeviceStatusAvailable) {
 236         vtxDevice->vTable->serializeCustomDeviceStatus(vtxDevice, dst);
 237     } else {
 238         sbufWriteU8(dst, 0);
 239     }
 240 }
 241
 242 static void vtxCommonSerializePowerLevels(const vtxDevice_t *vtxDevice, sbuf_t *dst)
 243 {
 244     uint16_t levels[VTX_TABLE_MAX_POWER_LEVELS];
 245     uint16_t powers[VTX_TABLE_MAX_POWER_LEVELS];
 246
 247     const uint8_t powerLevelCount = vtxCommonGetVTXPowerLevels(vtxDevice, levels, powers);
 248
 249     sbufWriteU8(dst, powerLevelCount);
 250
 251     for (int i = 0; i < powerLevelCount; i++) {
 252         sbufWriteU16(dst, levels[i]);
 253         sbufWriteU16(dst, powers[i]);
 254     }
 255 }
 256
 257 void vtxCommonSerializeDeviceStatus(const vtxDevice_t *vtxDevice, sbuf_t *dst)
 258 {
 259     if (vtxDevice) {
 260         const vtxDevType_e vtxType = vtxCommonGetDeviceType(vtxDevice);
 261         const bool deviceReady = vtxCommonDeviceIsReady(vtxDevice);
 262
 263         uint8_t band = 0;
 264         uint8_t channel = 0;
 265         const bool bandAndChannelAvailable = vtxCommonGetBandAndChannel(vtxDevice, &band, &channel);
 266
 267         uint8_t powerIndex = 0;
 268         const bool powerIndexAvailable = vtxCommonGetPowerIndex(vtxDevice, &powerIndex);
 269
 270         uint16_t frequency = 0;
 271         const bool frequencyAvailable = vtxCommonGetFrequency(vtxDevice, &frequency);
 272
 273         unsigned vtxStatus = 0; // pitmode and/or locked
 274         const bool vtxStatusAvailable = vtxCommonGetStatus(vtxDevice, &vtxStatus);
 275
 276         sbufWriteU8(dst, vtxType);
 277         sbufWriteU8(dst, deviceReady);
 278
 279         sbufWriteU8(dst, bandAndChannelAvailable);
 280         sbufWriteU8(dst, band);
 281         sbufWriteU8(dst, channel);
 282
 283         sbufWriteU8(dst, powerIndexAvailable);
 284         sbufWriteU8(dst, powerIndex);
 285
 286         sbufWriteU8(dst, frequencyAvailable);
 287         sbufWriteU16(dst, frequency);
 288
 289         sbufWriteU8(dst, vtxStatusAvailable);
 290         sbufWriteU32(dst, vtxStatus);
 291
 292         vtxCommonSerializePowerLevels(vtxDevice, dst);
 293         vtxCommonSerializeCustomDeviceStatus(vtxDevice, dst);
 294     }
 295 }
 296
 297 #endif