src/main/io/vtx.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 <stdint.h>
  22 #include <string.h>
  23
  24 #include "platform.h"
  25
  26 #if defined(USE_VTX_COMMON)
  27
  28 #include "cli/cli.h"
  29
  30 #include "common/maths.h"
  31 #include "common/time.h"
  32
  33 #include "drivers/vtx_common.h"
  34 #include "drivers/vtx_table.h"
  35
  36 #include "config/config.h"
  37 #include "fc/rc_modes.h"
  38 #include "fc/runtime_config.h"
  39
  40 #include "flight/failsafe.h"
  41
  42 #include "io/vtx_control.h"
  43
  44 #include "pg/pg.h"
  45 #include "pg/pg_ids.h"
  46
  47 #include "vtx.h"
  48
  49
  50 PG_REGISTER_WITH_RESET_FN(vtxSettingsConfig_t, vtxSettingsConfig, PG_VTX_SETTINGS_CONFIG, 1);
  51
  52 void pgResetFn_vtxSettingsConfig(vtxSettingsConfig_t *vtxSettingsConfig)
  53 {
  54 #ifdef USE_VTX_TABLE
  55     vtxSettingsConfig->band = 0;
  56     vtxSettingsConfig->channel = 0;
  57     vtxSettingsConfig->power = 0;
  58     vtxSettingsConfig->freq = 0;
  59 #else
  60     vtxSettingsConfig->freq = VTX_TABLE_DEFAULT_FREQ;
  61     vtxSettingsConfig->band = VTX_TABLE_DEFAULT_BAND;
  62     vtxSettingsConfig->channel = VTX_TABLE_DEFAULT_CHANNEL;
  63     vtxSettingsConfig->power = VTX_TABLE_DEFAULT_POWER;
  64 #endif
  65     vtxSettingsConfig->pitModeFreq = VTX_TABLE_DEFAULT_PITMODE_FREQ;
  66     vtxSettingsConfig->lowPowerDisarm = VTX_LOW_POWER_DISARM_OFF;
  67     vtxSettingsConfig->softserialAlt = 0;
  68 }
  69
  70 typedef enum {
  71     VTX_PARAM_POWER = 0,
  72     VTX_PARAM_BANDCHAN,
  73     VTX_PARAM_PITMODE,
  74     VTX_PARAM_CONFIRM,
  75     VTX_PARAM_COUNT
  76 } vtxScheduleParams_e;
  77
  78 void vtxInit(void)
  79 {
  80     bool settingsUpdated = false;
  81
  82     vtxDevice_t *vtxDevice = vtxCommonDevice();
  83
  84     if (!vtxDevice) {
  85         // If a device is not registered, we don't have any table to refer.
  86         // Don't manipulate settings and just return in this case.
  87         return;
  88     }
  89
  90     // sync frequency in parameter group when band/channel are specified
  91     const uint16_t freq = vtxCommonLookupFrequency(vtxDevice, vtxSettingsConfig()->band, vtxSettingsConfig()->channel);
  92     if (vtxSettingsConfig()->band && freq != vtxSettingsConfig()->freq) {
  93         vtxSettingsConfigMutable()->freq = freq;
  94         settingsUpdated = true;
  95     }
  96
  97 #if defined(VTX_SETTINGS_FREQCMD)
  98     // constrain pit mode frequency
  99     if (vtxSettingsConfig()->pitModeFreq) {
 100         const uint16_t constrainedPitModeFreq = MAX(vtxSettingsConfig()->pitModeFreq, VTX_TABLE_MIN_USER_FREQ);
 101         if (constrainedPitModeFreq != vtxSettingsConfig()->pitModeFreq) {
 102             vtxSettingsConfigMutable()->pitModeFreq = constrainedPitModeFreq;
 103             settingsUpdated = true;
 104         }
 105     }
 106 #endif
 107
 108     if (settingsUpdated) {
 109         saveConfigAndNotify();
 110     }
 111 }
 112
 113 STATIC_UNIT_TESTED vtxSettingsConfig_t vtxGetSettings(void)
 114 {
 115     vtxSettingsConfig_t settings = {
 116         .band = vtxSettingsConfig()->band,
 117         .channel = vtxSettingsConfig()->channel,
 118         .power = vtxSettingsConfig()->power,
 119         .freq = vtxSettingsConfig()->freq,
 120         .pitModeFreq = vtxSettingsConfig()->pitModeFreq,
 121         .lowPowerDisarm = vtxSettingsConfig()->lowPowerDisarm,
 122     };
 123
 124 #if defined(VTX_SETTINGS_FREQCMD)
 125     if (IS_RC_MODE_ACTIVE(BOXVTXPITMODE) && settings.pitModeFreq) {
 126         settings.band = 0;
 127         settings.freq = settings.pitModeFreq;
 128         settings.power = VTX_TABLE_DEFAULT_POWER;
 129     }
 130 #endif
 131
 132     if (!ARMING_FLAG(ARMED) && !failsafeIsActive() &&
 133         (settings.lowPowerDisarm == VTX_LOW_POWER_DISARM_ALWAYS ||
 134         (settings.lowPowerDisarm == VTX_LOW_POWER_DISARM_UNTIL_FIRST_ARM && !ARMING_FLAG(WAS_EVER_ARMED)))) {
 135         settings.power = VTX_TABLE_DEFAULT_POWER;
 136     }
 137
 138     return settings;
 139 }
 140
 141 static bool vtxProcessBandAndChannel(vtxDevice_t *vtxDevice)
 142 {
 143     if (!ARMING_FLAG(ARMED)) {
 144         uint8_t vtxBand;
 145         uint8_t vtxChan;
 146         if (vtxCommonGetBandAndChannel(vtxDevice, &vtxBand, &vtxChan)) {
 147             const vtxSettingsConfig_t settings = vtxGetSettings();
 148             if (vtxBand != settings.band || vtxChan != settings.channel) {
 149                 vtxCommonSetBandAndChannel(vtxDevice, settings.band, settings.channel);
 150                 return true;
 151             }
 152         }
 153     }
 154     return false;
 155 }
 156
 157 #if defined(VTX_SETTINGS_FREQCMD)
 158 static bool vtxProcessFrequency(vtxDevice_t *vtxDevice)
 159 {
 160     if (!ARMING_FLAG(ARMED)) {
 161         uint16_t vtxFreq;
 162         if (vtxCommonGetFrequency(vtxDevice, &vtxFreq)) {
 163             const vtxSettingsConfig_t settings = vtxGetSettings();
 164             if (vtxFreq != settings.freq) {
 165                 vtxCommonSetFrequency(vtxDevice, settings.freq);
 166                 return true;
 167             }
 168         }
 169     }
 170     return false;
 171 }
 172 #endif
 173
 174 static bool vtxProcessPower(vtxDevice_t *vtxDevice)
 175 {
 176     uint8_t vtxPower;
 177     if (vtxCommonGetPowerIndex(vtxDevice, &vtxPower)) {
 178         const vtxSettingsConfig_t settings = vtxGetSettings();
 179         if (vtxPower != settings.power) {
 180             vtxCommonSetPowerByIndex(vtxDevice, settings.power);
 181             return true;
 182         }
 183     }
 184     return false;
 185 }
 186
 187 static bool vtxProcessPitMode(vtxDevice_t *vtxDevice)
 188 {
 189     static bool prevPmSwitchState = false;
 190
 191     unsigned vtxStatus;
 192     if (!ARMING_FLAG(ARMED) && vtxCommonGetStatus(vtxDevice, &vtxStatus)) {
 193         bool currPmSwitchState = IS_RC_MODE_ACTIVE(BOXVTXPITMODE);
 194
 195         if (currPmSwitchState != prevPmSwitchState) {
 196             prevPmSwitchState = currPmSwitchState;
 197
 198             if (currPmSwitchState) {
 199 #if defined(VTX_SETTINGS_FREQCMD)
 200                 if (vtxSettingsConfig()->pitModeFreq) {
 201                     return false;
 202                 }
 203 #endif
 204                 if (!(vtxStatus & VTX_STATUS_PIT_MODE)) {
 205                     vtxCommonSetPitMode(vtxDevice, true);
 206
 207                     return true;
 208                 }
 209             } else {
 210                 if (vtxStatus & VTX_STATUS_PIT_MODE) {
 211                     vtxCommonSetPitMode(vtxDevice, false);
 212
 213                     return true;
 214                 }
 215             }
 216         }
 217     }
 218
 219     return false;
 220 }
 221
 222 static bool vtxProcessStateUpdate(vtxDevice_t *vtxDevice)
 223 {
 224     const vtxSettingsConfig_t vtxSettingsState = vtxGetSettings();
 225     vtxSettingsConfig_t vtxState = vtxSettingsState;
 226
 227     if (vtxSettingsState.band) {
 228         vtxCommonGetBandAndChannel(vtxDevice, &vtxState.band, &vtxState.channel);
 229 #if defined(VTX_SETTINGS_FREQCMD)
 230     } else {
 231         vtxCommonGetFrequency(vtxDevice, &vtxState.freq);
 232 #endif
 233     }
 234
 235     vtxCommonGetPowerIndex(vtxDevice, &vtxState.power);
 236
 237     return (bool)memcmp(&vtxSettingsState, &vtxState, sizeof(vtxSettingsConfig_t));
 238 }
 239
 240 void vtxUpdate(timeUs_t currentTimeUs)
 241 {
 242     static uint8_t currentSchedule = 0;
 243
 244     if (cliMode) {
 245         return;
 246     }
 247
 248     vtxDevice_t *vtxDevice = vtxCommonDevice();
 249     if (vtxDevice) {
 250         // Check input sources for config updates
 251         vtxControlInputPoll();
 252
 253         const uint8_t startingSchedule = currentSchedule;
 254         bool vtxUpdatePending = false;
 255         do {
 256             switch (currentSchedule) {
 257                 case VTX_PARAM_POWER:
 258                     vtxUpdatePending = vtxProcessPower(vtxDevice);
 259                     break;
 260                 case VTX_PARAM_BANDCHAN:
 261                     if (vtxGetSettings().band) {
 262                         vtxUpdatePending = vtxProcessBandAndChannel(vtxDevice);
 263 #if defined(VTX_SETTINGS_FREQCMD)
 264                     } else {
 265                         vtxUpdatePending = vtxProcessFrequency(vtxDevice);
 266 #endif
 267                     }
 268                     break;
 269                 case VTX_PARAM_PITMODE:
 270                     vtxUpdatePending = vtxProcessPitMode(vtxDevice);
 271                     break;
 272                 case VTX_PARAM_CONFIRM:
 273                     vtxUpdatePending = vtxProcessStateUpdate(vtxDevice);
 274                     break;
 275                 default:
 276                     break;
 277             }
 278             currentSchedule = (currentSchedule + 1) % VTX_PARAM_COUNT;
 279         } while (!vtxUpdatePending && currentSchedule != startingSchedule);
 280
 281         if (!ARMING_FLAG(ARMED) || vtxUpdatePending) {
 282             vtxCommonProcess(vtxDevice, currentTimeUs);
 283         }
 284     }
 285 }
 286
 287 #endif