Skip to content

Commit

Permalink
Merge pull request #427 from rosflight/PTT/remove_outlier_filter
Browse files Browse the repository at this point in the history 
Removed outlier filters and artifacts
  • Loading branch information
@bsutherland333
bsutherland333 authored Jul 10, 2024
2 parents 0f7ef64 + 1b25527 commit 7119082
Show file tree
Hide file tree
Showing 3 changed files with 22 additions and 113 deletions.
28 changes: 0 additions & 28 deletions include/sensors.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -204,31 +202,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;

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
{
Expand Down Expand Up @@ -302,11 +279,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
48 changes: 17 additions & 31 deletions src/comm_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -397,39 +397,29 @@ 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);
}
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);
}
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);
}
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);
}
void CommManager::send_mag(void) { 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);
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)
Expand All @@ -446,23 +436,19 @@ 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;
}
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;
}
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;
}
}

Expand Down
59 changes: 5 additions & 54 deletions src/sensors.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 = 60.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;

Expand All @@ -78,9 +69,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,14 +171,8 @@ 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;
correct_baro();
}
rf_.board_.baro_read(&data_.baro_pressure, &data_.baro_temperature);
correct_baro();
}
}

Expand All @@ -213,14 +195,8 @@ 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;
correct_diff_pressure();
}
rf_.board_.diff_pressure_read(&data_.diff_pressure, &data_.diff_pressure_temp);
correct_diff_pressure();
}
}

Expand All @@ -229,9 +205,7 @@ got_flags Sensors::run()
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 +566,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 7119082

Please sign in to comment.