forked from cleanflight/cleanflight
-
Notifications
You must be signed in to change notification settings - Fork 0
/
drv_pwm_fy90q.c
344 lines (288 loc) · 11.1 KB
/
drv_pwm_fy90q.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
#ifdef FY90Q
#include "board.h"
#define PULSE_1MS (1000) // 1ms pulse width
// #define PULSE_PERIOD (2500) // pulse period (400Hz)
// #define PULSE_PERIOD_SERVO_DIGITAL (5000) // pulse period for digital servo (200Hz)
// #define PULSE_PERIOD_SERVO_ANALOG (20000) // pulse period for analog servo (50Hz)
// Forward declaration
static void pwmIRQHandler(TIM_TypeDef *tim);
static void ppmIRQHandler(TIM_TypeDef *tim);
// external vars (ugh)
extern int16_t failsafeCnt;
// local vars
static struct TIM_Channel {
TIM_TypeDef *tim;
uint16_t channel;
uint16_t cc;
} Channels[] = {
{ TIM2, TIM_Channel_1, TIM_IT_CC1 },
{ TIM2, TIM_Channel_2, TIM_IT_CC2 },
{ TIM2, TIM_Channel_3, TIM_IT_CC3 },
{ TIM2, TIM_Channel_4, TIM_IT_CC4 },
{ TIM3, TIM_Channel_1, TIM_IT_CC1 },
{ TIM3, TIM_Channel_2, TIM_IT_CC2 },
{ TIM3, TIM_Channel_3, TIM_IT_CC3 },
{ TIM3, TIM_Channel_4, TIM_IT_CC4 },
};
static volatile uint16_t *OutputChannels[] = {
&(TIM4->CCR1),
&(TIM4->CCR2),
&(TIM4->CCR3),
&(TIM4->CCR4),
// Extended use during CPPM input (TODO)
&(TIM3->CCR1),
&(TIM3->CCR2),
&(TIM3->CCR3),
&(TIM3->CCR4),
};
static struct PWM_State {
uint8_t state;
uint16_t rise;
uint16_t fall;
uint16_t capture;
} Inputs[8] = { { 0, } };
static TIM_ICInitTypeDef TIM_ICInitStructure = { 0, };
static bool usePPMFlag = false;
static uint8_t numOutputChannels = 0;
void TIM2_IRQHandler(void)
{
if (usePPMFlag)
ppmIRQHandler(TIM2);
else
pwmIRQHandler(TIM2);
}
static void ppmIRQHandler(TIM_TypeDef *tim)
{
uint16_t diff;
static uint16_t now;
static uint16_t last = 0;
static uint8_t chan = 0;
static uint8_t GoodPulses;
if (TIM_GetITStatus(tim, TIM_IT_CC1) == SET) {
last = now;
now = TIM_GetCapture1(tim);
rcActive = true;
}
TIM_ClearITPendingBit(tim, TIM_IT_CC1);
if (now > last) {
diff = (now - last);
} else {
diff = ((0xFFFF - last) + now);
}
if (diff > 4000) {
chan = 0;
} else {
if (diff > 750 && diff < 2250 && chan < 8) { // 750 to 2250 ms is our 'valid' channel range
Inputs[chan].capture = diff;
if (chan < 4 && diff > FAILSAFE_DETECT_TRESHOLD)
GoodPulses |= (1 << chan); // if signal is valid - mark channel as OK
if (GoodPulses == 0x0F) { // If first four chanells have good pulses, clear FailSafe counter
GoodPulses = 0;
if (failsafeCnt > 20)
failsafeCnt -= 20;
else
failsafeCnt = 0;
}
}
chan++;
failsafeCnt = 0;
}
}
static void pwmIRQHandler(TIM_TypeDef *tim)
{
uint8_t i;
uint16_t val = 0;
for (i = 0; i < 8; i++) {
struct TIM_Channel channel = Channels[i];
struct PWM_State *state = &Inputs[i];
if (channel.tim == tim && (TIM_GetITStatus(tim, channel.cc) == SET)) {
TIM_ClearITPendingBit(channel.tim, channel.cc);
switch (channel.channel) {
case TIM_Channel_1:
val = TIM_GetCapture1(channel.tim);
break;
case TIM_Channel_2:
val = TIM_GetCapture2(channel.tim);
break;
case TIM_Channel_3:
val = TIM_GetCapture3(channel.tim);
break;
case TIM_Channel_4:
val = TIM_GetCapture4(channel.tim);
break;
}
if (state->state == 0)
state->rise = val;
else
state->fall = val;
if (state->state == 0) {
// switch states
state->state = 1;
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling;
TIM_ICInitStructure.TIM_Channel = channel.channel;
TIM_ICInit(channel.tim, &TIM_ICInitStructure);
} else {
// compute capture
if (state->fall > state->rise)
state->capture = (state->fall - state->rise);
else
state->capture = ((0xffff - state->rise) + state->fall);
// switch state
state->state = 0;
// ping failsafe
failsafeCnt = 0;
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
TIM_ICInitStructure.TIM_Channel = channel.channel;
TIM_ICInit(channel.tim, &TIM_ICInitStructure);
}
}
}
}
static void pwmInitializeInput(bool usePPM)
{
GPIO_InitTypeDef GPIO_InitStructure = { 0, };
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = { 0, };
NVIC_InitTypeDef NVIC_InitStructure = { 0, };
uint8_t i;
// Input pins
if (usePPM) {
// Configure TIM2_CH1 for PPM input
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
// Input timer on TIM2 only for PPM
NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
// TIM2 timebase
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
TIM_TimeBaseStructure.TIM_Prescaler = (72 - 1);
TIM_TimeBaseStructure.TIM_Period = 0xffff;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
// Input capture on TIM2_CH1 for PPM
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
TIM_ICInitStructure.TIM_ICFilter = 0x0;
TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;
TIM_ICInit(TIM2, &TIM_ICInitStructure);
// TIM2_CH1 capture compare interrupt enable
TIM_ITConfig(TIM2, TIM_IT_CC1, ENABLE);
TIM_Cmd(TIM2, ENABLE);
// configure number of PWM outputs, in PPM mode, we use bottom 4 channels more more motors
numOutputChannels = 10;
} else {
// Configure TIM2 all 4 channels
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3 | GPIO_Pin_4;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
// TODO Configure EXTI4 1 channel
// Input timers on TIM2 for PWM
NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
// TIM2 timebase
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
TIM_TimeBaseStructure.TIM_Prescaler = (72 - 1);
TIM_TimeBaseStructure.TIM_Period = 0xffff;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
// PWM Input capture
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
TIM_ICInitStructure.TIM_ICFilter = 0x0;
for (i = 0; i < 4; i++) {
TIM_ICInitStructure.TIM_Channel = Channels[i].channel;
TIM_ICInit(Channels[i].tim, &TIM_ICInitStructure);
}
// TODO EXTI4
TIM_ITConfig(TIM2, TIM_IT_CC1 | TIM_IT_CC2 | TIM_IT_CC3 | TIM_IT_CC4, ENABLE);
// TODO EXTI4
TIM_Cmd(TIM2, ENABLE);
// In PWM input mode, all 4 channels are wasted
numOutputChannels = 4;
}
}
bool pwmInit(drv_pwm_config_t *init)
{
GPIO_InitTypeDef GPIO_InitStructure = { 0, };
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = { 0, };
TIM_OCInitTypeDef TIM_OCInitStructure = { 0, };
uint8_t i;
// Inputs
// RX1 TIM2_CH1 PA0 [also PPM] [also used for throttle calibration]
// RX2 TIM2_CH2 PA1
// RX3 TIM2_CH3 PA2 [also UART2_TX]
// RX4 TIM2_CH4 PA3 [also UART2_RX]
// RX5 TIM3_CH1 PA6 [also ADC_IN6]
// RX6 TIM3_CH2 PA7 [also ADC_IN7]
// RX7 TIM3_CH3 PB0 [also ADC_IN8]
// RX8 TIM3_CH4 PB1 [also ADC_IN9]
// Outputs
// PWM1 TIM1_CH1 PA8
// PWM2 TIM1_CH4 PA11
// PWM3 TIM4_CH1 PB6 [also I2C1_SCL]
// PWM4 TIM4_CH2 PB7 [also I2C1_SDA]
// PWM5 TIM4_CH3 PB8
// PWM6 TIM4_CH4 PB9
// use PPM or PWM input
usePPMFlag = init->usePPM;
// preset channels to center
for (i = 0; i < 8; i++)
Inputs[i].capture = 1500;
// Timers run at 1mhz.
// TODO: clean this shit up. Make it all dynamic etc.
if (init->enableInput)
pwmInitializeInput(usePPMFlag);
// Output pins (4x)
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7 | GPIO_Pin_8 | GPIO_Pin_9;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_Init(GPIOB, &GPIO_InitStructure);
// Output timer
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
TIM_TimeBaseStructure.TIM_Prescaler = (72 - 1);
TIM_TimeBaseStructure.TIM_Period = (1000000 / init->motorPwmRate) - 1;
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable;
TIM_OCInitStructure.TIM_Pulse = PULSE_1MS;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low;
TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
// PWM1,2,3,4
TIM_OC1Init(TIM4, &TIM_OCInitStructure);
TIM_OC2Init(TIM4, &TIM_OCInitStructure);
TIM_OC3Init(TIM4, &TIM_OCInitStructure);
TIM_OC4Init(TIM4, &TIM_OCInitStructure);
TIM_OC1PreloadConfig(TIM4, TIM_OCPreload_Enable);
TIM_OC2PreloadConfig(TIM4, TIM_OCPreload_Enable);
TIM_OC3PreloadConfig(TIM4, TIM_OCPreload_Enable);
TIM_OC4PreloadConfig(TIM4, TIM_OCPreload_Enable);
TIM_Cmd(TIM4, ENABLE);
TIM_CtrlPWMOutputs(TIM4, ENABLE);
TIM_OC1PreloadConfig (TIM4, TIM_OCPreload_Enable);
// turn on more motor outputs if we're using ppm / not using pwm input
if (!init->enableInput || init->usePPM) {
// TODO
}
return false;
}
void pwmWrite(uint8_t channel, uint16_t value)
{
if (channel < numOutputChannels)
*OutputChannels[channel] = value;
}
uint16_t pwmRead(uint8_t channel)
{
return Inputs[channel].capture;
}
#endif