2 * This file is part of Cleanflight.
4 * Cleanflight is free software: you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation, either version 3 of the License, or
7 * (at your option) any later version.
9 * Cleanflight is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
20 #ifdef USE_CAMERA_CONTROL
22 #include "camera_control.h"
26 #include "pwm_output.h"
28 #include "config/parameter_group_ids.h"
30 #if defined(STM32F40_41xxx)
31 #define CAMERA_CONTROL_TIMER_HZ MHZ_TO_HZ(84)
32 #elif defined(STM32F7)
33 #define CAMERA_CONTROL_TIMER_HZ MHZ_TO_HZ(216)
35 #define CAMERA_CONTROL_TIMER_HZ MHZ_TO_HZ(72)
38 #define CAMERA_CONTROL_PWM_RESOLUTION 128
39 #define CAMERA_CONTROL_SOFT_PWM_RESOLUTION 448
41 #ifdef CURRENT_TARGET_CPU_VOLTAGE
42 #define ADC_VOLTAGE CURRENT_TARGET_CPU_VOLTAGE
44 #define ADC_VOLTAGE 3.3f
47 #if !defined(STM32F411xE) && !defined(STM32F7)
48 #define CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE
49 #include "build/atomic.h"
52 #define CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE
55 #ifndef CAMERA_CONTROL_PIN
56 #define CAMERA_CONTROL_PIN NONE
60 PG_REGISTER_WITH_RESET_TEMPLATE(cameraControlConfig_t
, cameraControlConfig
, PG_CAMERA_CONTROL_CONFIG
, 0);
62 PG_RESET_TEMPLATE(cameraControlConfig_t
, cameraControlConfig
,
63 .mode
= CAMERA_CONTROL_MODE_HARDWARE_PWM
,
66 .internalResistance
= 470,
67 .ioTag
= IO_TAG(CAMERA_CONTROL_PIN
)
73 timerChannel_t channel
;
75 } cameraControlRuntime
;
77 static uint32_t endTimeMillis
;
79 #ifdef CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE
80 void TIM6_DAC_IRQHandler()
82 IOHi(cameraControlRuntime
.io
);
87 void TIM7_IRQHandler()
89 IOLo(cameraControlRuntime
.io
);
95 void cameraControlInit()
97 if (cameraControlConfig()->ioTag
== IO_TAG_NONE
)
100 cameraControlRuntime
.io
= IOGetByTag(cameraControlConfig()->ioTag
);
101 IOInit(cameraControlRuntime
.io
, OWNER_CAMERA_CONTROL
, 0);
103 if (CAMERA_CONTROL_MODE_HARDWARE_PWM
== cameraControlConfig()->mode
) {
104 #ifdef CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE
105 const timerHardware_t
*timerHardware
= timerGetByTag(cameraControlConfig()->ioTag
, TIM_USE_ANY
);
107 if (!timerHardware
) {
111 #ifdef USE_HAL_DRIVER
112 IOConfigGPIOAF(cameraControlRuntime
.io
, IOCFG_AF_PP
, timerHardware
->alternateFunction
);
114 IOConfigGPIO(cameraControlRuntime
.io
, IOCFG_AF_PP
);
117 pwmOutConfig(&cameraControlRuntime
.channel
, timerHardware
, CAMERA_CONTROL_TIMER_HZ
, CAMERA_CONTROL_PWM_RESOLUTION
, 0, 0);
119 cameraControlRuntime
.period
= CAMERA_CONTROL_PWM_RESOLUTION
;
120 *cameraControlRuntime
.channel
.ccr
= cameraControlRuntime
.period
;
121 cameraControlRuntime
.enabled
= true;
123 } else if (CAMERA_CONTROL_MODE_SOFTWARE_PWM
== cameraControlConfig()->mode
) {
124 #ifdef CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE
125 IOConfigGPIO(cameraControlRuntime
.io
, IOCFG_OUT_PP
);
126 IOHi(cameraControlRuntime
.io
);
128 cameraControlRuntime
.period
= CAMERA_CONTROL_SOFT_PWM_RESOLUTION
;
129 cameraControlRuntime
.enabled
= true;
131 NVIC_InitTypeDef nvicTIM6
= {
132 TIM6_DAC_IRQn
, NVIC_PRIORITY_BASE(NVIC_PRIO_TIMER
), NVIC_PRIORITY_SUB(NVIC_PRIO_TIMER
), ENABLE
134 NVIC_Init(&nvicTIM6
);
135 NVIC_InitTypeDef nvicTIM7
= {
136 TIM7_IRQn
, NVIC_PRIORITY_BASE(NVIC_PRIO_TIMER
), NVIC_PRIORITY_SUB(NVIC_PRIO_TIMER
), ENABLE
138 NVIC_Init(&nvicTIM7
);
140 RCC
->APB1ENR
|= RCC_APB1Periph_TIM6
| RCC_APB1Periph_TIM7
;
144 } else if (CAMERA_CONTROL_MODE_DAC
== cameraControlConfig()->mode
) {
145 // @todo not yet implemented
149 void cameraControlProcess(uint32_t currentTimeUs
)
151 if (endTimeMillis
&& currentTimeUs
>= 1000 * endTimeMillis
) {
152 if (CAMERA_CONTROL_MODE_HARDWARE_PWM
== cameraControlConfig()->mode
) {
153 *cameraControlRuntime
.channel
.ccr
= cameraControlRuntime
.period
;
154 } else if (CAMERA_CONTROL_MODE_SOFTWARE_PWM
== cameraControlConfig()->mode
) {
162 static const int buttonResistanceValues
[] = { 45000, 27000, 15000, 6810, 0 };
164 static float calculateKeyPressVoltage(const cameraControlKey_e key
)
166 const int buttonResistance
= buttonResistanceValues
[key
];
167 return 1.0e-2f
* cameraControlConfig()->refVoltage
* buttonResistance
/ (100 * cameraControlConfig()->internalResistance
+ buttonResistance
);
170 #if defined(CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE) || defined(CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE)
171 static float calculatePWMDutyCycle(const cameraControlKey_e key
)
173 const float voltage
= calculateKeyPressVoltage(key
);
175 return voltage
/ ADC_VOLTAGE
;
179 void cameraControlKeyPress(cameraControlKey_e key
, uint32_t holdDurationMs
)
181 if (!cameraControlRuntime
.enabled
)
184 if (key
>= CAMERA_CONTROL_KEYS_COUNT
)
187 #if defined(CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE) || defined(CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE)
188 const float dutyCycle
= calculatePWMDutyCycle(key
);
190 (void) holdDurationMs
;
193 if (CAMERA_CONTROL_MODE_HARDWARE_PWM
== cameraControlConfig()->mode
) {
194 #ifdef CAMERA_CONTROL_HARDWARE_PWM_AVAILABLE
195 *cameraControlRuntime
.channel
.ccr
= lrintf(dutyCycle
* cameraControlRuntime
.period
);
196 endTimeMillis
= millis() + cameraControlConfig()->keyDelayMs
+ holdDurationMs
;
198 } else if (CAMERA_CONTROL_MODE_SOFTWARE_PWM
== cameraControlConfig()->mode
) {
199 #ifdef CAMERA_CONTROL_SOFTWARE_PWM_AVAILABLE
200 const uint32_t hiTime
= lrintf(dutyCycle
* cameraControlRuntime
.period
);
203 IOLo(cameraControlRuntime
.io
);
204 delay(cameraControlConfig()->keyDelayMs
+ holdDurationMs
);
205 IOHi(cameraControlRuntime
.io
);
208 TIM6
->ARR
= cameraControlRuntime
.period
;
211 TIM7
->ARR
= cameraControlRuntime
.period
;
213 // Start two timers as simultaneously as possible
214 ATOMIC_BLOCK(NVIC_PRIO_TIMER
) {
215 TIM6
->CR1
= TIM_CR1_CEN
;
216 TIM7
->CR1
= TIM_CR1_CEN
;
219 // Enable interrupt generation
220 TIM6
->DIER
= TIM_IT_Update
;
221 TIM7
->DIER
= TIM_IT_Update
;
223 const uint32_t endTime
= millis() + cameraControlConfig()->keyDelayMs
+ holdDurationMs
;
225 // Wait to give the camera a chance at registering the key press
226 while (millis() < endTime
);
228 // Disable timers and interrupt generation
229 TIM6
->CR1
&= ~TIM_CR1_CEN
;
230 TIM7
->CR1
&= ~TIM_CR1_CEN
;
234 // Reset to idle state
235 IOHi(cameraControlRuntime
.io
);
238 } else if (CAMERA_CONTROL_MODE_DAC
== cameraControlConfig()->mode
) {
239 // @todo not yet implemented