src/main/drivers/camera_control.c

   1 /*
   2  * This file is part of Betaflight.
   3  *
   4  * Betaflight is free software. You can redistribute this software
   5  * and/or modify this software under the terms of the GNU General
   6  * Public License as published by the Free Software Foundation,
   7  * either version 3 of the License, or (at your option) any later
   8  * version.
   9  *
  10  * Betaflight is distributed in the hope that it will be useful,
  11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  *
  14  * See the GNU General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU General Public
  17  * License along with this software.
  18  *
  19  * If not, see <http://www.gnu.org/licenses/>.
  20  */
  21
  22 #include "platform.h"
  23
  24 #ifdef USE_CAMERA_CONTROL
  25
  26 #ifndef CAMERA_CONTROL_PIN
  27 #define CAMERA_CONTROL_PIN NONE
  28 #endif
  29
  30 #include <math.h>
  31
  32 #include "drivers/camera_control_impl.h"
  33 #include "drivers/io.h"
  34 #include "drivers/pwm_output.h"
  35 #include "drivers/time.h"
  36 #include "pg/pg_ids.h"
  37
  38 #ifdef CURRENT_TARGET_CPU_VOLTAGE
  39 #define ADC_VOLTAGE CURRENT_TARGET_CPU_VOLTAGE
  40 #else
  41 #define ADC_VOLTAGE 3.3f
  42 #endif
  43
  44 #ifdef USE_OSD
  45 #include "osd/osd.h"
  46 #endif
  47
  48 PG_REGISTER_WITH_RESET_FN(cameraControlConfig_t, cameraControlConfig, PG_CAMERA_CONTROL_CONFIG, 0);
  49
  50 void pgResetFn_cameraControlConfig(cameraControlConfig_t *cameraControlConfig)
  51 {
  52     cameraControlConfig->mode = CAMERA_CONTROL_MODE_HARDWARE_PWM;
  53     cameraControlConfig->refVoltage = 330;
  54     cameraControlConfig->keyDelayMs = 180;
  55     cameraControlConfig->internalResistance = 470;
  56     cameraControlConfig->ioTag = IO_TAG(CAMERA_CONTROL_PIN);
  57     cameraControlConfig->inverted = 0;   // Output is inverted externally
  58     cameraControlConfig->buttonResistanceValues[CAMERA_CONTROL_KEY_ENTER] = 450;
  59     cameraControlConfig->buttonResistanceValues[CAMERA_CONTROL_KEY_LEFT]  = 270;
  60     cameraControlConfig->buttonResistanceValues[CAMERA_CONTROL_KEY_UP]    = 150;
  61     cameraControlConfig->buttonResistanceValues[CAMERA_CONTROL_KEY_RIGHT] = 68;
  62     cameraControlConfig->buttonResistanceValues[CAMERA_CONTROL_KEY_DOWN]  = 0;
  63 }
  64
  65 static struct {
  66     bool enabled;
  67     IO_t io;
  68     timerChannel_t channel;
  69     uint32_t period;
  70     uint8_t inverted;
  71 } cameraControlRuntime;
  72
  73 static uint32_t endTimeMillis;
  74
  75 #ifdef CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE
  76 void cameraControlHi(void)
  77 {
  78     if (cameraControlRuntime.inverted) {
  79         IOLo(cameraControlRuntime.io);
  80     } else {
  81         IOHi(cameraControlRuntime.io);
  82     }
  83 }
  84
  85 void cameraControlLo(void)
  86 {
  87     if (cameraControlRuntime.inverted) {
  88         IOHi(cameraControlRuntime.io);
  89     } else {
  90         IOLo(cameraControlRuntime.io);
  91     }
  92 }
  93 #endif
  94
  95 void cameraControlInit(void)
  96 {
  97     if (cameraControlConfig()->ioTag == IO_TAG_NONE)
  98         return;
  99
 100     cameraControlRuntime.inverted = cameraControlConfig()->inverted;
 101     cameraControlRuntime.io = IOGetByTag(cameraControlConfig()->ioTag);
 102     IOInit(cameraControlRuntime.io, OWNER_CAMERA_CONTROL, 0);
 103
 104     if (CAMERA_CONTROL_MODE_HARDWARE_PWM == cameraControlConfig()->mode) {
 105 #ifdef CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE
 106         const timerHardware_t *timerHardware = timerAllocate(cameraControlConfig()->ioTag, OWNER_CAMERA_CONTROL, 0);
 107
 108         if (!timerHardware) {
 109             return;
 110         }
 111
 112         IOConfigGPIOAF(cameraControlRuntime.io, IOCFG_AF_PP, timerHardware->alternateFunction);
 113
 114         cameraControlHardwarePwmInit(&cameraControlRuntime.channel, timerHardware, cameraControlRuntime.inverted);
 115
 116         cameraControlRuntime.period = CAMERA_CONTROL_PWM_RESOLUTION;
 117         *cameraControlRuntime.channel.ccr = cameraControlRuntime.period;
 118         cameraControlRuntime.enabled = true;
 119 #endif
 120     } else if (CAMERA_CONTROL_MODE_SOFTWARE_PWM == cameraControlConfig()->mode) {
 121 #ifdef CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE
 122
 123         IOConfigGPIO(cameraControlRuntime.io, IOCFG_OUT_PP);
 124         cameraControlHi();
 125
 126         cameraControlRuntime.period = CAMERA_CONTROL_SOFT_PWM_RESOLUTION;
 127         cameraControlRuntime.enabled = true;
 128
 129         cameraControlSoftwarePwmInit();
 130 #endif
 131     } else if (CAMERA_CONTROL_MODE_DAC == cameraControlConfig()->mode) {
 132         // @todo not yet implemented
 133     }
 134 }
 135
 136 void cameraControlProcess(uint32_t currentTimeUs)
 137 {
 138     if (endTimeMillis && currentTimeUs >= 1000 * endTimeMillis) {
 139         if (CAMERA_CONTROL_MODE_HARDWARE_PWM == cameraControlConfig()->mode) {
 140             *cameraControlRuntime.channel.ccr = cameraControlRuntime.period;
 141         } else if (CAMERA_CONTROL_MODE_SOFTWARE_PWM == cameraControlConfig()->mode) {
 142
 143         }
 144
 145         endTimeMillis = 0;
 146     }
 147 }
 148
 149 #if defined(CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE) || defined(CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE)
 150
 151 static float calculateKeyPressVoltage(const cameraControlKey_e key)
 152 {
 153     const int buttonResistance = cameraControlConfig()->buttonResistanceValues[key] * 100;
 154     return 1.0e-2f * cameraControlConfig()->refVoltage * buttonResistance / (100 * cameraControlConfig()->internalResistance + buttonResistance);
 155 }
 156
 157 static float calculatePWMDutyCycle(const cameraControlKey_e key)
 158 {
 159     const float voltage = calculateKeyPressVoltage(key);
 160
 161     return voltage / ADC_VOLTAGE;
 162 }
 163 #endif
 164
 165 void cameraControlKeyPress(cameraControlKey_e key, uint32_t holdDurationMs)
 166 {
 167     if (!cameraControlRuntime.enabled)
 168         return;
 169
 170     if (key >= CAMERA_CONTROL_KEYS_COUNT)
 171         return;
 172
 173 #if defined(CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE) || defined(CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE)
 174     const float dutyCycle = calculatePWMDutyCycle(key);
 175 #else
 176     (void) holdDurationMs;
 177 #endif
 178
 179 #ifdef USE_OSD
 180     // Force OSD timeout so we are alone on the display.
 181     resumeRefreshAt = 0;
 182 #endif
 183
 184     if (CAMERA_CONTROL_MODE_HARDWARE_PWM == cameraControlConfig()->mode) {
 185 #ifdef CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE
 186         *cameraControlRuntime.channel.ccr = lrintf(dutyCycle * cameraControlRuntime.period);
 187         endTimeMillis = millis() + cameraControlConfig()->keyDelayMs + holdDurationMs;
 188 #endif
 189     } else if (CAMERA_CONTROL_MODE_SOFTWARE_PWM == cameraControlConfig()->mode) {
 190 #ifdef CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE
 191         const uint32_t hiTime = lrintf(dutyCycle * cameraControlRuntime.period);
 192
 193         if (0 == hiTime) {
 194             cameraControlLo();
 195             delay(cameraControlConfig()->keyDelayMs + holdDurationMs);
 196             cameraControlHi();
 197         } else {
 198             cameraControlSoftwarePwmEnable(hiTime, cameraControlRuntime.period);
 199
 200             const uint32_t endTime = millis() + cameraControlConfig()->keyDelayMs + holdDurationMs;
 201
 202             // Wait to give the camera a chance at registering the key press
 203             while (millis() < endTime);
 204
 205             // Disable timers and interrupt generation
 206             cameraControlSoftwarePwmDisable();
 207
 208             // Reset to idle state
 209             IOHi(cameraControlRuntime.io);
 210         }
 211 #endif
 212     } else if (CAMERA_CONTROL_MODE_DAC == cameraControlConfig()->mode) {
 213         // @todo not yet implemented
 214     }
 215 }
 216
 217 #endif // USE_CAMERA_CONTROL