From 8542b02bdec1ca3e35ed8617b8758f02d44a0584 Mon Sep 17 00:00:00 2001 From: bkleiner Date: Sun, 6 Aug 2023 19:17:24 +0200 Subject: [PATCH] rx: automatically detect filter hz --- src/config/config.h | 2 - src/flight/control.c | 2 + src/flight/control.h | 1 + src/flight/pid.c | 14 ++--- src/rx/express_lrs.c | 31 ---------- src/rx/rx.c | 121 ++++++++++++---------------------------- src/rx/rx.h | 1 - src/rx/rx_spi.h | 2 - src/rx/unified_crsf.c | 31 ---------- src/rx/unified_dsm.c | 15 +---- src/rx/unified_serial.h | 2 - 11 files changed, 46 insertions(+), 176 deletions(-) diff --git a/src/config/config.h b/src/config/config.h index adca34dce..f7956a0be 100644 --- a/src/config/config.h +++ b/src/config/config.h @@ -74,8 +74,6 @@ // ************* expo for throttle with the zero crossing at THROTTLE_MID #define THROTTLE_EXPO 0.0f -#define RX_SMOOTHING - //********************************************************************************************************************** //***********************************************RECEIVER SETTINGS****************************************************** diff --git a/src/flight/control.c b/src/flight/control.c index 7e200f68e..52c2698fc 100644 --- a/src/flight/control.c +++ b/src/flight/control.c @@ -67,6 +67,8 @@ FAST_RAM control_state_t state = { .stick_calibration_wizard = STICK_WIZARD_INACTIVE, + .rx_filter_hz = 0.0f, + .rx_rssi = 0, .rx_status = 0, diff --git a/src/flight/control.h b/src/flight/control.h index 22ce8937a..6e7f3aa97 100644 --- a/src/flight/control.h +++ b/src/flight/control.h @@ -70,6 +70,7 @@ typedef struct { vec4_t rx; // holds the raw or calibrated main four channels, roll, pitch, yaw, throttle vec4_t rx_filtered; // same as above, but with constraints (just in case), smoothing and deadband applied vec4_t rx_override; // override values, activated by controls_override + float rx_filter_hz; stick_wizard_state_t stick_calibration_wizard; // current phase of the calibration wizard diff --git a/src/flight/pid.c b/src/flight/pid.c index cc50dbd2c..88ee3806b 100644 --- a/src/flight/pid.c +++ b/src/flight/pid.c @@ -9,7 +9,6 @@ #include "flight/control.h" #include "flight/filter.h" #include "io/led.h" -#include "rx/rx.h" #include "util/util.h" #define PID_SIZE 3 @@ -73,7 +72,7 @@ static filter_lp_pt1 rx_filter; static filter_state_t rx_filter_state[3]; void pid_init() { - filter_lp_pt1_init(&rx_filter, rx_filter_state, 3, rx_smoothing_hz()); + filter_lp_pt1_init(&rx_filter, rx_filter_state, 3, state.rx_filter_hz); for (uint8_t i = 0; i < FILTER_MAX_SLOTS; i++) { filter_init(profile.filter.dterm[i].type, &filter[i], filter_state[i], 3, profile.filter.dterm[i].cutoff_freq); @@ -91,7 +90,7 @@ void pid_init() { void pid_precalc() { timefactor = 0.0032f / state.looptime; - filter_lp_pt1_coeff(&rx_filter, rx_smoothing_hz()); + filter_lp_pt1_coeff(&rx_filter, state.rx_filter_hz); filter_coeff(profile.filter.dterm[0].type, &filter[0], profile.filter.dterm[0].cutoff_freq); filter_coeff(profile.filter.dterm[1].type, &filter[1], profile.filter.dterm[1].cutoff_freq); @@ -217,11 +216,10 @@ static inline void pid(uint8_t x) { transition_setpoint_weight = (fabsf(state.rx_filtered.axis[x]) * (stick_transition / stick_accelerator)) + (1 - stick_transition); } -#ifdef RX_SMOOTHING - const float setpoint_derivative = filter_lp_pt1_step(&rx_filter, &rx_filter_state[x], (state.setpoint.axis[x] - lastsetpoint[x]) * current_kd[x] * timefactor); -#else - const float setpoint_derivative = (state.setpoint.axis[x] - lastsetpoint[x]) * current_kd[x] * timefactor; -#endif + float setpoint_derivative = (state.setpoint.axis[x] - lastsetpoint[x]) * current_kd[x] * timefactor; + if (state.rx_filter_hz > 0.0f) { + setpoint_derivative = filter_lp_pt1_step(&rx_filter, &rx_filter_state[x], setpoint_derivative); + } const float gyro_derivative = (state.gyro.axis[x] - lastrate[x]) * current_kd[x] * timefactor * tda_compensation; const float dterm = (setpoint_derivative * stick_accelerator * transition_setpoint_weight) - (gyro_derivative); diff --git a/src/rx/express_lrs.c b/src/rx/express_lrs.c index 1e12b6726..379716292 100644 --- a/src/rx/express_lrs.c +++ b/src/rx/express_lrs.c @@ -799,37 +799,6 @@ void rx_expresslrs_init() { in_binding_mode = false; } -uint16_t rx_expresslrs_smoothing_cutoff() { - switch (current_air_rate_config()->rate) { - case RATE_FLRC_1000HZ: - return 450; - case RATE_FLRC_500HZ: - case RATE_DVDA_500HZ: - case RATE_LORA_500HZ: - return 225; - case RATE_LORA_333HZ_8CH: - return 150; - case RATE_DVDA_250HZ: - case RATE_LORA_250HZ: - return 112; - case RATE_LORA_200HZ: - return 90; - case RATE_LORA_150HZ: - return 67; - case RATE_LORA_100HZ_8CH: - case RATE_LORA_100HZ: - return 45; - case RATE_LORA_50HZ: - return 22; - case RATE_LORA_25HZ: - return 11; - case RATE_LORA_4HZ: - return 1; - } - - return 1; -} - bool rx_expresslrs_check() { bool channels_received = false; diff --git a/src/rx/rx.c b/src/rx/rx.c index 3bb489278..cb4996d04 100644 --- a/src/rx/rx.c +++ b/src/rx/rx.c @@ -11,6 +11,8 @@ #include "flight/filter.h" #include "util/util.h" +#define RX_FITER_SAMPLE_TIME (5000) + extern profile_t profile; uint8_t failsafe_siglost = 0; @@ -21,72 +23,9 @@ static uint32_t frames_per_second = 0; static uint32_t frames_missed = 0; static uint32_t frames_received = 0; -static filter_lp_pt1 rx_filter; +static filter_lp_pt2 rx_filter; static filter_state_t rx_filter_state[4]; -// Select filter cut, Formula is [(1/rx framerate)/2] * 0.9 -// 0 will trigger selection via rx_smoothing_cutoff -static const uint16_t RX_SMOOTHING_HZ[RX_PROTOCOL_MAX] = { - 0, // RX_PROTOCOL_INVALID, wont happen - 0, // RX_PROTOCOL_UNIFIED_SERIAL, will autodetect following - 25, // RX_PROTOCOL_SBUS, - 67, // RX_PROTOCOL_CRSF, - 50, // RX_PROTOCOL_IBUS, check these - 50, // RX_PROTOCOL_FPORT, check these - 0, // RX_PROTOCOL_DSM, - 90, // RX_PROTOCOL_NRF24_BAYANG_TELEMETRY, - 90, // RX_PROTOCOL_BAYANG_PROTOCOL_BLE_BEACON, - 90, // RX_PROTOCOL_BAYANG_PROTOCOL_TELEMETRY_AUTOBIND, - 50, // RX_PROTOCOL_FRSKY_D8, - 50, // RX_PROTOCOL_FRSKY_D16_FCC, - 50, // RX_PROTOCOL_FRSKY_D16_LBT, - 225, // RX_PROTOCOL_FRSKY_REDPINE, - 0, // RX_PROTOCOL_EXPRESS_LRS - 225, // RX_PROTOCOL_FLYSKY_AFHDS 225 = (666pps * 0.75)/2 * 0.9 Note: mul by 0.75 for expected packet loss - 88 // RX_PROTOCOL_FLYSKY_AFHDS2A 88 = (260pps * 0.75)/2 * 0.9 -}; - -static const uint16_t SERIAL_PROTO_MAP[] = { - RX_PROTOCOL_INVALID, // RX_SERIAL_PROTOCOL_INVALID - RX_PROTOCOL_DSM, // RX_SERIAL_PROTOCOL_DSM - RX_PROTOCOL_SBUS, // RX_SERIAL_PROTOCOL_SBUS - RX_PROTOCOL_IBUS, // RX_SERIAL_PROTOCOL_IBUS - RX_PROTOCOL_FPORT, // RX_SERIAL_PROTOCOL_FPORT - RX_PROTOCOL_CRSF, // RX_SERIAL_PROTOCOL_CRSF - RX_PROTOCOL_REDPINE, // RX_SERIAL_PROTOCOL_REDPINE - // No need to filter differently for inverted. - RX_PROTOCOL_SBUS, // RX_SERIAL_PROTOCOL_SBUS_INVERTED - RX_PROTOCOL_FPORT, // RX_SERIAL_PROTOCOL_FPORT_INVERTED - RX_PROTOCOL_REDPINE, // RX_SERIAL_PROTOCOL_REDPINE_INVERTED -}; - -uint16_t rx_serial_smoothing_cutoff() { - extern rx_serial_protocol_t serial_rx_detected_protcol; - const uint16_t serial_proto = SERIAL_PROTO_MAP[serial_rx_detected_protcol]; - if (serial_proto == RX_PROTOCOL_CRSF) { - return rx_serial_crsf_smoothing_cutoff(); - } - if (serial_proto == RX_PROTOCOL_DSM) { - return rx_serial_dsm_smoothing_cutoff(); - } - return RX_SMOOTHING_HZ[serial_proto]; -} - -float rx_smoothing_hz() { - switch (profile.receiver.protocol) { - case RX_PROTOCOL_UNIFIED_SERIAL: - return rx_serial_smoothing_cutoff(); - -#ifdef USE_RX_SPI_EXPRESS_LRS - case RX_PROTOCOL_EXPRESS_LRS: - return rx_expresslrs_smoothing_cutoff(); -#endif - - default: - return RX_SMOOTHING_HZ[profile.receiver.protocol]; - } -} - uint8_t rx_aux_on(aux_function_t function) { return state.aux[profile.receiver.aux[function]]; } @@ -142,27 +81,25 @@ void rx_lqi_update_direct(float rssi) { } static void rx_apply_smoothing() { - filter_lp_pt1_coeff(&rx_filter, rx_smoothing_hz()); - - for (int i = 0; i < 4; ++i) { - if (i == 3) { - // throttle is 0 - 1.0 - state.rx.axis[i] = constrain(state.rx.axis[i], 0.0, 1.0); - } else { - // other channels are -1.0 - 1.0 - state.rx.axis[i] = constrain(state.rx.axis[i], -1.0, 1.0); - } -#ifdef RX_SMOOTHING - state.rx_filtered.axis[i] = filter_lp_pt1_step(&rx_filter, &rx_filter_state[i], state.rx.axis[i]); -#endif - if (i == 3) { - // throttle is 0 - 1.0 - state.rx_filtered.axis[i] = constrain(state.rx_filtered.axis[i], 0.0, 1.0); - } else { - // other channels are -1.0 - 1.0 - state.rx_filtered.axis[i] = constrain(state.rx_filtered.axis[i], -1.0, 1.0); - } + if (state.rx_filter_hz > 0.0f) { + state.rx_filtered.roll = state.rx.roll = constrain(state.rx.roll, -1.0, 1.0); + state.rx_filtered.pitch = state.rx.pitch = constrain(state.rx.pitch, -1.0, 1.0); + state.rx_filtered.yaw = state.rx.yaw = constrain(state.rx.yaw, -1.0, 1.0); + state.rx_filtered.throttle = state.rx.throttle = constrain(state.rx.throttle, 0.0, 1.0); + return; } + + filter_lp_pt2_coeff(&rx_filter, state.rx_filter_hz); + + state.rx.roll = constrain(state.rx.roll, -1.0, 1.0); + state.rx.pitch = constrain(state.rx.pitch, -1.0, 1.0); + state.rx.yaw = constrain(state.rx.yaw, -1.0, 1.0); + state.rx.throttle = constrain(state.rx.throttle, 0.0, 1.0); + + state.rx_filtered.roll = constrain(filter_lp_pt2_step(&rx_filter, &rx_filter_state[0], state.rx.roll), -1.0, 1.0); + state.rx_filtered.pitch = constrain(filter_lp_pt2_step(&rx_filter, &rx_filter_state[1], state.rx.pitch), -1.0, 1.0); + state.rx_filtered.yaw = constrain(filter_lp_pt2_step(&rx_filter, &rx_filter_state[2], state.rx.yaw), -1.0, 1.0); + state.rx_filtered.throttle = constrain(filter_lp_pt2_step(&rx_filter, &rx_filter_state[3], state.rx.throttle), 0.0, 1.0); } static float rx_apply_deadband(float val) { @@ -192,7 +129,7 @@ static void rx_init_state() { #ifdef GESTURE_AUX_START_ON state.aux[AUX_CHANNEL_GESTURE] = 1; #endif - filter_lp_pt1_init(&rx_filter, rx_filter_state, 4, rx_smoothing_hz()); + filter_lp_pt2_init(&rx_filter, rx_filter_state, 4, state.rx_filter_hz); } void rx_init() { @@ -344,12 +281,26 @@ bool rx_check() { } void rx_update() { + static uint32_t rx_filter_start = 0; + static uint32_t rx_filter_counter = 0; + if (rx_check()) { rx_apply_stick_scale(); state.rx.roll = rx_apply_deadband(state.rx.roll); state.rx.pitch = rx_apply_deadband(state.rx.pitch); state.rx.yaw = rx_apply_deadband(state.rx.yaw); + + rx_filter_counter += 1; + } + + const uint32_t rx_filter_delta = (time_millis() - rx_filter_start); + if (rx_filter_delta > RX_FITER_SAMPLE_TIME) { + const float sample_hz = (float)rx_filter_counter / ((float)rx_filter_delta / (float)RX_FITER_SAMPLE_TIME); + + state.rx_filter_hz = sample_hz * 0.5; + rx_filter_start = time_millis(); + rx_filter_counter = 0; } rx_apply_smoothing(); diff --git a/src/rx/rx.h b/src/rx/rx.h index c960d14eb..f8da19535 100644 --- a/src/rx/rx.h +++ b/src/rx/rx.h @@ -82,7 +82,6 @@ void rx_init(); void rx_update(); void rx_stop(); -float rx_smoothing_hz(); void rx_map_channels(const float channels[4]); void rx_lqi_lost_packet(); diff --git a/src/rx/rx_spi.h b/src/rx/rx_spi.h index 648ca32b4..f75740ed0 100644 --- a/src/rx/rx_spi.h +++ b/src/rx/rx_spi.h @@ -28,6 +28,4 @@ bool rx_flysky_afhds2a_check(); void rx_expresslrs_stop(); -uint16_t rx_expresslrs_smoothing_cutoff(); - void rx_spi_detect(); \ No newline at end of file diff --git a/src/rx/unified_crsf.c b/src/rx/unified_crsf.c index 6b2fd23c2..effed83a5 100644 --- a/src/rx/unified_crsf.c +++ b/src/rx/unified_crsf.c @@ -102,37 +102,6 @@ float rx_serial_crsf_expected_fps() { return 1; } -uint16_t rx_serial_crsf_smoothing_cutoff() { - switch (crsf_rf_mode) { - case RATE_LORA_4HZ: - return 1; - case RATE_LORA_25HZ: - return 11; - case RATE_LORA_50HZ: - return 22; - case RATE_LORA_100HZ: - case RATE_LORA_100HZ_8CH: - return 45; - case RATE_LORA_150HZ: - return 67; - case RATE_LORA_200HZ: - return 90; - case RATE_LORA_250HZ: - case RATE_DVDA_250HZ: - return 112; - case RATE_LORA_333HZ_8CH: - return 150; - case RATE_LORA_500HZ: - case RATE_FLRC_500HZ: - case RATE_DVDA_500HZ: - return 225; - case RATE_FLRC_1000HZ: - return 450; - } - - return 67; -} - static uint16_t telemetry_interval() { switch (crsf_rf_mode) { case RATE_LORA_4HZ: diff --git a/src/rx/unified_dsm.c b/src/rx/unified_dsm.c index 89e667639..70414b123 100644 --- a/src/rx/unified_dsm.c +++ b/src/rx/unified_dsm.c @@ -164,7 +164,7 @@ void rx_spektrum_bind() { gpio_init.mode = GPIO_OUTPUT; gpio_init.output = GPIO_PUSHPULL; gpio_init.pull = GPIO_NO_PULL; - gpio_pin_init( spectrum_bind_pin, gpio_init); + gpio_pin_init(spectrum_bind_pin, gpio_init); // RX line, set high gpio_pin_set(spectrum_bind_pin); @@ -183,19 +183,6 @@ void rx_spektrum_bind() { } } -uint16_t rx_serial_dsm_smoothing_cutoff() { - switch (dsm_protocol) { - case DSM_PROTO_INVALID: - case DSMX_11_2048: - case DSM2_11_2048: - return 40; - case DSMX_22_2048: - case DSM2_22_1024: - return 20; - } - return 40; -} - float rx_serial_dsm_expected_fps() { switch (dsm_protocol) { case DSM_PROTO_INVALID: diff --git a/src/rx/unified_serial.h b/src/rx/unified_serial.h index 27b3df4fe..d40653b6a 100644 --- a/src/rx/unified_serial.h +++ b/src/rx/unified_serial.h @@ -37,8 +37,6 @@ typedef enum { float rx_serial_crsf_expected_fps(); float rx_serial_dsm_expected_fps(); -uint16_t rx_serial_crsf_smoothing_cutoff(); -uint16_t rx_serial_dsm_smoothing_cutoff(); void rx_serial_init(); bool rx_serial_check();