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