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 #if defined(USE_VTX_CONTROL)
  26
  27 #include "common/maths.h"
  28 #include "common/time.h"
  29 #include "common/utils.h"
  30
  31 #include "config/parameter_group.h"
  32 #include "config/parameter_group_ids.h"
  33
  34 #include "drivers/vtx_common.h"
  35
  36 #include "fc/cli.h"
  37 #include "fc/config.h"
  38 #include "fc/rc_modes.h"
  39 #include "fc/runtime_config.h"
  40 #include "fc/settings.h"
  41
  42 #include "flight/failsafe.h"
  43
  44 #include "io/vtx.h"
  45 #include "io/vtx_string.h"
  46 #include "io/vtx_control.h"
  47
  48 PG_REGISTER_WITH_RESET_TEMPLATE(vtxSettingsConfig_t, vtxSettingsConfig, PG_VTX_SETTINGS_CONFIG, 2);
  49
  50 PG_RESET_TEMPLATE(vtxSettingsConfig_t, vtxSettingsConfig,
  51     .band = SETTING_VTX_BAND_DEFAULT,
  52     .channel = SETTING_VTX_CHANNEL_DEFAULT,
  53     .power = SETTING_VTX_POWER_DEFAULT,
  54     .pitModeChan = SETTING_VTX_PIT_MODE_CHAN_DEFAULT,
  55     .lowPowerDisarm = SETTING_VTX_LOW_POWER_DISARM_DEFAULT,
  56     .maxPowerOverride = SETTING_VTX_MAX_POWER_OVERRIDE_DEFAULT,
  57     .frequencyGroup = SETTING_VTX_FREQUENCY_GROUP_DEFAULT,
  58 );
  59
  60 typedef enum {
  61     VTX_PARAM_POWER = 0,
  62     VTX_PARAM_BANDCHAN,
  63     VTX_PARAM_PITMODE,
  64     VTX_PARAM_COUNT
  65 } vtxScheduleParams_e;
  66
  67 void vtxInit(void)
  68 {
  69 }
  70
  71 static vtxSettingsConfig_t * vtxGetRuntimeSettings(void)
  72 {
  73     static vtxSettingsConfig_t settings;
  74
  75     settings.band = vtxSettingsConfig()->band;
  76     settings.channel = vtxSettingsConfig()->channel;
  77     settings.power = vtxSettingsConfig()->power;
  78     settings.pitModeChan = vtxSettingsConfig()->pitModeChan;
  79     settings.lowPowerDisarm = vtxSettingsConfig()->lowPowerDisarm;
  80
  81     if (!ARMING_FLAG(ARMED) && !failsafeIsActive() &&
  82         ((settings.lowPowerDisarm == VTX_LOW_POWER_DISARM_ALWAYS) ||
  83         (settings.lowPowerDisarm == VTX_LOW_POWER_DISARM_UNTIL_FIRST_ARM && !ARMING_FLAG(WAS_EVER_ARMED)))) {
  84
  85         settings.power = VTX_SETTINGS_DEFAULT_POWER;
  86     }
  87
  88     return &settings;
  89 }
  90
  91 static bool vtxProcessBandAndChannel(vtxDevice_t *vtxDevice, const vtxSettingsConfig_t * runtimeSettings)
  92 {
  93     uint8_t vtxBand;
  94     uint8_t vtxChan;
  95
  96     // Shortcut for undefined band
  97     if (!runtimeSettings->band) {
  98         return false;
  99     }
 100
 101     if (!vtxCommonGetBandAndChannel(vtxDevice, &vtxBand, &vtxChan)) {
 102         return false;
 103     }
 104
 105     if (vtxBand != runtimeSettings->band || vtxChan != runtimeSettings->channel) {
 106         vtxCommonSetBandAndChannel(vtxDevice, runtimeSettings->band, runtimeSettings->channel);
 107         return true;
 108     }
 109
 110     return false;
 111 }
 112
 113 static bool vtxProcessPower(vtxDevice_t *vtxDevice, const vtxSettingsConfig_t * runtimeSettings)
 114 {
 115     uint8_t vtxPower;
 116
 117     if (!vtxCommonGetPowerIndex(vtxDevice, &vtxPower)) {
 118         return false;
 119     }
 120
 121     if (vtxPower != runtimeSettings->power) {
 122         vtxCommonSetPowerByIndex(vtxDevice, runtimeSettings->power);
 123         return true;
 124     }
 125
 126     return false;
 127 }
 128
 129 static bool vtxProcessPitMode(vtxDevice_t *vtxDevice, const vtxSettingsConfig_t * runtimeSettings)
 130 {
 131     UNUSED(runtimeSettings);
 132
 133     uint8_t pitOnOff;
 134
 135     bool        currPmSwitchState = false;
 136     static bool prevPmSwitchState = false;
 137
 138     if (!vtxCommonGetPitMode(vtxDevice, &pitOnOff)) {
 139         return false;
 140     }
 141
 142     if (currPmSwitchState != prevPmSwitchState) {
 143         prevPmSwitchState = currPmSwitchState;
 144
 145         if (currPmSwitchState) {
 146             if (0) {
 147                 if (!pitOnOff) {
 148                     vtxCommonSetPitMode(vtxDevice, true);
 149                     return true;
 150                 }
 151             }
 152         } else {
 153             if (pitOnOff) {
 154                 vtxCommonSetPitMode(vtxDevice, false);
 155                 return true;
 156             }
 157         }
 158     }
 159
 160     return false;
 161 }
 162
 163 void vtxUpdate(timeUs_t currentTimeUs)
 164 {
 165     static uint8_t currentSchedule = 0;
 166
 167     if (cliMode) {
 168         return;
 169     }
 170
 171     vtxDevice_t *vtxDevice = vtxCommonDevice();
 172     if (vtxDevice) {
 173         // Check input sources for config updates
 174         vtxControlInputPoll();
 175
 176         // Build runtime settings
 177         const vtxSettingsConfig_t * runtimeSettings = vtxGetRuntimeSettings();
 178
 179         switch (currentSchedule) {
 180             case VTX_PARAM_POWER:
 181                 vtxProcessPower(vtxDevice, runtimeSettings);
 182                 break;
 183             case VTX_PARAM_BANDCHAN:
 184                 vtxProcessBandAndChannel(vtxDevice, runtimeSettings);
 185                 break;
 186             case VTX_PARAM_PITMODE:
 187                 vtxProcessPitMode(vtxDevice, runtimeSettings);
 188                 break;
 189             default:
 190                 break;
 191         }
 192
 193         vtxCommonProcess(vtxDevice, currentTimeUs);
 194
 195         currentSchedule = (currentSchedule + 1) % VTX_PARAM_COUNT;
 196     }
 197 }
 198
 199 #endif