Skip to content

Commit

Permalink
Removed outlier filters and artifacts
Browse files Browse the repository at this point in the history
  • Loading branch information
avtoku committed Jul 9, 2024
1 parent 437a652 commit 9216738
Show file tree
Hide file tree
Showing 3 changed files with 7 additions and 77 deletions.
23 changes: 2 additions & 21 deletions include/sensors.h
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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};
Expand Down
13 changes: 1 addition & 12 deletions src/comm_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
Expand All @@ -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)
Expand Down
48 changes: 4 additions & 44 deletions src/sensors.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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()) {
Expand All @@ -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);
}
}

Expand Down Expand Up @@ -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);
Expand Down

0 comments on commit 9216738

Please sign in to comment.