Make ppm/pwm input filtering configurable.
This commit is contained in:
parent
226e38de3e
commit
0ac2b51c60
14
docs/Rx.md
14
docs/Rx.md
|
@ -26,4 +26,18 @@ Allows you to use MSP commands as the RC input. Only 8 channel support to maint
|
||||||
12 channels via serial supported currently.
|
12 channels via serial supported currently.
|
||||||
|
|
||||||
|
|
||||||
|
### Configuration
|
||||||
|
|
||||||
|
See the Configuration document some some RX configuration examples.
|
||||||
|
|
||||||
|
#### PPM/PWM input filtering.
|
||||||
|
|
||||||
|
Hardware input filtering can be enabled if you are experiencing interference on the signal sent via your PWM/PPM RX.
|
||||||
|
|
||||||
|
Use the `input_filtering_mode` cli command to select a mode.
|
||||||
|
|
||||||
|
| Value | Meaning |
|
||||||
|
| ----- | --------- |
|
||||||
|
| 0 | Disabled |
|
||||||
|
| 1 | Enabled |
|
||||||
|
|
||||||
|
|
|
@ -27,6 +27,7 @@
|
||||||
|
|
||||||
#include "drivers/accgyro.h"
|
#include "drivers/accgyro.h"
|
||||||
#include "drivers/system.h"
|
#include "drivers/system.h"
|
||||||
|
#include "drivers/pwm_rx.h"
|
||||||
|
|
||||||
#include "sensors/sensors.h"
|
#include "sensors/sensors.h"
|
||||||
#include "sensors/gyro.h"
|
#include "sensors/gyro.h"
|
||||||
|
@ -244,6 +245,8 @@ static void resetConf(void)
|
||||||
masterConfig.rxConfig.maxcheck = 1900;
|
masterConfig.rxConfig.maxcheck = 1900;
|
||||||
masterConfig.rxConfig.rssi_channel = 0;
|
masterConfig.rxConfig.rssi_channel = 0;
|
||||||
|
|
||||||
|
masterConfig.inputFilteringMode = INPUT_FILTERING_DISABLED;
|
||||||
|
|
||||||
masterConfig.retarded_arm = 0; // disable arm/disarm on roll left/right
|
masterConfig.retarded_arm = 0; // disable arm/disarm on roll left/right
|
||||||
masterConfig.airplaneConfig.flaps_speed = 0;
|
masterConfig.airplaneConfig.flaps_speed = 0;
|
||||||
masterConfig.fixedwing_althold_dir = 1;
|
masterConfig.fixedwing_althold_dir = 1;
|
||||||
|
|
|
@ -57,6 +57,7 @@ typedef struct master_t {
|
||||||
batteryConfig_t batteryConfig;
|
batteryConfig_t batteryConfig;
|
||||||
|
|
||||||
rxConfig_t rxConfig;
|
rxConfig_t rxConfig;
|
||||||
|
inputFilteringMode_e inputFilteringMode; // Use hardware input filtering, e.g. for OrangeRX PPM/PWM receivers.
|
||||||
|
|
||||||
uint8_t retarded_arm; // allow disarsm/arm on throttle down + roll left/right
|
uint8_t retarded_arm; // allow disarsm/arm on throttle down + roll left/right
|
||||||
|
|
||||||
|
|
|
@ -38,7 +38,11 @@
|
||||||
#define PWM_PORTS_OR_PPM_CAPTURE_COUNT PWM_INPUT_PORT_COUNT
|
#define PWM_PORTS_OR_PPM_CAPTURE_COUNT PWM_INPUT_PORT_COUNT
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity); // from pwm_output.c
|
#define INPUT_FILTER_TO_HELP_WITH_NOISE_FROM_OPENLRS_TELEMETRY_RX 0x03
|
||||||
|
|
||||||
|
static inputFilteringMode_e inputFilteringMode;
|
||||||
|
|
||||||
|
void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity);
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
INPUT_MODE_PPM,
|
INPUT_MODE_PPM,
|
||||||
|
@ -105,6 +109,11 @@ void resetPPMDataReceivedState(void)
|
||||||
|
|
||||||
#define MIN_CHANNELS_BEFORE_PPM_FRAME_CONSIDERED_VALID 4
|
#define MIN_CHANNELS_BEFORE_PPM_FRAME_CONSIDERED_VALID 4
|
||||||
|
|
||||||
|
void pwmRxInit(inputFilteringMode_e initialInputFilteringMode)
|
||||||
|
{
|
||||||
|
inputFilteringMode = initialInputFilteringMode;
|
||||||
|
}
|
||||||
|
|
||||||
static void ppmInit(void)
|
static void ppmInit(void)
|
||||||
{
|
{
|
||||||
ppmDev.pulseIndex = 0;
|
ppmDev.pulseIndex = 0;
|
||||||
|
@ -232,8 +241,6 @@ static void pwmGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, GPIO_Mode mode)
|
||||||
gpioInit(gpio, &cfg);
|
gpioInit(gpio, &cfg);
|
||||||
}
|
}
|
||||||
|
|
||||||
#define INPUT_FILTER_TO_HELP_WITH_NOISE_FROM_OPENLRS_TELEMETRY_RX 0x03
|
|
||||||
|
|
||||||
void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
|
void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
|
||||||
{
|
{
|
||||||
TIM_ICInitTypeDef TIM_ICInitStructure;
|
TIM_ICInitTypeDef TIM_ICInitStructure;
|
||||||
|
@ -243,7 +250,12 @@ void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
|
||||||
TIM_ICInitStructure.TIM_ICPolarity = polarity;
|
TIM_ICInitStructure.TIM_ICPolarity = polarity;
|
||||||
TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
|
TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
|
||||||
TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
|
TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
|
||||||
TIM_ICInitStructure.TIM_ICFilter = INPUT_FILTER_TO_HELP_WITH_NOISE_FROM_OPENLRS_TELEMETRY_RX;
|
|
||||||
|
if (inputFilteringMode == INPUT_FILTERING_ENABLED) {
|
||||||
|
TIM_ICInitStructure.TIM_ICFilter = INPUT_FILTER_TO_HELP_WITH_NOISE_FROM_OPENLRS_TELEMETRY_RX;
|
||||||
|
} else {
|
||||||
|
TIM_ICInitStructure.TIM_ICFilter = 0x00;
|
||||||
|
}
|
||||||
|
|
||||||
TIM_ICInit(tim, &TIM_ICInitStructure);
|
TIM_ICInit(tim, &TIM_ICInitStructure);
|
||||||
}
|
}
|
||||||
|
|
|
@ -17,6 +17,11 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
typedef enum {
|
||||||
|
INPUT_FILTERING_DISABLED = 0,
|
||||||
|
INPUT_FILTERING_ENABLED
|
||||||
|
} inputFilteringMode_e;
|
||||||
|
|
||||||
void ppmInConfig(uint8_t timerIndex);
|
void ppmInConfig(uint8_t timerIndex);
|
||||||
void pwmInConfig(uint8_t timerIndex, uint8_t channel);
|
void pwmInConfig(uint8_t timerIndex, uint8_t channel);
|
||||||
|
|
||||||
|
@ -24,3 +29,5 @@ uint16_t pwmRead(uint8_t channel);
|
||||||
|
|
||||||
bool isPPMDataBeingReceived(void);
|
bool isPPMDataBeingReceived(void);
|
||||||
void resetPPMDataReceivedState(void);
|
void resetPPMDataReceivedState(void);
|
||||||
|
|
||||||
|
void pwmRxInit(inputFilteringMode_e initialInputFilteringMode);
|
||||||
|
|
|
@ -26,6 +26,7 @@
|
||||||
|
|
||||||
#include "drivers/accgyro.h"
|
#include "drivers/accgyro.h"
|
||||||
#include "drivers/serial.h"
|
#include "drivers/serial.h"
|
||||||
|
#include "drivers/pwm_rx.h"
|
||||||
|
|
||||||
// FIXME remove dependency on currentProfile and masterConfig globals and clean up include file list.
|
// FIXME remove dependency on currentProfile and masterConfig globals and clean up include file list.
|
||||||
|
|
||||||
|
|
|
@ -32,6 +32,7 @@
|
||||||
#include "drivers/accgyro.h"
|
#include "drivers/accgyro.h"
|
||||||
#include "drivers/serial.h"
|
#include "drivers/serial.h"
|
||||||
#include "drivers/bus_i2c.h"
|
#include "drivers/bus_i2c.h"
|
||||||
|
#include "drivers/pwm_rx.h"
|
||||||
#include "flight/flight.h"
|
#include "flight/flight.h"
|
||||||
#include "flight/mixer.h"
|
#include "flight/mixer.h"
|
||||||
#include "flight/failsafe.h"
|
#include "flight/failsafe.h"
|
||||||
|
@ -167,6 +168,7 @@ const clivalue_t valueTable[] = {
|
||||||
{ "min_check", VAR_UINT16, &masterConfig.rxConfig.mincheck, PWM_RANGE_ZERO, PWM_RANGE_MAX },
|
{ "min_check", VAR_UINT16, &masterConfig.rxConfig.mincheck, PWM_RANGE_ZERO, PWM_RANGE_MAX },
|
||||||
{ "max_check", VAR_UINT16, &masterConfig.rxConfig.maxcheck, PWM_RANGE_ZERO, PWM_RANGE_MAX },
|
{ "max_check", VAR_UINT16, &masterConfig.rxConfig.maxcheck, PWM_RANGE_ZERO, PWM_RANGE_MAX },
|
||||||
{ "rssi_channel", VAR_INT8, &masterConfig.rxConfig.rssi_channel, 0, MAX_SUPPORTED_RC_CHANNEL_COUNT },
|
{ "rssi_channel", VAR_INT8, &masterConfig.rxConfig.rssi_channel, 0, MAX_SUPPORTED_RC_CHANNEL_COUNT },
|
||||||
|
{ "input_filtering_mode", VAR_INT8, &masterConfig.inputFilteringMode, 0, 1 },
|
||||||
|
|
||||||
{ "min_throttle", VAR_UINT16, &masterConfig.escAndServoConfig.minthrottle, PWM_RANGE_ZERO, PWM_RANGE_MAX },
|
{ "min_throttle", VAR_UINT16, &masterConfig.escAndServoConfig.minthrottle, PWM_RANGE_ZERO, PWM_RANGE_MAX },
|
||||||
{ "max_throttle", VAR_UINT16, &masterConfig.escAndServoConfig.maxthrottle, PWM_RANGE_ZERO, PWM_RANGE_MAX },
|
{ "max_throttle", VAR_UINT16, &masterConfig.escAndServoConfig.maxthrottle, PWM_RANGE_ZERO, PWM_RANGE_MAX },
|
||||||
|
|
|
@ -31,6 +31,7 @@
|
||||||
#include "drivers/accgyro.h"
|
#include "drivers/accgyro.h"
|
||||||
#include "drivers/serial.h"
|
#include "drivers/serial.h"
|
||||||
#include "drivers/bus_i2c.h"
|
#include "drivers/bus_i2c.h"
|
||||||
|
#include "drivers/pwm_rx.h"
|
||||||
|
|
||||||
#include "flight/flight.h"
|
#include "flight/flight.h"
|
||||||
#include "flight/mixer.h"
|
#include "flight/mixer.h"
|
||||||
|
|
|
@ -35,6 +35,7 @@
|
||||||
#include "drivers/serial_uart.h"
|
#include "drivers/serial_uart.h"
|
||||||
#include "drivers/accgyro.h"
|
#include "drivers/accgyro.h"
|
||||||
#include "drivers/pwm_mapping.h"
|
#include "drivers/pwm_mapping.h"
|
||||||
|
#include "drivers/pwm_rx.h"
|
||||||
#include "drivers/adc.h"
|
#include "drivers/adc.h"
|
||||||
|
|
||||||
#include "flight/flight.h"
|
#include "flight/flight.h"
|
||||||
|
@ -204,6 +205,8 @@ void init(void)
|
||||||
pwm_params.idlePulse = 0; // brushed motors
|
pwm_params.idlePulse = 0; // brushed motors
|
||||||
pwm_params.servoCenterPulse = masterConfig.rxConfig.midrc;
|
pwm_params.servoCenterPulse = masterConfig.rxConfig.midrc;
|
||||||
|
|
||||||
|
pwmRxInit(masterConfig.inputFilteringMode);
|
||||||
|
|
||||||
pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params);
|
pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params);
|
||||||
|
|
||||||
mixerUsePWMOutputConfiguration(pwmOutputConfiguration);
|
mixerUsePWMOutputConfiguration(pwmOutputConfiguration);
|
||||||
|
|
|
@ -32,6 +32,7 @@
|
||||||
#include "drivers/gpio.h"
|
#include "drivers/gpio.h"
|
||||||
#include "drivers/system.h"
|
#include "drivers/system.h"
|
||||||
#include "drivers/serial.h"
|
#include "drivers/serial.h"
|
||||||
|
#include "drivers/pwm_rx.h"
|
||||||
|
|
||||||
#include "flight/flight.h"
|
#include "flight/flight.h"
|
||||||
#include "sensors/sensors.h"
|
#include "sensors/sensors.h"
|
||||||
|
|
Loading…
Reference in New Issue