2 * This file is part of Cleanflight and Betaflight.
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)
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.
15 * You should have received a copy of the GNU General Public License
16 * along with this software.
18 * If not, see <http://www.gnu.org/licenses/>.
30 #include "drivers/io.h"
31 #include "drivers/motor.h"
32 #include "drivers/pwm_output.h"
33 #include "drivers/time.h"
34 #include "drivers/timer.h"
38 FAST_RAM_ZERO_INIT pwmOutputPort_t motors
[MAX_SUPPORTED_MOTORS
];
40 static void pwmOCConfig(TIM_TypeDef
*tim
, uint8_t channel
, uint16_t value
, uint8_t output
)
42 #if defined(USE_HAL_DRIVER)
43 TIM_HandleTypeDef
* Handle
= timerFindTimerHandle(tim
);
44 if (Handle
== NULL
) return;
46 TIM_OC_InitTypeDef TIM_OCInitStructure
;
48 TIM_OCInitStructure
.OCMode
= TIM_OCMODE_PWM1
;
49 TIM_OCInitStructure
.OCIdleState
= TIM_OCIDLESTATE_SET
;
50 TIM_OCInitStructure
.OCPolarity
= (output
& TIMER_OUTPUT_INVERTED
) ? TIM_OCPOLARITY_LOW
: TIM_OCPOLARITY_HIGH
;
51 TIM_OCInitStructure
.OCNIdleState
= TIM_OCNIDLESTATE_SET
;
52 TIM_OCInitStructure
.OCNPolarity
= (output
& TIMER_OUTPUT_INVERTED
) ? TIM_OCNPOLARITY_LOW
: TIM_OCNPOLARITY_HIGH
;
53 TIM_OCInitStructure
.Pulse
= value
;
54 TIM_OCInitStructure
.OCFastMode
= TIM_OCFAST_DISABLE
;
56 HAL_TIM_PWM_ConfigChannel(Handle
, &TIM_OCInitStructure
, channel
);
58 TIM_OCInitTypeDef TIM_OCInitStructure
;
60 TIM_OCStructInit(&TIM_OCInitStructure
);
61 TIM_OCInitStructure
.TIM_OCMode
= TIM_OCMode_PWM1
;
63 if (output
& TIMER_OUTPUT_N_CHANNEL
) {
64 TIM_OCInitStructure
.TIM_OutputNState
= TIM_OutputNState_Enable
;
65 TIM_OCInitStructure
.TIM_OCNIdleState
= TIM_OCNIdleState_Reset
;
66 TIM_OCInitStructure
.TIM_OCNPolarity
= (output
& TIMER_OUTPUT_INVERTED
) ? TIM_OCNPolarity_Low
: TIM_OCNPolarity_High
;
68 TIM_OCInitStructure
.TIM_OutputState
= TIM_OutputState_Enable
;
69 TIM_OCInitStructure
.TIM_OCIdleState
= TIM_OCIdleState_Set
;
70 TIM_OCInitStructure
.TIM_OCPolarity
= (output
& TIMER_OUTPUT_INVERTED
) ? TIM_OCPolarity_Low
: TIM_OCPolarity_High
;
72 TIM_OCInitStructure
.TIM_Pulse
= value
;
74 timerOCInit(tim
, channel
, &TIM_OCInitStructure
);
75 timerOCPreloadConfig(tim
, channel
, TIM_OCPreload_Enable
);
79 void pwmOutConfig(timerChannel_t
*channel
, const timerHardware_t
*timerHardware
, uint32_t hz
, uint16_t period
, uint16_t value
, uint8_t inversion
)
81 #if defined(USE_HAL_DRIVER)
82 TIM_HandleTypeDef
* Handle
= timerFindTimerHandle(timerHardware
->tim
);
83 if (Handle
== NULL
) return;
86 configTimeBase(timerHardware
->tim
, period
, hz
);
87 pwmOCConfig(timerHardware
->tim
,
88 timerHardware
->channel
,
90 inversion
? timerHardware
->output
^ TIMER_OUTPUT_INVERTED
: timerHardware
->output
93 #if defined(USE_HAL_DRIVER)
94 if (timerHardware
->output
& TIMER_OUTPUT_N_CHANNEL
)
95 HAL_TIMEx_PWMN_Start(Handle
, timerHardware
->channel
);
97 HAL_TIM_PWM_Start(Handle
, timerHardware
->channel
);
98 HAL_TIM_Base_Start(Handle
);
100 TIM_CtrlPWMOutputs(timerHardware
->tim
, ENABLE
);
101 TIM_Cmd(timerHardware
->tim
, ENABLE
);
104 channel
->ccr
= timerChCCR(timerHardware
);
106 channel
->tim
= timerHardware
->tim
;
111 static FAST_RAM_ZERO_INIT motorDevice_t motorPwmDevice
;
113 static void pwmWriteUnused(uint8_t index
, float value
)
119 static void pwmWriteStandard(uint8_t index
, float value
)
121 /* TODO: move value to be a number between 0-1 (i.e. percent throttle from mixer) */
122 *motors
[index
].channel
.ccr
= lrintf((value
* motors
[index
].pulseScale
) + motors
[index
].pulseOffset
);
125 void pwmShutdownPulsesForAllMotors(void)
127 for (int index
= 0; index
< motorPwmDevice
.count
; index
++) {
128 // Set the compare register to 0, which stops the output pulsing if the timer overflows
129 if (motors
[index
].channel
.ccr
) {
130 *motors
[index
].channel
.ccr
= 0;
135 void pwmDisableMotors(void)
137 pwmShutdownPulsesForAllMotors();
140 static motorVTable_t motorPwmVTable
;
141 bool pwmEnableMotors(void)
143 /* check motors can be enabled */
144 return (motorPwmVTable
.write
!= &pwmWriteUnused
);
147 bool pwmIsMotorEnabled(uint8_t index
)
149 return motors
[index
].enabled
;
152 static void pwmCompleteOneshotMotorUpdate(void)
154 for (int index
= 0; index
< motorPwmDevice
.count
; index
++) {
155 if (motors
[index
].forceOverflow
) {
156 timerForceOverflow(motors
[index
].channel
.tim
);
158 // Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again.
159 // This compare register will be set to the output value on the next main loop.
160 *motors
[index
].channel
.ccr
= 0;
164 static float pwmConvertFromExternal(uint16_t externalValue
)
166 return (float)externalValue
;
169 static uint16_t pwmConvertToExternal(float motorValue
)
171 return (uint16_t)motorValue
;
174 static motorVTable_t motorPwmVTable
= {
175 .postInit
= motorPostInitNull
,
176 .enable
= pwmEnableMotors
,
177 .disable
= pwmDisableMotors
,
178 .isMotorEnabled
= pwmIsMotorEnabled
,
179 .shutdown
= pwmShutdownPulsesForAllMotors
,
180 .convertExternalToMotor
= pwmConvertFromExternal
,
181 .convertMotorToExternal
= pwmConvertToExternal
,
184 motorDevice_t
*motorPwmDevInit(const motorDevConfig_t
*motorConfig
, uint16_t idlePulse
, uint8_t motorCount
, bool useUnsyncedPwm
)
186 motorPwmDevice
.vTable
= motorPwmVTable
;
190 switch (motorConfig
->motorPwmProtocol
) {
192 case PWM_TYPE_ONESHOT125
:
196 case PWM_TYPE_ONESHOT42
:
200 case PWM_TYPE_MULTISHOT
:
204 case PWM_TYPE_BRUSHED
:
206 useUnsyncedPwm
= true;
209 case PWM_TYPE_STANDARD
:
212 useUnsyncedPwm
= true;
217 motorPwmDevice
.vTable
.write
= pwmWriteStandard
;
218 motorPwmDevice
.vTable
.updateStart
= motorUpdateStartNull
;
219 motorPwmDevice
.vTable
.updateComplete
= useUnsyncedPwm
? motorUpdateCompleteNull
: pwmCompleteOneshotMotorUpdate
;
221 for (int motorIndex
= 0; motorIndex
< MAX_SUPPORTED_MOTORS
&& motorIndex
< motorCount
; motorIndex
++) {
222 const ioTag_t tag
= motorConfig
->ioTags
[motorIndex
];
223 const timerHardware_t
*timerHardware
= timerAllocate(tag
, OWNER_MOTOR
, RESOURCE_INDEX(motorIndex
));
225 if (timerHardware
== NULL
) {
226 /* not enough motors initialised for the mixer or a break in the motors */
227 motorPwmDevice
.vTable
.write
= &pwmWriteUnused
;
228 motorPwmDevice
.vTable
.updateComplete
= motorUpdateCompleteNull
;
229 /* TODO: block arming and add reason system cannot arm */
233 motors
[motorIndex
].io
= IOGetByTag(tag
);
234 IOInit(motors
[motorIndex
].io
, OWNER_MOTOR
, RESOURCE_INDEX(motorIndex
));
237 IOConfigGPIO(motors
[motorIndex
].io
, IOCFG_AF_PP
);
239 IOConfigGPIOAF(motors
[motorIndex
].io
, IOCFG_AF_PP
, timerHardware
->alternateFunction
);
242 /* standard PWM outputs */
243 // margin of safety is 4 periods when unsynced
244 const unsigned pwmRateHz
= useUnsyncedPwm
? motorConfig
->motorPwmRate
: ceilf(1 / ((sMin
+ sLen
) * 4));
246 const uint32_t clock
= timerClock(timerHardware
->tim
);
247 /* used to find the desired timer frequency for max resolution */
248 const unsigned prescaler
= ((clock
/ pwmRateHz
) + 0xffff) / 0x10000; /* rounding up */
249 const uint32_t hz
= clock
/ prescaler
;
250 const unsigned period
= useUnsyncedPwm
? hz
/ pwmRateHz
: 0xffff;
253 if brushed then it is the entire length of the period.
254 TODO: this can be moved back to periodMin and periodLen
255 once mixer outputs a 0..1 float value.
257 motors
[motorIndex
].pulseScale
= ((motorConfig
->motorPwmProtocol
== PWM_TYPE_BRUSHED
) ? period
: (sLen
* hz
)) / 1000.0f
;
258 motors
[motorIndex
].pulseOffset
= (sMin
* hz
) - (motors
[motorIndex
].pulseScale
* 1000);
260 pwmOutConfig(&motors
[motorIndex
].channel
, timerHardware
, hz
, period
, idlePulse
, motorConfig
->motorPwmInversion
);
262 bool timerAlreadyUsed
= false;
263 for (int i
= 0; i
< motorIndex
; i
++) {
264 if (motors
[i
].channel
.tim
== motors
[motorIndex
].channel
.tim
) {
265 timerAlreadyUsed
= true;
269 motors
[motorIndex
].forceOverflow
= !timerAlreadyUsed
;
270 motors
[motorIndex
].enabled
= true;
273 return &motorPwmDevice
;
276 pwmOutputPort_t
*pwmGetMotors(void)
282 static pwmOutputPort_t servos
[MAX_SUPPORTED_SERVOS
];
284 void pwmWriteServo(uint8_t index
, float value
)
286 if (index
< MAX_SUPPORTED_SERVOS
&& servos
[index
].channel
.ccr
) {
287 *servos
[index
].channel
.ccr
= lrintf(value
);
291 void servoDevInit(const servoDevConfig_t
*servoConfig
)
293 for (uint8_t servoIndex
= 0; servoIndex
< MAX_SUPPORTED_SERVOS
; servoIndex
++) {
294 const ioTag_t tag
= servoConfig
->ioTags
[servoIndex
];
300 servos
[servoIndex
].io
= IOGetByTag(tag
);
302 IOInit(servos
[servoIndex
].io
, OWNER_SERVO
, RESOURCE_INDEX(servoIndex
));
304 const timerHardware_t
*timer
= timerAllocate(tag
, OWNER_SERVO
, RESOURCE_INDEX(servoIndex
));
307 /* flag failure and disable ability to arm */
312 IOConfigGPIO(servos
[servoIndex
].io
, IOCFG_AF_PP
);
314 IOConfigGPIOAF(servos
[servoIndex
].io
, IOCFG_AF_PP
, timer
->alternateFunction
);
317 pwmOutConfig(&servos
[servoIndex
].channel
, timer
, PWM_TIMER_1MHZ
, PWM_TIMER_1MHZ
/ servoConfig
->servoPwmRate
, servoConfig
->servoCenterPulse
, 0);
318 servos
[servoIndex
].enabled
= true;
322 #endif // USE_PWM_OUTPUT