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