rx: automatically detect filter hz

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******************************************************
 

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,
 

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
 

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);

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;
 

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();

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();

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();

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:

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:

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();