src/main/io/spektrum_vtx_control.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 #include "platform.h"
  22 #if defined(USE_SPEKTRUM_VTX_CONTROL) && defined(USE_VTX_COMMON)
  23
  24 #include <string.h>
  25
  26 #include "config/config.h"
  27 #include "drivers/vtx_common.h"
  28 #include "io/vtx.h"
  29
  30 #include "io/spektrum_vtx_control.h"
  31
  32 // We can not use the common set/get-frequncy API.
  33 // Some VTX devices do not support it.
  34 //#define USE_VTX_COMMON_FREQ_API
  35
  36 #ifdef USE_VTX_COMMON_FREQ_API
  37 const uint16_t SpektrumVtxfrequencyTable[SPEKTRUM_VTX_BAND_COUNT][SPEKTRUM_VTX_CHAN_COUNT] =
  38     {
  39         { 5740, 5760, 5780, 5800, 5820, 5840, 5860, 5880 }, // FatShark
  40         { 5658, 5695, 5732, 5769, 5806, 5843, 5880, 5917 }, // RaceBand
  41         { 5705, 5685, 5665, 5645, 5885, 5905, 5925, 5945 }, // Boscam E
  42         { 5733, 5752, 5771, 5790, 5809, 5828, 5847, 5866 }, // Boscam B
  43         { 5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725 }, // Boscam A
  44     };
  45 #else
  46 // Translation table, Spektrum bands to BF internal vtx_common bands
  47 const uint8_t spek2commonBand[SPEKTRUM_VTX_BAND_COUNT]= {
  48     VTX_COMMON_BAND_FS,
  49     VTX_COMMON_BAND_RACE,
  50     VTX_COMMON_BAND_E,
  51     VTX_COMMON_BAND_B,
  52     VTX_COMMON_BAND_A,
  53 };
  54 #endif
  55
  56 // RF Power Index translation tables. No generic power API available.....
  57
  58 #ifdef USE_VTX_TRAMP
  59 // Tramp "---", 25, 200, 400. 600 mW
  60 const uint8_t vtxTrampPi[SPEKTRUM_VTX_POWER_COUNT] = {
  61                                        // Spektrum Spec    Tx menu  Tx sends   To VTX    Watt
  62     VTX_TRAMP_POWER_OFF,               //         Off      INHIBIT         0        0     -
  63     VTX_TRAMP_POWER_OFF,               //   1 -  14mW            -         -        -     -
  64     VTX_TRAMP_POWER_25,                //  15 -  25mW   15 -  25mW         2        1    25mW
  65     VTX_TRAMP_POWER_100,               //  26 -  99mW   26 -  99mW         3        2   100mW Slightly outside range
  66     VTX_TRAMP_POWER_200,               // 100 - 299mW  100 - 200mW         4        3   200mW
  67     VTX_TRAMP_POWER_400,               // 300 - 600mW  300 - 600mW         5        4   400mW
  68     VTX_TRAMP_POWER_600,               // 601 - max    601+ mW             6        5   600mW Slightly outside range
  69     VTX_TRAMP_POWER_200                // Manual               -           -        -     -
  70 };
  71 #endif // USE_VTX_TRAMP
  72 //todo: enable pit mode where appropriate, for all protcols
  73 #ifdef USE_VTX_RTC6705
  74 // RTC6705 "---", 25 or 200 mW
  75 const uint8_t vtxRTC6705Pi[SPEKTRUM_VTX_POWER_COUNT] = {
  76     VTX_6705_POWER_25,                // Off
  77     VTX_6705_POWER_25,                //   1 -  14mW
  78     VTX_6705_POWER_25,                 //  15 -  25mW
  79     VTX_6705_POWER_25,                 //  26 -  99mW
  80     VTX_6705_POWER_200,                // 100 - 299mW
  81     VTX_6705_POWER_200,                // 300 - 600mW
  82     VTX_6705_POWER_200,                // 601 - max
  83     VTX_6705_POWER_200                 // Manual
  84 };
  85 #endif //USE_VTX_RTC6705
  86
  87 #ifdef USE_VTX_SMARTAUDIO
  88 // SmartAudio "---", 25, 200, 500. 800 mW
  89 const uint8_t vtxSaPi[SPEKTRUM_VTX_POWER_COUNT] = {
  90     VTX_SA_POWER_OFF,                  // Off
  91     VTX_SA_POWER_OFF,                  //   1 -  14mW
  92     VTX_SA_POWER_25,                   //  15 -  25mW
  93     VTX_SA_POWER_25,                   //  26 -  99mW
  94     VTX_SA_POWER_200,                  // 100 - 299mW
  95     VTX_SA_POWER_500,                  // 300 - 600mW
  96     VTX_SA_POWER_800,                  // 601 - max
  97     VTX_SA_POWER_200                   // Manual
  98 };
  99 #endif // USE_VTX_SMARTAUDIO
 100
 101 uint8_t convertSpektrumVtxPowerIndex(uint8_t sPower)
 102 {
 103     uint8_t devicePower = 0;
 104
 105     const vtxDevice_t *vtxDevice = vtxCommonDevice();
 106     switch (vtxCommonGetDeviceType(vtxDevice)) {
 107 #ifdef USE_VTX_RTC6705
 108     case VTXDEV_RTC6705:
 109         devicePower = vtxRTC6705Pi[sPower];
 110         break;
 111 #endif // USE_VTX_RTC6705
 112
 113 #ifdef USE_VTX_SMARTAUDIO
 114     case VTXDEV_SMARTAUDIO:
 115         devicePower = vtxSaPi[sPower];
 116         break;
 117 #endif // USE_VTX_SMARTAUDIO
 118
 119 #ifdef USE_VTX_TRAMP
 120     case VTXDEV_TRAMP:
 121         devicePower = vtxTrampPi[sPower];
 122         break;
 123 #endif // USE_VTX_TRAMP
 124
 125     case VTXDEV_UNKNOWN:
 126     case VTXDEV_UNSUPPORTED:
 127     default:
 128         break;
 129
 130     }
 131     return devicePower;
 132 }
 133
 134 #ifdef USE_SPEKTRUM_REGION_CODES
 135 // Just a global SpektrumRegion for now, To save VTX ctrl input to VTX tm output.
 136 // Would need a PG item to survive power cycle. Not really used so let it be as is.
 137 uint8_t SpektrumRegion = SPEKTRUM_VTX_REGION_NONE;
 138 #endif
 139
 140 // Mark an inital invalid VTX ctrl frame to force first VTX settings cheange to actually come from Tx/Rx.
 141 static uint32_t vtxControl_ipc = ~(SPEKTRUM_VTX_CONTROL_FRAME);
 142
 143 // ############ RX task ######################
 144 void spektrumHandleVtxControl(uint32_t vtxCntrl)
 145 {
 146   vtxControl_ipc = vtxCntrl;
 147 }
 148 // ###########################################
 149
 150
 151 // ############ VTX_CONTROL task #############
 152 void spektrumVtxControl(void)
 153 {
 154     static uint32_t prevVtxControl =0;
 155     uint32_t vtxControl;
 156
 157     // Check for invalid VTX ctrl frames
 158     if ((vtxControl_ipc & SPEKTRUM_VTX_CONTROL_FRAME_MASK) != SPEKTRUM_VTX_CONTROL_FRAME) return;
 159
 160     vtxControl = vtxControl_ipc;
 161     vtxControl_ipc = 0;
 162
 163     if (prevVtxControl == vtxControl) return;
 164     prevVtxControl = vtxControl;
 165
 166     spektrumVtx_t vtx = {
 167         .pitMode = (vtxControl & SPEKTRUM_VTX_PIT_MODE_MASK) >> SPEKTRUM_VTX_PIT_MODE_SHIFT,
 168         .region  = (vtxControl & SPEKTRUM_VTX_REGION_MASK)   >> SPEKTRUM_VTX_REGION_SHIFT,
 169         .power   = (vtxControl & SPEKTRUM_VTX_POWER_MASK)    >> SPEKTRUM_VTX_POWER_SHIFT,
 170         .band    = (vtxControl & SPEKTRUM_VTX_BAND_MASK)     >> SPEKTRUM_VTX_BAND_SHIFT,
 171         .channel = (vtxControl & SPEKTRUM_VTX_CHANNEL_MASK)  >> SPEKTRUM_VTX_CHANNEL_SHIFT,
 172 #ifdef USE_SPEKTRUM_REGION_CODES
 173         .region  = (vtxControl & SPEKTRUM_VTX_REGION_MASK)   >> SPEKTRUM_VTX_REGION_SHIFT;
 174 #endif
 175     };
 176
 177     const vtxSettingsConfig_t prevSettings = {
 178         .band    = vtxSettingsConfig()->band,
 179         .channel = vtxSettingsConfig()->channel,
 180         .freq    = vtxSettingsConfig()->freq,
 181         .power   = vtxSettingsConfig()->power,
 182         .lowPowerDisarm = vtxSettingsConfig()->lowPowerDisarm,
 183     };
 184     vtxSettingsConfig_t newSettings = prevSettings;
 185
 186     vtxDevice_t *vtxDevice = vtxCommonDevice();
 187     if (vtxDevice) {
 188 #ifdef USE_VTX_COMMON_FREQ_API
 189         uint16_t freq = SpektrumVtxfrequencyTable[vtx.band][vtx.channel];
 190         if (prevSettings.freq != freq) {
 191             newSettings.band    = VTX_COMMON_BAND_USER;
 192             newSettings.channel = vtx.channel;
 193             newSettings.freq    = freq;
 194         }
 195 #else
 196         // Convert to the internal Common Band index
 197         const uint8_t band    = spek2commonBand[vtx.band];
 198         const uint8_t channel = vtx.channel +1; // 0 based to 1 based
 199         if ((prevSettings.band != band) || (prevSettings.channel != channel)) {
 200             newSettings.band    = band;
 201             newSettings.channel = channel;
 202             newSettings.freq    = vtxCommonLookupFrequency(vtxDevice, band, channel);
 203         }
 204 #endif
 205         // Seems to be no unified internal VTX API standard for power levels/indexes, VTX device brand specific.
 206         const uint8_t power = convertSpektrumVtxPowerIndex(vtx.power);
 207         if (prevSettings.power != power) {
 208             newSettings.power   = power;
 209         }
 210         // Everyone seems to agree on what PIT ON/OFF means
 211         unsigned vtxCurrentStatus;
 212         if (vtxCommonGetStatus(vtxDevice, &vtxCurrentStatus)) {
 213             if ((vtxCurrentStatus & VTX_STATUS_PIT_MODE) != vtx.pitMode) {
 214                 vtxCommonSetPitMode(vtxDevice, vtx.pitMode);
 215             }
 216         }
 217     }
 218
 219     if (memcmp(&prevSettings,&newSettings,sizeof(vtxSettingsConfig_t))) {
 220         vtxSettingsConfigMutable()->band = newSettings.band;
 221         vtxSettingsConfigMutable()->channel = newSettings.channel;
 222         vtxSettingsConfigMutable()->power   = newSettings.power;
 223         vtxSettingsConfigMutable()->freq    = newSettings.freq;
 224         saveConfigAndNotify();
 225     }
 226 #ifdef USE_SPEKTRUM_REGION_CODES
 227     // Save region code
 228     SpektrumRegion = vtx.region;
 229 #endif
 230
 231 }
 232
 233 #endif // USE_SPEKTRUM_VTX_CONTROL && USE_VTX_COMMON