add lpf to the battery based on the existing alpha parameters

Author: JMoore5353

include/sensors.h

@@ -122,6 +122,7 @@ class Sensors : public ParamListenerInterface
   void correct_mag(void);
   void correct_baro(void);
   void correct_diff_pressure(void);
+  void lpf_battery(void);
   void update_battery_monitor_multipliers(void);
 
   // IMU calibration
@@ -151,6 +152,8 @@ class Sensors : public ParamListenerInterface
   // Battery Monitor
   float battery_voltage_alpha_{0.995};
   float battery_current_alpha_{0.995};
+  float previous_battery_voltage_{0.0};
+  float previous_battery_current_{0.0};
 };
 
 } // namespace rosflight_firmware

src/sensors.cpp

@@ -52,7 +52,9 @@ const float Sensors::DIFF_PRESSURE_MAX_CALIBRATION_VARIANCE = 100.0; // standard
 
 Sensors::Sensors(ROSflight & rosflight)
     : rf_(rosflight)
-{}
+{
+  previous_battery_voltage_ = rf_.params_.get_param_float(PARAM_BATTERY_VOLTAGE_ALPHA);
+}
 
 void Sensors::init()
 {
@@ -169,7 +171,9 @@ got_flags Sensors::run()
   got.sonar = rf_.board_.sonar_read(&sonar_);
 
   // BATTERY_MONITOR:
-  got.battery = rf_.board_.battery_read(&battery_);
+  if ((got.battery = rf_.board_.battery_read(&battery_))) {
+    lpf_battery();
+  }
 
   return got;
 }
@@ -477,6 +481,14 @@ void Sensors::correct_diff_pressure()
     * sqrt((fabs(diff_pressure_.pressure) / (0.5 * 1.225)));
 }
 
+void Sensors::lpf_battery()
+{
+  battery_.voltage = battery_.voltage * (1.0 - battery_voltage_alpha_) + previous_battery_voltage_ * battery_voltage_alpha_;
+  battery_.current = battery_.current * (1.0 - battery_current_alpha_) + previous_battery_current_ * battery_current_alpha_;
+  previous_battery_voltage_ = battery_.voltage;
+  previous_battery_current_ = battery_.current;
+}
+
 void Sensors::update_battery_monitor_multipliers()
 {
   float voltage_multiplier = this->rf_.params_.get_param_float(PARAM_BATTERY_VOLTAGE_MULTIPLIER);