diff --git a/include/sensors.h b/include/sensors.h index 6f41f1e4..ff02ecc3 100644 --- a/include/sensors.h +++ b/include/sensors.h @@ -268,7 +268,6 @@ class Sensors : public ParamListenerInterface // Baro Calibration bool baro_calibrated_ = false; - float ground_pressure_ = 0.0f; uint16_t baro_calibration_count_ = 0; uint32_t last_baro_cal_iter_ms_ = 0; float baro_calibration_mean_ = 0.0f; diff --git a/src/sensors.cpp b/src/sensors.cpp index fb491836..98de3162 100644 --- a/src/sensors.cpp +++ b/src/sensors.cpp @@ -66,9 +66,6 @@ void Sensors::init() next_sensor_to_update_ = BAROMETER; - float alt = rf_.params_.get_param_float(PARAM_GROUND_LEVEL); - ground_pressure_ = 101325.0f * static_cast(pow((1 - 2.25694e-5 * alt), 5.2553)); - int_start_us_ = rf_.board_.clock_micros(); this->update_battery_monitor_multipliers(); @@ -462,17 +459,19 @@ void Sensors::calibrate_baro() // else reset cal variables and start over if (baro_calibration_var_ < BARO_MAX_CALIBRATION_VARIANCE) { rf_.params_.set_param_float(PARAM_BARO_BIAS, baro_calibration_mean_); + // set ground altitude to be pressure altitude at PARAM_BARO_BIAS + rf_.params_.set_param_float(PARAM_GROUND_LEVEL,turbomath::alt(rf_.params_.get_param_float(PARAM_BARO_BIAS))); baro_calibrated_ = true; - rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO, "Baro Cal successful!"); + rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO, "Baro ground pressure cal successful!"); } else { rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, - "Too much movement for barometer cal"); + "Too much movement for barometer ground pressure cal"); } baro_calibration_mean_ = 0.0f; baro_calibration_var_ = 0.0f; baro_calibration_count_ = 0; } else if (baro_calibration_count_ > SENSOR_CAL_DELAY_CYCLES) { - float measurement = data_.baro_pressure - ground_pressure_; + float measurement = data_.baro_pressure; float delta = measurement - baro_calibration_mean_; baro_calibration_mean_ += delta / (baro_calibration_count_ - SENSOR_CAL_DELAY_CYCLES); float delta2 = measurement - baro_calibration_mean_; @@ -551,9 +550,7 @@ void Sensors::correct_mag(void) void Sensors::correct_baro(void) { if (!baro_calibrated_) { calibrate_baro(); } - data_.baro_pressure -= rf_.params_.get_param_float(PARAM_BARO_BIAS); - data_.baro_altitude = - turbomath::alt(data_.baro_pressure) - rf_.params_.get_param_float(PARAM_GROUND_LEVEL); + data_.baro_altitude = turbomath::alt(data_.baro_pressure); } void Sensors::correct_diff_pressure()