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
  37 static vtxDevice_t *vtxDevice = NULL;
  38 static uint8_t selectedBand = 0;
  39 static uint8_t selectedChannel = 0;
  40
  41 void vtxCommonInit(void)
  42 {
  43 }
  44
  45 void vtxCommonSetDevice(vtxDevice_t *pDevice)
  46 {
  47     vtxDevice = pDevice;
  48 }
  49
  50 vtxDevice_t *vtxCommonDevice(void)
  51 {
  52     return vtxDevice;
  53 }
  54
  55 vtxDevType_e vtxCommonGetDeviceType(const vtxDevice_t *vtxDevice)
  56 {
  57     if (!vtxDevice) {
  58         return VTXDEV_UNKNOWN;
  59     }
  60     return vtxDevice->vTable->getDeviceType(vtxDevice);
  61 }
  62
  63 bool vtxCommonDeviceIsReady(const vtxDevice_t *vtxDevice)
  64 {
  65     if (vtxDevice && vtxDevice->vTable->isReady) {
  66         return vtxDevice->vTable->isReady(vtxDevice);
  67     }
  68
  69     return false;
  70 }
  71
  72 void vtxCommonProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
  73 {
  74     if (vtxDevice) {
  75         vtxDevice->vTable->process(vtxDevice, currentTimeUs);
  76     }
  77 }
  78
  79 // band and channel are 1 origin
  80 void vtxCommonSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel)
  81 {
  82     uint16_t freq = vtxCommonLookupFrequency(vtxDevice, band, channel);
  83     if (freq != 0) {
  84         selectedChannel = channel;
  85         selectedBand = band;
  86         if (vtxTableIsFactoryBand[band - 1]) {
  87             vtxDevice->vTable->setBandAndChannel(vtxDevice, band, channel);
  88         } else {
  89             vtxDevice->vTable->setFrequency(vtxDevice, freq);
  90         }
  91     }
  92 }
  93
  94 // index is one origin, zero = unknown power level
  95 void vtxCommonSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index)
  96 {
  97     if (index <= vtxTablePowerLevels) {
  98         vtxDevice->vTable->setPowerByIndex(vtxDevice, index);
  99     }
 100 }
 101
 102 // on = 1, off = 0
 103 void vtxCommonSetPitMode(vtxDevice_t *vtxDevice, uint8_t onOff)
 104 {
 105     vtxDevice->vTable->setPitMode(vtxDevice, onOff);
 106 }
 107
 108 void vtxCommonSetFrequency(vtxDevice_t *vtxDevice, uint16_t frequency)
 109 {
 110     selectedBand = 0;
 111     selectedChannel = 0;
 112     vtxDevice->vTable->setFrequency(vtxDevice, frequency);
 113 }
 114
 115 bool vtxCommonGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel)
 116 {
 117     bool result = vtxDevice->vTable->getBandAndChannel(vtxDevice, pBand, pChannel);
 118     if ((!result || (*pBand == 0 && *pChannel == 0)) && selectedBand != 0 && selectedChannel != 0
 119         && !vtxTableIsFactoryBand[selectedBand - 1]) {
 120         uint16_t freq;
 121         result = vtxCommonGetFrequency(vtxDevice, &freq);
 122         if (result) {
 123             vtxCommonLookupBandChan(vtxDevice, freq, pBand, pChannel);
 124         }
 125     }
 126     return result;
 127 }
 128
 129 bool vtxCommonGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
 130 {
 131     return vtxDevice->vTable->getPowerIndex(vtxDevice, pIndex);
 132 }
 133
 134 bool vtxCommonGetFrequency(const vtxDevice_t *vtxDevice, uint16_t *pFrequency)
 135 {
 136     return vtxDevice->vTable->getFrequency(vtxDevice, pFrequency);
 137 }
 138
 139 bool vtxCommonGetStatus(const vtxDevice_t *vtxDevice, unsigned *status)
 140 {
 141     return vtxDevice->vTable->getStatus(vtxDevice, status);
 142 }
 143
 144 uint8_t vtxCommonGetVTXPowerLevels(const vtxDevice_t *vtxDevice, uint16_t *levels, uint16_t *powers)
 145 {
 146     return vtxDevice->vTable->getPowerLevels(vtxDevice, levels, powers);
 147 }
 148
 149 const char *vtxCommonLookupBandName(const vtxDevice_t *vtxDevice, int band)
 150 {
 151     if (vtxDevice && band > 0 && band <= vtxTableBandCount) {
 152         return vtxTableBandNames[band];
 153     } else {
 154         return "?";
 155     }
 156 }
 157
 158 char vtxCommonLookupBandLetter(const vtxDevice_t *vtxDevice, int band)
 159 {
 160     if (vtxDevice && band > 0 && band <= vtxTableBandCount) {
 161         return vtxTableBandLetters[band];
 162     } else {
 163         return '?';
 164     }
 165 }
 166
 167 const char *vtxCommonLookupChannelName(const vtxDevice_t *vtxDevice, int channel)
 168 {
 169     if (vtxDevice && channel > 0 && channel <= vtxTableChannelCount) {
 170         return vtxTableChannelNames[channel];
 171     } else {
 172         return "?";
 173     }
 174 }
 175
 176 //Converts frequency (in MHz) to band and channel values.
 177 //If frequency not found in the vtxtable then band and channel will return 0
 178 void vtxCommonLookupBandChan(const vtxDevice_t *vtxDevice, uint16_t freq, uint8_t *pBand, uint8_t *pChannel)
 179 {
 180     *pBand = 0;
 181     *pChannel = 0;
 182     if (vtxDevice) {
 183         // Use reverse lookup order so that 5880Mhz
 184         // get Raceband 7 instead of Fatshark 8.
 185         for (int band = vtxTableBandCount - 1 ; band >= 0 ; band--) {
 186             for (int channel = 0 ; channel < vtxTableChannelCount ; channel++) {
 187                 if (vtxTableFrequency[band][channel] == freq) {
 188                     *pBand = band + 1;
 189                     *pChannel = channel + 1;
 190                     return;
 191                 }
 192             }
 193         }
 194     }
 195 }
 196
 197 //Converts band and channel values to a frequency (in MHz) value.
 198 // band:  Band value (1 to 5).
 199 // channel:  Channel value (1 to 8).
 200 // Returns frequency value (in MHz), or 0 if band/channel out of range.
 201 uint16_t vtxCommonLookupFrequency(const vtxDevice_t *vtxDevice, int band, int channel)
 202 {
 203     if (vtxDevice) {
 204         if (band > 0 && band <= vtxTableBandCount &&
 205             channel > 0 && channel <= vtxTableChannelCount) {
 206             return vtxTableFrequency[band - 1][channel - 1];
 207         }
 208     }
 209
 210     return 0;
 211 }
 212
 213 const char *vtxCommonLookupPowerName(const vtxDevice_t *vtxDevice, int index)
 214 {
 215     if (vtxDevice && index > 0 && index <= vtxTablePowerLevels) {
 216         return vtxTablePowerLabels[index];
 217     } else {
 218         return "?";
 219     }
 220 }
 221
 222 bool vtxCommonLookupPowerValue(const vtxDevice_t *vtxDevice, int index, uint16_t *pPowerValue)
 223 {
 224     if (vtxDevice && index > 0 && index <= vtxTablePowerLevels) {
 225         *pPowerValue = vtxTablePowerValues[index - 1];
 226         return true;
 227     } else {
 228         return false;
 229     }
 230 }
 231
 232 static void vtxCommonSerializeCustomDeviceStatus(const vtxDevice_t *vtxDevice, sbuf_t *dst)
 233 {
 234     const bool customDeviceStatusAvailable = vtxDevice && vtxDevice->vTable->serializeCustomDeviceStatus;
 235
 236     if (customDeviceStatusAvailable) {
 237         vtxDevice->vTable->serializeCustomDeviceStatus(vtxDevice, dst);
 238     } else {
 239         sbufWriteU8(dst, 0);
 240     }
 241 }
 242
 243 static void vtxCommonSerializePowerLevels(const vtxDevice_t *vtxDevice, sbuf_t *dst)
 244 {
 245     uint16_t levels[VTX_TABLE_MAX_POWER_LEVELS];
 246     uint16_t powers[VTX_TABLE_MAX_POWER_LEVELS];
 247
 248     const uint8_t powerLevelCount = vtxCommonGetVTXPowerLevels(vtxDevice, levels, powers);
 249
 250     sbufWriteU8(dst, powerLevelCount);
 251
 252     for (int i = 0; i < powerLevelCount; i++) {
 253         sbufWriteU16(dst, levels[i]);
 254         sbufWriteU16(dst, powers[i]);
 255     }
 256 }
 257
 258 void vtxCommonSerializeDeviceStatus(const vtxDevice_t *vtxDevice, sbuf_t *dst)
 259 {
 260     if (vtxDevice) {
 261         const vtxDevType_e vtxType = vtxCommonGetDeviceType(vtxDevice);
 262         const bool deviceReady = vtxCommonDeviceIsReady(vtxDevice);
 263
 264         uint8_t band = 0;
 265         uint8_t channel = 0;
 266         const bool bandAndChannelAvailable = vtxCommonGetBandAndChannel(vtxDevice, &band, &channel);
 267
 268         uint8_t powerIndex = 0;
 269         const bool powerIndexAvailable = vtxCommonGetPowerIndex(vtxDevice, &powerIndex);
 270
 271         uint16_t frequency = 0;
 272         const bool frequencyAvailable = vtxCommonGetFrequency(vtxDevice, &frequency);
 273
 274         unsigned vtxStatus = 0; // pitmode and/or locked
 275         const bool vtxStatusAvailable = vtxCommonGetStatus(vtxDevice, &vtxStatus);
 276
 277         sbufWriteU8(dst, vtxType);
 278         sbufWriteU8(dst, deviceReady);
 279
 280         sbufWriteU8(dst, bandAndChannelAvailable);
 281         sbufWriteU8(dst, band);
 282         sbufWriteU8(dst, channel);
 283
 284         sbufWriteU8(dst, powerIndexAvailable);
 285         sbufWriteU8(dst, powerIndex);
 286
 287         sbufWriteU8(dst, frequencyAvailable);
 288         sbufWriteU16(dst, frequency);
 289
 290         sbufWriteU8(dst, vtxStatusAvailable);
 291         sbufWriteU32(dst, vtxStatus);
 292
 293         vtxCommonSerializePowerLevels(vtxDevice, dst);
 294         vtxCommonSerializeCustomDeviceStatus(vtxDevice, dst);
 295     }
 296 }
 297
 298 #endif