diff --git a/include/sensors.h b/include/sensors.h index d819c5b2..a6be8da4 100644 --- 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 index de28bbd7..7ebefa72 100644 --- 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 index e23a48b1..f1cfc52e 100644 --- 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(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);