Add power modeling and scheduling based on thesis

src/power/Power.cpp

@@ -74,6 +74,16 @@ float Power::getBatteryVoltage() {
 }
 
 int Power::getBatteryChargePercent() {
+  return percentRemaining;
+}
+
+float Power::getBatteryChargeCoulombs() {
+  return coloumbsRemaining;
+}
+
+
+
+int Power::getBatteryVoltageChargePercent() {
   // Get the battery voltage and calculate the charge state based on the
   // discharge curve.
   float batteryVoltage = getBatteryVoltage();
@@ -105,7 +115,69 @@ int Power::getBatteryChargePercent() {
   }
 }
 
-PowerScheduler* Power::powerScheduler = nullptr;
+void Power::updatePowerStateHandler() {
+  float currentCurrent = powerScheduler->getCurrentCurrent();
+  int referenceCurrentMa =
+      PowerParameters::Battery::DISCHARGE_CURVE::REFERENCE_CURRENT_A * 1000;
+
+  // Calculate remaining battery charge in Coulombs based on current and time
+  float coloumbsConsumedSinceLastUpdate =
+      (currentCurrent / 1000) *
+      ((pdTICKS_TO_MS(xTaskGetTickCount() - lastPowerStateUpdate)) / 1000.0);
+
+  // Update coloumbs remaining
+  coloumbsRemaining -= coloumbsConsumedSinceLastUpdate;
+
+  float chargeState;
+
+  // If current flow is close enough to reference, get battery charge state via
+  // voltage curve
+  if ((currentCurrent > (referenceCurrentMa * 0.6)) &&
+      (currentCurrent < (referenceCurrentMa * 1.4))) {
+    // Get battery charge state from voltage curve
+    chargeState = getBatteryVoltageChargePercent();
+  } else {
+    // Calculate battery charge state from Charge consumption
+    float oldChargeState = lastSOC[latestSoCIndex];
+    float chargeState =
+        oldChargeState - ((coloumbsConsumedSinceLastUpdate /
+                           PowerParameters::Battery::CELL_CHARGE_FULL_COLOUMB) *
+                          100);
+  }
+
+  addSoCSample(chargeState);
+
+  // Update percentage remaining based on charge state average
+  float sampleSum = 0;
+  for (int i = 0; i < PowerParameters::Battery::AVERAGING_SAMPLES; i++) {
+    sampleSum += lastSOC[i];
+  }
+  percentRemaining = sampleSum / PowerParameters::Battery::AVERAGING_SAMPLES;
+
+  // Update last update time
+  lastPowerStateUpdate = xTaskGetTickCount();
+
+  // Update the available current (changes based on battery state of charge)
+  powerScheduler->recalculateCurrentBudgets();
+
+  return;
+
+}
+
+float Power::getMax3V3Current() {
+  float u_bat = getBatteryVoltage();
+  float i_bat = PowerParameters::Battery::CELL_CURRENT_1C;
+  float eta = PowerParameters::BUCK_BOOST_EFFICIENCY;
+  constexpr float u_3v3 = 3.3;
+  return (u_bat * i_bat * eta) / u_3v3;
+}
+
+void Power::addSoCSample(float soc) {
+  latestSoCIndex = (latestSoCIndex + 1) % PowerParameters::Battery::AVERAGING_SAMPLES;
+  lastSOC[latestSoCIndex] = soc;
+}
+
+PowerScheduler *Power::powerScheduler = nullptr;
 
 Power::Power() {
   // Initialize the power scheduler