Removed outlier filters and artifacts

rosflight · Jul 9, 2024 · 9216738 · 9216738
1 parent 437a652
commit 9216738
Show file tree

Hide file tree

Showing 3 changed files with 7 additions and 77 deletions.
diff --git a/include/sensors.h b/include/sensors.h
@@ -149,12 +149,12 @@ class Sensors : public ParamListenerInterface
     float diff_pressure_velocity = 0;
     float diff_pressure = 0;
     float diff_pressure_temp = 0;
-    bool diff_pressure_valid = false;
+  //  bool diff_pressure_valid = false;
 
     float baro_altitude = 0;
     float baro_pressure = 0;
     float baro_temperature = 0;
-    bool baro_valid = false;
+  //  bool baro_valid = false;
 
     float sonar_range = 0;
     bool sonar_range_valid = false;
@@ -216,20 +216,6 @@ class Sensors : public ParamListenerInterface
   static const float DIFF_PRESSURE_MAX_CALIBRATION_VARIANCE;
   static constexpr uint32_t BATTERY_MONITOR_UPDATE_PERIOD_MS = 10;
 
-  class OutlierFilter
-  {
-  private:
-    float max_change_;
-    float center_;
-    int window_size_;
-    bool init_ = false;
-
-  public:
-    OutlierFilter() {}
-    void init(float max_change_rate, float update_rate, float center);
-    bool update(float new_val, float * val);
-  };
-
   enum : uint8_t
   {
     BAROMETER,
@@ -302,11 +288,6 @@ class Sensors : public ParamListenerInterface
   float diff_pressure_calibration_mean_ = 0.0f;
   float diff_pressure_calibration_var_ = 0.0f;
 
-  // Sensor Measurement Outlier Filters
-  OutlierFilter baro_outlier_filt_;
-  OutlierFilter diff_outlier_filt_;
-  OutlierFilter sonar_outlier_filt_;
-
   uint32_t last_battery_monitor_update_ms_ = 0;
   // Battery Monitor
   float battery_voltage_alpha_{0.995};

diff --git a/src/comm_manager.cpp b/src/comm_manager.cpp
@@ -397,37 +397,30 @@ void CommManager::send_rc_raw(void)
 
 void CommManager::send_diff_pressure(void)
 {
-  if (RF_.sensors_.data().diff_pressure_valid) {
     comm_link_.send_diff_pressure(sysid_, RF_.sensors_.data().diff_pressure_velocity,
                                   RF_.sensors_.data().diff_pressure,
                                   RF_.sensors_.data().diff_pressure_temp);
   }
-}
 
 void CommManager::send_baro(void)
 {
-  if (RF_.sensors_.data().baro_valid) {
     comm_link_.send_baro(sysid_, RF_.sensors_.data().baro_altitude,
                          RF_.sensors_.data().baro_pressure, RF_.sensors_.data().baro_temperature);
   }
-}
 
 void CommManager::send_sonar(void)
 {
-  if (RF_.sensors_.data().sonar_range_valid) {
     comm_link_.send_sonar(sysid_,
                           0, // TODO set sensor type (sonar/lidar), use enum
                           RF_.sensors_.data().sonar_range, 8.0, 0.25);
   }
-}
 
 void CommManager::send_mag(void)
 {
-  if (RF_.sensors_.data().mag_present) comm_link_.send_mag(sysid_, RF_.sensors_.data().mag);
+ 	  	  comm_link_.send_mag(sysid_, RF_.sensors_.data().mag);
 }
 void CommManager::send_battery_status(void)
 {
-  if (RF_.sensors_.data().battery_monitor_present)
     comm_link_.send_battery_status(sysid_, RF_.sensors_.data().battery_voltage,
                                    RF_.sensors_.data().battery_current);
 }
@@ -446,24 +439,20 @@ void CommManager::send_gnss(void)
 {
   const GNSSData & gnss_data = RF_.sensors_.data().gnss_data;
 
-  if (RF_.sensors_.data().gnss_present) {
     if (gnss_data.time_of_week != last_sent_gnss_tow_) {
       comm_link_.send_gnss(sysid_, gnss_data);
       last_sent_gnss_tow_ = gnss_data.time_of_week;
     }
   }
-}
 
 void CommManager::send_gnss_full()
 {
   const GNSSFull & gnss_full = RF_.sensors_.data().gnss_full;
 
-  if (RF_.sensors_.data().gnss_present) {
     if (gnss_full.time_of_week != last_sent_gnss_full_tow_) {
       comm_link_.send_gnss_full(sysid_, RF_.sensors_.data().gnss_full);
       last_sent_gnss_full_tow_ = gnss_full.time_of_week;
     }
-  }
 }
 
 void CommManager::send_low_priority(void)

diff --git a/src/sensors.cpp b/src/sensors.cpp
@@ -47,7 +47,7 @@ namespace rosflight_firmware
 // TODO: These values don't change actual rates, is there a way to just reference actual rates
 //  as defined in hardware board implementation?
 const float Sensors::BARO_MAX_CHANGE_RATE = 200.0f; // approx 200 m/s
-const float Sensors::BARO_SAMPLE_RATE = 60.0f;
+const float Sensors::BARO_SAMPLE_RATE = 50.0f;
 const float Sensors::DIFF_MAX_CHANGE_RATE = 225.0f; // approx 15 m/s^2
 const float Sensors::DIFF_SAMPLE_RATE = 100.0f;
 const float Sensors::SONAR_MAX_CHANGE_RATE = 100.0f; // 100 m/s
@@ -78,9 +78,6 @@ void Sensors::init()
   float alt = rf_.params_.get_param_float(PARAM_GROUND_LEVEL);
   ground_pressure_ = 101325.0f * static_cast<float>(pow((1 - 2.25694e-5 * alt), 5.2553));
 
-  baro_outlier_filt_.init(BARO_MAX_CHANGE_RATE, BARO_SAMPLE_RATE, ground_pressure_);
-  diff_outlier_filt_.init(DIFF_MAX_CHANGE_RATE, DIFF_SAMPLE_RATE, 0.0f);
-  sonar_outlier_filt_.init(SONAR_MAX_CHANGE_RATE, SONAR_SAMPLE_RATE, 0.0f);
   int_start_us_ = rf_.board_.clock_micros();
 
   this->update_battery_monitor_multipliers();
@@ -183,16 +180,10 @@ got_flags Sensors::run()
     data_.baro_present = true;
     if (rf_.board_.baro_has_new_data()) {
       got.baro = true;
-      float raw_pressure;
-      float raw_temp;
-      rf_.board_.baro_read(&raw_pressure, &raw_temp);
-      data_.baro_valid = baro_outlier_filt_.update(raw_pressure, &data_.baro_pressure);
-      if (data_.baro_valid) {
-        data_.baro_temperature = raw_temp;
+      rf_.board_.baro_read(&data_.baro_pressure, &data_.baro_temperature);
         correct_baro();
       }
     }
-  }
 
   // MAGNETOMETER:
   if (rf_.board_.mag_present()) {
@@ -213,25 +204,17 @@ got_flags Sensors::run()
     data_.diff_pressure_present = true;
     if (rf_.board_.diff_pressure_has_new_data()) {
       got.diff_pressure = true;
-      float raw_pressure;
-      float raw_temp;
-      rf_.board_.diff_pressure_read(&raw_pressure, &raw_temp);
-      data_.diff_pressure_valid = diff_outlier_filt_.update(raw_pressure, &data_.diff_pressure);
-      if (data_.diff_pressure_valid) {
-        data_.diff_pressure_temp = raw_temp;
+      rf_.board_.diff_pressure_read(&data_.diff_pressure, &data_.diff_pressure_temp);
         correct_diff_pressure();
       }
     }
-  }
 
   // SONAR:
   if (rf_.board_.sonar_present()) {
     data_.sonar_present = true;
     if (rf_.board_.sonar_has_new_data()) {
       got.sonar = true;
-      float raw_distance;
-      rf_.board_.sonar_read(&raw_distance);
-      data_.sonar_range_valid = sonar_outlier_filt_.update(raw_distance, &data_.sonar_range);
+      rf_.board_.sonar_read(&data_.sonar_range);
     }
   }
 
@@ -592,29 +575,6 @@ void Sensors::correct_diff_pressure()
     / turbomath::inv_sqrt((turbomath::fabs(data_.diff_pressure) * data_.diff_pressure_temp / atm));
 }
 
-void Sensors::OutlierFilter::init(float max_change_rate, float update_rate, float center)
-{
-  max_change_ = max_change_rate / update_rate;
-  window_size_ = 1;
-  center_ = center;
-  init_ = true;
-}
-
-bool Sensors::OutlierFilter::update(float new_val, float * val)
-{
-  float diff = new_val - center_;
-  if (fabsf(diff) < window_size_ * max_change_) {
-    *val = new_val;
-
-    center_ += turbomath::fsign(diff) * fminf(max_change_, fabsf(diff));
-    if (window_size_ > 1) { window_size_--; }
-    return true;
-  } else {
-    window_size_++;
-    return false;
-  }
-}
-
 void Sensors::update_battery_monitor_multipliers()
 {
   float voltage_multiplier = this->rf_.params_.get_param_float(PARAM_BATTERY_VOLTAGE_MULTIPLIER);