Simple Battery measurement

Makefile

@@ -641,6 +641,7 @@ OBJECTS += ./mbed-os/targets/TARGET_STM/trng_api.o
 OBJECTS += ./mbed-os/targets/TARGET_STM/us_ticker.o
 
 OBJECTS += ./src/control/lpf.o
+OBJECTS += ./src/drivers/Battery.o
 OBJECTS += ./src/drivers/MPU6000.o
 OBJECTS += ./src/drivers/MS5611.o
 OBJECTS += ./src/drivers/stepper.o
@@ -652,7 +653,6 @@ OBJECTS += ./src/math/Utilities.o
 OBJECTS += ./src/serialization/RCProtocol.o
 
 
-
 INCLUDE_PATHS += -I../
 INCLUDE_PATHS += -I../.
 INCLUDE_PATHS += -I.././.

main.cpp

@@ -11,6 +11,7 @@
 // Mmath
 #include "src/math/Utilities.h"
 // Drivers
+#include "src/drivers/Battery.h"
 #include "src/drivers/MPU6000.h"
 #include "src/drivers/stepper.h"
 #include "src/drivers/servo.h"
@@ -53,6 +54,8 @@ Stepper motorR(PB_15, PB_14, PC_6);
 
 Servo servo(PA_0);
 
+Lipo lipo(PC_1, 13.5242, 3);
+
 // Interrupt pin from Gyro to MCU
 InterruptIn gyroISR(PC_4);
 // Madwick filter
@@ -190,7 +193,7 @@ void runControl()
     motorL.setSpeed(controlOutput - steering);
     motorR.setSpeed(controlOutput + steering);
 
-    // Blink LED at 1hz
+    // Blink LED at 1hz if active, 4hz otherwise
     static int i = 0;
     if (++i > (disabled ? 25 : 100))
     {
@@ -221,7 +224,7 @@ void serialWrite()
     packetOut.BatteryLevel = 123;
     packetOut.Mode = 213;
     RC.setOutput(packetOut);
-    bool done;
+    bool done(false);
     
     while (true)
     {
@@ -233,6 +236,12 @@ void serialWrite()
         // writing a packet we sleep
         if (done)
         {
+            float voltage = lipo.getVoltage();
+            float charge = lipo.getCharge();
+            Lipo::State state = lipo.getState();
+
+            packetOut.BatteryLevel = charge*100;
+            RC.setOutput(packetOut);
             Thread::wait(500);
         }
     }

src/drivers/Battery.cpp

@@ -0,0 +1,56 @@
+#include "src/drivers/Battery.h"
+#include "src/math/Utilities.h"
+
+using namespace math;
+
+Lipo::Lipo(AnalogIn pin, float scale, int cells)
+: m_pin(pin)
+, m_scale(scale)
+, m_cells(cells)
+, m_state(Lipo::Full)
+{   
+}
+
+float Lipo::getCharge()
+{
+    float voltage = constrain(getCellVoltage(), 3.3f, 4.2f);
+
+    static float range = 4.2f - 3.3f;
+
+    return (voltage - 3.3f)/range;
+}
+
+float Lipo::getVoltage()
+{
+    return m_pin.read()*m_scale;
+}
+
+Lipo::State Lipo::getState()
+{
+    float cells = getCellVoltage();
+
+    if (cells < 3.3f)
+    {
+        return Lipo::Empty;
+    }
+
+    if (cells < 3.5f)
+    {
+        return Lipo::Low;
+    }
+
+    if (cells < 4.1)
+    {
+        return Lipo::Okej;
+    }
+
+    return Full;
+}
+
+float Lipo::getCellVoltage()
+{
+    return getVoltage()/m_cells;
+}
+
+
+

src/drivers/Battery.h

@@ -0,0 +1,34 @@
+#include "mbed.h"
+
+class Lipo
+{
+public:
+    Lipo(AnalogIn pin, float scale, int cells = 3);
+
+    //! Returns the charge in [0-1]
+    //! On a lipo the range is 3.3 - 4.2V 
+    float getCharge();
+
+    float getVoltage();
+
+    enum State {
+        Full,
+        Okej,
+        Low,
+        Empty,
+    };
+
+    State getState();
+
+private:
+
+    float getCellVoltage();
+
+    AnalogIn m_pin;
+
+    float m_scale;
+
+    int m_cells;
+
+    State m_state;
+};

src/math/Utilities.cpp

@@ -16,5 +16,18 @@ float constrain(float value, float range)
     }
     return value;
 }
+
+float constrain(float value, float min, float max)
+{
+    if (value < min)
+    {
+        return min;
+    }
+    if (value > max)
+    {
+        return max;
+    }
+    return value;
+}
     
 }

src/math/Utilities.h

@@ -7,4 +7,6 @@ namespace math {
 
 float constrain(float value, float range);
 
+float constrain(float value, float min, float max);
+
 }