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