From b7478a5e93094a7f4fd783d95ab1a9842ccec653 Mon Sep 17 00:00:00 2001 From: Phil Tokumaru Date: Tue, 9 Jul 2024 08:20:43 -0700 Subject: [PATCH 1/5] Removed outlier filters and artifacts --- include/sensors.h | 23 ++------------------- src/comm_manager.cpp | 13 +----------- src/sensors.cpp | 48 ++++---------------------------------------- 3 files changed, 7 insertions(+), 77 deletions(-) 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); From 053a037430e3c309dc273c6786fb353f03e145db Mon Sep 17 00:00:00 2001 From: Phil Tokumaru Date: Tue, 9 Jul 2024 17:02:51 -0700 Subject: [PATCH 2/5] Removed some more stuff that was not neede when the outlier filters were removed. --- include/sensors.h | 7 ------- src/sensors.cpp | 9 --------- 2 files changed, 16 deletions(-) diff --git a/include/sensors.h b/include/sensors.h index a6be8da4..1b083855 100644 --- a/include/sensors.h +++ b/include/sensors.h @@ -204,17 +204,10 @@ class Sensors : public ParamListenerInterface got_flags got; private: - static const float BARO_MAX_CHANGE_RATE; - static const float BARO_SAMPLE_RATE; - static const float DIFF_MAX_CHANGE_RATE; - static const float DIFF_SAMPLE_RATE; - static const float SONAR_MAX_CHANGE_RATE; - static const float SONAR_SAMPLE_RATE; static const int SENSOR_CAL_DELAY_CYCLES; static const int SENSOR_CAL_CYCLES; static const float BARO_MAX_CALIBRATION_VARIANCE; static const float DIFF_PRESSURE_MAX_CALIBRATION_VARIANCE; - static constexpr uint32_t BATTERY_MONITOR_UPDATE_PERIOD_MS = 10; enum : uint8_t { diff --git a/src/sensors.cpp b/src/sensors.cpp index f1cfc52e..d91c7748 100644 --- a/src/sensors.cpp +++ b/src/sensors.cpp @@ -44,15 +44,6 @@ 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 = 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 -const float Sensors::SONAR_SAMPLE_RATE = 50.0f; - const int Sensors::SENSOR_CAL_DELAY_CYCLES = 128; const int Sensors::SENSOR_CAL_CYCLES = 127; From 9f3187a46fcf4eb414ebbe2f45fdc9c28dd82aae Mon Sep 17 00:00:00 2001 From: Phil Tokumaru Date: Wed, 10 Jul 2024 09:52:45 -0700 Subject: [PATCH 3/5] removed a couple of lines of commented-out code --- include/sensors.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/include/sensors.h b/include/sensors.h index 1b083855..00914614 100644 --- a/include/sensors.h +++ b/include/sensors.h @@ -149,12 +149,10 @@ class Sensors : public ParamListenerInterface float diff_pressure_velocity = 0; float diff_pressure = 0; float diff_pressure_temp = 0; - // bool diff_pressure_valid = false; float baro_altitude = 0; float baro_pressure = 0; float baro_temperature = 0; - // bool baro_valid = false; float sonar_range = 0; bool sonar_range_valid = false; From 4485f7cb19a8f49c0e19b90069edd5bdc55aba60 Mon Sep 17 00:00:00 2001 From: Phil Tokumaru Date: Wed, 10 Jul 2024 09:58:50 -0700 Subject: [PATCH 4/5] ran clang-format -i comm_manager.cpp --- src/comm_manager.cpp | 47 +++++++++++++++++++++----------------------- 1 file changed, 22 insertions(+), 25 deletions(-) diff --git a/src/comm_manager.cpp b/src/comm_manager.cpp index 7ebefa72..557c92c4 100644 --- a/src/comm_manager.cpp +++ b/src/comm_manager.cpp @@ -397,32 +397,29 @@ void CommManager::send_rc_raw(void) void CommManager::send_diff_pressure(void) { - comm_link_.send_diff_pressure(sysid_, RF_.sensors_.data().diff_pressure_velocity, - RF_.sensors_.data().diff_pressure, - RF_.sensors_.data().diff_pressure_temp); - } + 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) { - comm_link_.send_baro(sysid_, RF_.sensors_.data().baro_altitude, - RF_.sensors_.data().baro_pressure, RF_.sensors_.data().baro_temperature); - } + 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) { - 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) -{ - comm_link_.send_mag(sysid_, RF_.sensors_.data().mag); + 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) { comm_link_.send_mag(sysid_, RF_.sensors_.data().mag); } void CommManager::send_battery_status(void) { - comm_link_.send_battery_status(sysid_, RF_.sensors_.data().battery_voltage, - RF_.sensors_.data().battery_current); + comm_link_.send_battery_status(sysid_, RF_.sensors_.data().battery_voltage, + RF_.sensors_.data().battery_current); } void CommManager::send_backup_data(const StateManager::BackupData & backup_data) @@ -439,20 +436,20 @@ void CommManager::send_gnss(void) { const GNSSData & gnss_data = RF_.sensors_.data().gnss_data; - 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; - } + 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 (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; - } + 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) From 1b25527ef8af53789eb78d59d039ac5865b5c8d9 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Wed, 10 Jul 2024 12:43:17 -0600 Subject: [PATCH 5/5] misc formatting tweaks --- src/sensors.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/sensors.cpp b/src/sensors.cpp index d91c7748..9dea58be 100644 --- a/src/sensors.cpp +++ b/src/sensors.cpp @@ -172,9 +172,9 @@ got_flags Sensors::run() if (rf_.board_.baro_has_new_data()) { got.baro = true; rf_.board_.baro_read(&data_.baro_pressure, &data_.baro_temperature); - correct_baro(); - } + correct_baro(); } + } // MAGNETOMETER: if (rf_.board_.mag_present()) { @@ -196,9 +196,9 @@ got_flags Sensors::run() if (rf_.board_.diff_pressure_has_new_data()) { got.diff_pressure = true; rf_.board_.diff_pressure_read(&data_.diff_pressure, &data_.diff_pressure_temp); - correct_diff_pressure(); - } + correct_diff_pressure(); } + } // SONAR: if (rf_.board_.sonar_present()) {