AP_BattMonitor: Add calculate_mah, add unit tests

libraries/AP_BattMonitor/AP_BattMonitor_Analog.cpp

@@ -93,8 +93,7 @@ AP_BattMonitor_Analog::read()
 
         // update total current drawn since startup
         if (_state.last_time_micros != 0 && dt < 2000000.0f) {
-            // .0002778 is 1/3600 (conversion to hours)
-            float mah = _state.current_amps * dt * 0.0000002778f;
+            float mah = calculate_mah(_state.current_amps, dt);
             _state.consumed_mah += mah;
             _state.consumed_wh  += 0.001f * mah * _state.voltage;
         }

libraries/AP_BattMonitor/AP_BattMonitor_Backend.h

@@ -79,6 +79,10 @@ public:
     void Log_Write_BAT(const uint8_t instance, const uint64_t time_us) const;
     void Log_Write_BCL(const uint8_t instance, const uint64_t time_us) const;
 
+    // amps: current (A)
+    // dt_us: time between samples (micro-seconds)
+    static float calculate_mah(float amps, float dt_us) { return (float) (amps * dt_us * AUS_TO_MAH); }
+
 protected:
     AP_BattMonitor                      &_mon;      // reference to front-end
     AP_BattMonitor::BattMonitor_State   &_state;    // reference to this instances state (held in the front-end)

libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp

@@ -141,8 +141,7 @@ void AP_BattMonitor_UAVCAN::update_interim_state(const float voltage, const floa
 
         // update total current drawn since startup
         if (_interim_state.last_time_micros != 0 && dt < 2000000) {
-            // .0002778 is 1/3600 (conversion to hours)
-            float mah = (float) ((double) _interim_state.current_amps * (double) dt * (double) 0.0000002778f);
+            float mah = calculate_mah(_interim_state.current_amps, dt);
             _interim_state.consumed_mah += mah;
             _interim_state.consumed_wh  += 0.001f * mah * _interim_state.voltage;
         }

libraries/AP_BattMonitor/tests/test_calculate_mah.cpp

@@ -0,0 +1,39 @@
+#include <AP_gtest.h>
+
+#include <AP_BattMonitor/AP_BattMonitor_Backend.h>
+
+float calculate_mah_with_double_cast(float amps, float dt)
+{
+    return (float) ((double) amps * (double) dt * (double) 0.0000002778f);
+}
+
+float calculate_mah(float amps, float dt)
+{
+    return AP_BattMonitor_Backend::calculate_mah(amps, dt);
+}
+
+TEST(AP_BATTMONITOR_MAH, test_calculate_mah)
+{
+    /* Basic unit tests to check for regressions */
+    EXPECT_FLOAT_EQ(0.0002778, calculate_mah(1000, 1));
+    EXPECT_FLOAT_EQ(2.778e-06, calculate_mah(1, 10));
+    EXPECT_FLOAT_EQ(0.34296274, calculate_mah(1234567, 1));
+
+    /* Test negative amperes */
+    EXPECT_FLOAT_EQ(-0.02778, calculate_mah(-100, 1000));
+    EXPECT_FLOAT_EQ(-34296.3, calculate_mah(-12345678, 10000));
+    EXPECT_FLOAT_EQ(-2.778e-10, calculate_mah(-0.000001, 1000));
+    EXPECT_FLOAT_EQ(-2.778e-07, calculate_mah(-1, 1));
+}
+
+TEST(AP_BATTMONITOR_ACCURACY, test_float_accuracy)
+{
+    /* Test for loss of accuracy */
+    EXPECT_FLOAT_EQ(calculate_mah(100, 1), calculate_mah_with_double_cast(100, 1));
+    EXPECT_FLOAT_EQ(calculate_mah(-1, 1), calculate_mah_with_double_cast(-1, 1));
+    EXPECT_FLOAT_EQ(calculate_mah(0.0000000001f, 1), calculate_mah_with_double_cast(0.0000000001f, 1)); 
+    EXPECT_FLOAT_EQ(calculate_mah(1234.123456789, 1), calculate_mah_with_double_cast(1234.123456789, 1));
+    EXPECT_FLOAT_EQ(calculate_mah(-1234.123456789, 1), calculate_mah_with_double_cast(-1234.123456789, 1));
+}
+
+AP_GTEST_MAIN()

libraries/AP_BattMonitor/tests/wscript

@@ -0,0 +1,7 @@
+#!/usr/bin/env python
+# encoding: utf-8
+
+def build(bld):
+    bld.ap_find_tests(
+        use='ap',
+    )