From 11518857b6a107aba877e287a50b22c4a7e16e1f Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 18 Jun 2019 19:30:18 +1000
Subject: [PATCH] AP_BattMonitor: added reset_remaining() API

used to allow battery swap before takeoff
---
 libraries/AP_BattMonitor/AP_BattMonitor.cpp   | 15 +++++++++++++++
 libraries/AP_BattMonitor/AP_BattMonitor.h     |  3 +++
 .../AP_BattMonitor/AP_BattMonitor_Backend.cpp | 19 +++++++++++++++++++
 .../AP_BattMonitor/AP_BattMonitor_Backend.h   |  3 +++
 .../AP_BattMonitor/AP_BattMonitor_Bebop.h     |  3 +++
 .../AP_BattMonitor/AP_BattMonitor_SMBus.h     |  3 +++
 6 files changed, 46 insertions(+)

diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp
index 27af61c49a..fadb8aed81 100644
--- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp
+++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp
@@ -494,6 +494,21 @@ void AP_BattMonitor::checkPoweringOff(void)
     }
 }
 
+/*
+  reset battery remaining percentage for batteries that integrate to
+  calculate percentage remaining
+*/
+bool AP_BattMonitor::reset_remaining(uint16_t battery_mask, float percentage)
+{
+    bool ret = true;
+    for (uint8_t i = 0; i < _num_instances; i++) {
+        if ((1U<<i) & battery_mask) {
+            ret &= drivers[i]->reset_remaining(percentage);
+        }
+    }
+    return ret;
+}
+
 namespace AP {
 
 AP_BattMonitor &battery()
diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h
index 1100b519ea..3c282140a6 100644
--- a/libraries/AP_BattMonitor/AP_BattMonitor.h
+++ b/libraries/AP_BattMonitor/AP_BattMonitor.h
@@ -167,6 +167,9 @@ public:
     // sends powering off mavlink broadcasts and sets notify flag
     void checkPoweringOff(void);
 
+    // reset battery remaining percentage
+    bool reset_remaining(uint16_t battery_mask, float percentage);
+
     static const struct AP_Param::GroupInfo var_info[];
 
 protected:
diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp
index c1a9f25ad1..4e80f1f0ef 100644
--- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp
+++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp
@@ -215,3 +215,22 @@ void AP_BattMonitor_Backend::check_failsafe_types(bool &low_voltage, bool &low_c
         low_capacity = false;
     }
 }
+
+/*
+  default implementation for reset_remaining(). This sets consumed_wh
+  and consumed_mah based on the given percentage. Use percentage=100
+  for a full battery
+*/
+bool AP_BattMonitor_Backend::reset_remaining(float percentage)
+{
+    percentage = constrain_float(percentage, 0, 100);
+    const float used_proportion = (100 - percentage) * 0.01;
+    _state.consumed_mah = used_proportion * _params._pack_capacity;
+    // without knowing the history we can't do consumed_wh
+    // accurately. Best estimate is based on current voltage. This
+    // will be good when resetting the battery to a value close to
+    // full charge
+    _state.consumed_wh = _state.consumed_mah * 1000 * _state.voltage;
+
+    return true;
+}
diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h
index 33fc9277bb..1f2fa762c7 100644
--- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h
+++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h
@@ -59,6 +59,9 @@ public:
     // returns false if we fail arming checks, in which case the buffer will be populated with a failure message
     bool arming_checks(char * buffer, size_t buflen) const;
 
+    // reset remaining percentage to given value
+    virtual bool reset_remaining(float percentage);
+
 protected:
     AP_BattMonitor                      &_mon;      // reference to front-end
     AP_BattMonitor::BattMonitor_State   &_state;    // reference to this instances state (held in the front-end)
diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Bebop.h b/libraries/AP_BattMonitor/AP_BattMonitor_Bebop.h
index 66ae455a45..1204531c53 100644
--- a/libraries/AP_BattMonitor/AP_BattMonitor_Bebop.h
+++ b/libraries/AP_BattMonitor/AP_BattMonitor_Bebop.h
@@ -40,6 +40,9 @@ public:
     // bebop provides current info
     bool has_current() const override { return true; };
 
+    // don't allow reset of remaining capacity for bebop battery
+    bool reset_remaining(float percentage) override { return false; }
+
 private:
     float _prev_vbat_raw;
     float _prev_vbat;
diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h
index c365a0d49a..74294bc00d 100644
--- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h
+++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h
@@ -42,6 +42,9 @@ public:
     // all smart batteries are expected to provide current
     bool has_current() const override { return true; }
 
+    // don't allow reset of remaining capacity for SMBus
+    bool reset_remaining(float percentage) override { return false; }
+    
     void init(void) override;
 
 protected: