Merge branch 'main' into ptt/pitotspeed

rosflight · Oct 7, 2024 · 05ef074 · 05ef074
2 parents 77ce456 + 02c8779
commit 05ef074
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 10 deletions.
diff --git 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
@@ -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<float>(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()