[4.4.2] Remove 15 m/s limit on estimated vario (#12788)
[betaflight.git] / src / main / drivers / rx / rx_pwm.c
blobfd9932b6959905d1527a1969dfeaf6f3584d30be
1 /*
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)
8 * any later version.
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/>.
21 #include <stdbool.h>
22 #include <stdint.h>
24 #include "platform.h"
26 #if defined(USE_RX_PWM) || defined(USE_RX_PPM)
28 #include "build/build_config.h"
29 #include "build/debug.h"
31 #include "common/utils.h"
33 #include "drivers/io.h"
34 #include "drivers/nvic.h"
35 #include "drivers/pwm_output.h"
36 #include "drivers/timer.h"
38 #include "pg/rx_pwm.h"
40 #include "rx_pwm.h"
42 #define DEBUG_PPM_ISR
44 #define PPM_CAPTURE_COUNT 12
46 #if PPM_CAPTURE_COUNT > PWM_INPUT_PORT_COUNT
47 #define PWM_PORTS_OR_PPM_CAPTURE_COUNT PPM_CAPTURE_COUNT
48 #else
49 #define PWM_PORTS_OR_PPM_CAPTURE_COUNT PWM_INPUT_PORT_COUNT
50 #endif
52 // TODO - change to timer clocks ticks
53 #define INPUT_FILTER_TO_HELP_WITH_NOISE_FROM_OPENLRS_TELEMETRY_RX 0x03
55 static inputFilteringMode_e inputFilteringMode;
57 void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity);
59 typedef enum {
60 INPUT_MODE_PPM,
61 INPUT_MODE_PWM
62 } pwmInputMode_e;
64 typedef struct {
65 pwmInputMode_e mode;
66 uint8_t channel; // only used for pwm, ignored by ppm
68 uint8_t state;
69 uint8_t missedEvents;
71 captureCompare_t rise;
72 captureCompare_t fall;
73 captureCompare_t capture;
75 const timerHardware_t *timerHardware;
76 timerCCHandlerRec_t edgeCb;
77 timerOvrHandlerRec_t overflowCb;
78 } pwmInputPort_t;
80 static pwmInputPort_t pwmInputPorts[PWM_INPUT_PORT_COUNT];
82 static uint16_t captures[PWM_PORTS_OR_PPM_CAPTURE_COUNT];
84 #define PPM_TIMER_PERIOD 0x10000
85 #define PWM_TIMER_PERIOD 0x10000
87 static uint8_t ppmFrameCount = 0;
88 static uint8_t lastPPMFrameCount = 0;
89 static uint8_t ppmCountDivisor = 1;
91 typedef struct ppmDevice_s {
92 //uint32_t previousTime;
93 uint32_t currentCapture;
94 uint32_t currentTime;
95 uint32_t deltaTime;
96 uint32_t captures[PWM_PORTS_OR_PPM_CAPTURE_COUNT];
97 uint32_t largeCounter;
98 uint8_t pulseIndex;
99 int8_t numChannels;
100 int8_t numChannelsPrevFrame;
101 uint8_t stableFramesSeenCount;
103 bool tracking;
104 bool overflowed;
105 } ppmDevice_t;
107 static ppmDevice_t ppmDev;
109 #define PPM_IN_MIN_SYNC_PULSE_US 2700 // microseconds
110 #define PPM_IN_MIN_CHANNEL_PULSE_US 750 // microseconds
111 #define PPM_IN_MAX_CHANNEL_PULSE_US 2250 // microseconds
112 #define PPM_STABLE_FRAMES_REQUIRED_COUNT 25
113 #define PPM_IN_MIN_NUM_CHANNELS 4
114 #define PPM_IN_MAX_NUM_CHANNELS PWM_PORTS_OR_PPM_CAPTURE_COUNT
116 bool isPPMDataBeingReceived(void)
118 return (ppmFrameCount != lastPPMFrameCount);
121 void resetPPMDataReceivedState(void)
123 lastPPMFrameCount = ppmFrameCount;
126 #define MIN_CHANNELS_BEFORE_PPM_FRAME_CONSIDERED_VALID 4
128 #ifdef DEBUG_PPM_ISR
129 typedef enum {
130 SOURCE_OVERFLOW = 0,
131 SOURCE_EDGE = 1
132 } eventSource_e;
134 typedef struct ppmISREvent_s {
135 uint32_t capture;
136 eventSource_e source;
137 } ppmISREvent_t;
139 static ppmISREvent_t ppmEvents[20];
140 static uint8_t ppmEventIndex = 0;
142 void ppmISREvent(eventSource_e source, uint32_t capture)
144 ppmEventIndex = (ppmEventIndex + 1) % ARRAYLEN(ppmEvents);
146 ppmEvents[ppmEventIndex].source = source;
147 ppmEvents[ppmEventIndex].capture = capture;
149 #else
150 void ppmISREvent(eventSource_e source, uint32_t capture) {}
151 #endif
153 static void ppmResetDevice(void)
155 ppmDev.pulseIndex = 0;
156 ppmDev.currentCapture = 0;
157 ppmDev.currentTime = 0;
158 ppmDev.deltaTime = 0;
159 ppmDev.largeCounter = 0;
160 ppmDev.numChannels = -1;
161 ppmDev.numChannelsPrevFrame = -1;
162 ppmDev.stableFramesSeenCount = 0;
163 ppmDev.tracking = false;
164 ppmDev.overflowed = false;
167 static void ppmOverflowCallback(timerOvrHandlerRec_t* cbRec, captureCompare_t capture)
169 UNUSED(cbRec);
170 ppmISREvent(SOURCE_OVERFLOW, capture);
172 ppmDev.largeCounter += capture + 1;
173 if (capture == PPM_TIMER_PERIOD - 1) {
174 ppmDev.overflowed = true;
178 static void ppmEdgeCallback(timerCCHandlerRec_t* cbRec, captureCompare_t capture)
180 UNUSED(cbRec);
181 ppmISREvent(SOURCE_EDGE, capture);
183 int32_t i;
185 uint32_t previousTime = ppmDev.currentTime;
186 uint32_t previousCapture = ppmDev.currentCapture;
188 /* Grab the new count */
189 uint32_t currentTime = capture;
191 /* Convert to 32-bit timer result */
192 currentTime += ppmDev.largeCounter;
194 if (capture < previousCapture) {
195 if (ppmDev.overflowed) {
196 currentTime += PPM_TIMER_PERIOD;
200 // Divide value if Oneshot, Multishot or brushed motors are active and the timer is shared
201 currentTime = currentTime / ppmCountDivisor;
203 /* Capture computation */
204 if (currentTime > previousTime) {
205 ppmDev.deltaTime = currentTime - (previousTime + (ppmDev.overflowed ? (PPM_TIMER_PERIOD / ppmCountDivisor) : 0));
206 } else {
207 ppmDev.deltaTime = (PPM_TIMER_PERIOD / ppmCountDivisor) + currentTime - previousTime;
210 ppmDev.overflowed = false;
213 /* Store the current measurement */
214 ppmDev.currentTime = currentTime;
215 ppmDev.currentCapture = capture;
217 /* Sync pulse detection */
218 if (ppmDev.deltaTime > PPM_IN_MIN_SYNC_PULSE_US) {
219 if (ppmDev.pulseIndex == ppmDev.numChannelsPrevFrame
220 && ppmDev.pulseIndex >= PPM_IN_MIN_NUM_CHANNELS
221 && ppmDev.pulseIndex <= PPM_IN_MAX_NUM_CHANNELS) {
222 /* If we see n simultaneous frames of the same
223 number of channels we save it as our frame size */
224 if (ppmDev.stableFramesSeenCount < PPM_STABLE_FRAMES_REQUIRED_COUNT) {
225 ppmDev.stableFramesSeenCount++;
226 } else {
227 ppmDev.numChannels = ppmDev.pulseIndex;
229 } else {
230 ppmDev.stableFramesSeenCount = 0;
233 /* Check if the last frame was well formed */
234 if (ppmDev.pulseIndex == ppmDev.numChannels && ppmDev.tracking) {
235 /* The last frame was well formed */
236 for (i = 0; i < ppmDev.numChannels; i++) {
237 captures[i] = ppmDev.captures[i];
239 for (i = ppmDev.numChannels; i < PPM_IN_MAX_NUM_CHANNELS; i++) {
240 captures[i] = PPM_RCVR_TIMEOUT;
242 ppmFrameCount++;
245 ppmDev.tracking = true;
246 ppmDev.numChannelsPrevFrame = ppmDev.pulseIndex;
247 ppmDev.pulseIndex = 0;
249 /* We rely on the supervisor to set captureValue to invalid
250 if no valid frame is found otherwise we ride over it */
251 } else if (ppmDev.tracking) {
252 /* Valid pulse duration 0.75 to 2.5 ms*/
253 if (ppmDev.deltaTime > PPM_IN_MIN_CHANNEL_PULSE_US
254 && ppmDev.deltaTime < PPM_IN_MAX_CHANNEL_PULSE_US
255 && ppmDev.pulseIndex < PPM_IN_MAX_NUM_CHANNELS) {
256 ppmDev.captures[ppmDev.pulseIndex] = ppmDev.deltaTime;
257 ppmDev.pulseIndex++;
258 } else {
259 /* Not a valid pulse duration */
260 ppmDev.tracking = false;
261 for (i = 0; i < PWM_PORTS_OR_PPM_CAPTURE_COUNT; i++) {
262 ppmDev.captures[i] = PPM_RCVR_TIMEOUT;
268 #define MAX_MISSED_PWM_EVENTS 10
270 bool isPWMDataBeingReceived(void)
272 int channel;
273 for (channel = 0; channel < PWM_PORTS_OR_PPM_CAPTURE_COUNT; channel++) {
274 if (captures[channel] != PPM_RCVR_TIMEOUT) {
275 return true;
278 return false;
281 static void pwmOverflowCallback(timerOvrHandlerRec_t* cbRec, captureCompare_t capture)
283 UNUSED(capture);
284 pwmInputPort_t *pwmInputPort = container_of(cbRec, pwmInputPort_t, overflowCb);
286 if (++pwmInputPort->missedEvents > MAX_MISSED_PWM_EVENTS) {
287 captures[pwmInputPort->channel] = PPM_RCVR_TIMEOUT;
288 pwmInputPort->missedEvents = 0;
292 static void pwmEdgeCallback(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
294 pwmInputPort_t *pwmInputPort = container_of(cbRec, pwmInputPort_t, edgeCb);
295 const timerHardware_t *timerHardwarePtr = pwmInputPort->timerHardware;
297 if (pwmInputPort->state == 0) {
298 pwmInputPort->rise = capture;
299 pwmInputPort->state = 1;
300 #if defined(USE_HAL_DRIVER)
301 pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPOLARITY_FALLING);
302 #else
303 pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Falling);
304 #endif
305 } else {
306 pwmInputPort->fall = capture;
308 // compute and store capture
309 pwmInputPort->capture = pwmInputPort->fall - pwmInputPort->rise;
310 captures[pwmInputPort->channel] = pwmInputPort->capture;
312 // switch state
313 pwmInputPort->state = 0;
314 #if defined(USE_HAL_DRIVER)
315 pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPOLARITY_RISING);
316 #else
317 pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Rising);
318 #endif
319 pwmInputPort->missedEvents = 0;
323 #ifdef USE_HAL_DRIVER
325 void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
327 TIM_HandleTypeDef* Handle = timerFindTimerHandle(tim);
328 if (Handle == NULL) return;
330 TIM_IC_InitTypeDef TIM_ICInitStructure;
332 TIM_ICInitStructure.ICPolarity = polarity;
333 TIM_ICInitStructure.ICSelection = TIM_ICSELECTION_DIRECTTI;
334 TIM_ICInitStructure.ICPrescaler = TIM_ICPSC_DIV1;
336 if (inputFilteringMode == INPUT_FILTERING_ENABLED) {
337 TIM_ICInitStructure.ICFilter = INPUT_FILTER_TO_HELP_WITH_NOISE_FROM_OPENLRS_TELEMETRY_RX;
338 } else {
339 TIM_ICInitStructure.ICFilter = 0x00;
342 HAL_TIM_IC_ConfigChannel(Handle, &TIM_ICInitStructure, channel);
343 HAL_TIM_IC_Start_IT(Handle,channel);
345 #else
346 void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
348 TIM_ICInitTypeDef TIM_ICInitStructure;
350 TIM_ICStructInit(&TIM_ICInitStructure);
351 TIM_ICInitStructure.TIM_Channel = channel;
352 TIM_ICInitStructure.TIM_ICPolarity = polarity;
353 TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
354 TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
356 if (inputFilteringMode == INPUT_FILTERING_ENABLED) {
357 TIM_ICInitStructure.TIM_ICFilter = INPUT_FILTER_TO_HELP_WITH_NOISE_FROM_OPENLRS_TELEMETRY_RX;
358 } else {
359 TIM_ICInitStructure.TIM_ICFilter = 0x00;
362 TIM_ICInit(tim, &TIM_ICInitStructure);
364 #endif
366 void pwmRxInit(const pwmConfig_t *pwmConfig)
368 inputFilteringMode = pwmConfig->inputFilteringMode;
370 for (int channel = 0; channel < PWM_INPUT_PORT_COUNT; channel++) {
372 pwmInputPort_t *port = &pwmInputPorts[channel];
374 const timerHardware_t *timer = timerAllocate(pwmConfig->ioTags[channel], OWNER_PWMINPUT, RESOURCE_INDEX(channel));
376 if (!timer) {
377 /* TODO: maybe fail here if not enough channels? */
378 continue;
381 port->state = 0;
382 port->missedEvents = 0;
383 port->channel = channel;
384 port->mode = INPUT_MODE_PWM;
385 port->timerHardware = timer;
387 IO_t io = IOGetByTag(pwmConfig->ioTags[channel]);
388 IOInit(io, OWNER_PWMINPUT, RESOURCE_INDEX(channel));
389 IOConfigGPIOAF(io, IOCFG_AF_PP, timer->alternateFunction);
390 timerConfigure(timer, (uint16_t)PWM_TIMER_PERIOD, PWM_TIMER_1MHZ);
391 timerChCCHandlerInit(&port->edgeCb, pwmEdgeCallback);
392 timerChOvrHandlerInit(&port->overflowCb, pwmOverflowCallback);
393 timerChConfigCallbacks(timer, &port->edgeCb, &port->overflowCb);
395 #if defined(USE_HAL_DRIVER)
396 pwmICConfig(timer->tim, timer->channel, TIM_ICPOLARITY_RISING);
397 #else
398 pwmICConfig(timer->tim, timer->channel, TIM_ICPolarity_Rising);
399 #endif
404 #define FIRST_PWM_PORT 0
406 #ifdef USE_PWM_OUTPUT
407 void ppmAvoidPWMTimerClash(TIM_TypeDef *pwmTimer)
409 pwmOutputPort_t *motors = pwmGetMotors();
410 for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS; motorIndex++) {
411 if (!motors[motorIndex].enabled || motors[motorIndex].channel.tim != pwmTimer) {
412 continue;
415 ppmCountDivisor = timerClock(pwmTimer) / (pwmTimer->PSC + 1);
416 return;
419 #endif
421 void ppmRxInit(const ppmConfig_t *ppmConfig)
423 ppmResetDevice();
425 pwmInputPort_t *port = &pwmInputPorts[FIRST_PWM_PORT];
427 const timerHardware_t *timer = timerAllocate(ppmConfig->ioTag, OWNER_PPMINPUT, 0);
428 if (!timer) {
429 /* TODO: fail here? */
430 return;
433 #ifdef USE_PWM_OUTPUT
434 ppmAvoidPWMTimerClash(timer->tim);
435 #endif
437 port->mode = INPUT_MODE_PPM;
438 port->timerHardware = timer;
440 IO_t io = IOGetByTag(ppmConfig->ioTag);
441 IOInit(io, OWNER_PPMINPUT, 0);
442 IOConfigGPIOAF(io, IOCFG_AF_PP, timer->alternateFunction);
444 timerConfigure(timer, (uint16_t)PPM_TIMER_PERIOD, PWM_TIMER_1MHZ);
445 timerChCCHandlerInit(&port->edgeCb, ppmEdgeCallback);
446 timerChOvrHandlerInit(&port->overflowCb, ppmOverflowCallback);
447 timerChConfigCallbacks(timer, &port->edgeCb, &port->overflowCb);
449 #if defined(USE_HAL_DRIVER)
450 pwmICConfig(timer->tim, timer->channel, TIM_ICPOLARITY_RISING);
451 #else
452 pwmICConfig(timer->tim, timer->channel, TIM_ICPolarity_Rising);
453 #endif
456 uint16_t ppmRead(uint8_t channel)
458 return captures[channel];
461 uint16_t pwmRead(uint8_t channel)
463 return captures[channel];
465 #endif