extend battery lib with messuared values and new

calculation

lib/Battery/battery.cpp

@@ -15,17 +15,29 @@ Battery::Battery(uint8_t pin, uint32_t r1, uint32_t r2) {
     this->pin = pin;
     this->r1 = r1;
     this->r2 = r2;
-    this->calculateBatteryVoltage();
+    this->initBuffer();
 }
 
-void Battery::loop() {
+Battery::Battery(uint8_t pin) {
-    if (millis() - this->lastMillis < this->delay)
+    this->pin = pin;
-        return;
+    this->initBuffer();
+}
 
+bool Battery::loop() {
+    if (millis() - this->lastMillis < this->delay)
+        return false;
+
+    this->readAdcToBuf();
+    this->loopCounter++;
+    this->lastMillis = millis();
+
+    if (this->loopCounter == this->calulationDelayMultiplier) {
         this->calculateBatteryVoltage();
         this->calculateBatteryPercent();
-
+        return true;
-    this->lastMillis = millis();
+        this->loopCounter = 0;
+    }
+    return false;
 }
 
 double Battery::getBatteryVoltage() {
@@ -39,9 +51,9 @@ bool Battery::isBatteryLow(uint8_t percent) {
     return false;
 }
 
-double Battery::readInputVoltage() {
+double Battery::calculateInputVoltage() {
     // Reference voltage is 3v3 so maximum reading is 3v3 = 4095 in range 0 to 4095
-    double reading = analogRead(this->pin);
+    double reading = this->getBufAvg();
     if(reading < 1 || reading > 4095) return 0;
     return  - 0.000000000000016 * pow(reading,4)
             + 0.000000000118171 * pow(reading,3)
@@ -51,7 +63,34 @@ double Battery::readInputVoltage() {
 }
 
 void Battery::calculateBatteryVoltage() {
-    this->batteryVoltage = (readInputVoltage() * (double) (this->r1 + this->r2)) / (double) this->r2;
+    if (this->r1 && this->r2) {
+        this->batteryVoltage = (this->calculateInputVoltage() * (double) (this->r1 + this->r2)) / (double) this->r2;
+        return;
+    }
+
+    uint16_t adcValue = this->getBufAvg();
+    if (adcValue < this->rawAdcVoltages[0]) {
+        this->batteryVoltage = -1;
+        return;
+    }
+
+    if (adcValue > this->rawAdcVoltages[this->rawAdcVoltagesCount] + 50) {
+        this->batteryVoltage = -2;
+        return;
+    }
+
+    uint8_t index;
+    for (index = 1; index < this->rawAdcVoltagesCount; index++) {
+        if (adcValue < this->rawAdcVoltages[index])
+            break;
+    }
+    double indexDelta = this->rawAdcVoltages[index] - this->rawAdcVoltages[index - 1];
+    double valueDelta = this->rawAdcVoltages[index] - adcValue;
+    double voltage = this->startVoltage + (index - 1) * this->stepVoltage;
+    voltage += valueDelta / indexDelta * this->stepVoltage;
+    this->batteryVoltage = voltage;
+
+    // std::cout << "Battery::calculateBatteryVoltage() - Voltage: " << voltage << " Index: " <<(int) index << " adcValue: " << (int) adcValue <<" iD: " << indexDelta << " vD: " << valueDelta << std::endl;
 }
 
 void Battery::calculateBatteryPercent() {
@@ -75,3 +114,28 @@ void Battery::calculateBatteryPercent() {
     }
     this->batteryPercent = i * (100 / (size - 1));
 }
+
+void Battery::readAdcToBuf() {
+    this->adcBuffer[this->bufferPos] = analogRead(this->pin);
+    this->bufferPos++;
+    if (this->bufferPos == Battery::bufferSize)
+        this->bufferPos = 0;
+
+    // std::cout << "Battery::readAdcToBuf added Value: " << this->adcBuffer[this->bufferPos] << std::endl;
+}
+
+void Battery::initBuffer() {
+    for (uint8_t i = 0; i < Battery::bufferSize; i++)
+        this->adcBuffer[i] = 0;
+}
+
+uint16_t Battery::getBufAvg() {
+    uint32_t res = 0;
+    uint8_t emptyPos = 0;
+    for (uint8_t i = 0; i < Battery::bufferSize; i++) {
+        if (this->adcBuffer[i] == 0)
+            emptyPos++;
+        res += this->adcBuffer[i];
+    }
+    return res / (Battery::bufferSize - emptyPos);
+}

lib/Battery/battery.h

@@ -9,8 +9,8 @@
  * 
  */
 
-#ifndef AKKU_H
+#ifndef BATTERY_H
-#define AKKU_H
+#define BATTERY_H
 
 #include <stdint.h>
 #include <iostream>
@@ -39,11 +39,14 @@ class Battery {
          * @param r2 Second resistor of the voltage devider.
          */
         Battery(uint8_t pin, uint32_t r1, uint32_t r2);
+        Battery(uint8_t pin);
 
         /**
          * @brief Read new values if the delay is reached.
+         * 
+         * @return bool true if new values are calculated
          */
-        void loop();
+        bool loop();
 
         /**
          * @brief Set the delay to wait befor new values are read
@@ -87,19 +90,28 @@ class Battery {
         bool isBatteryLow(uint8_t percent);
 
     private:
-        double readInputVoltage();
+        double calculateInputVoltage();
         void calculateBatteryVoltage();
         void calculateBatteryPercent();
+        void readAdcToBuf();
+        void initBuffer();
+        uint16_t getBufAvg();
+
+        static const uint8_t bufferSize = 30;
 
         uint8_t absurdLowVoltage = 5;
         uint8_t pin;
-        uint8_t batteryPercent;
+        uint8_t batteryPercent = 0;
         uint8_t batteryLowPercent = 10;
-        uint16_t delay = 20000;
+        uint8_t bufferPos = 0;
-        uint32_t r1;
+        uint8_t delay = 100;
-        uint32_t r2;
+        uint8_t calulationDelayMultiplier = 10;
+        uint8_t loopCounter = 0;
+        uint16_t adcBuffer[bufferSize];
+        uint32_t r1 = 0;
+        uint32_t r2 = 0;
         uint64_t lastMillis = 0;
-        double batteryVoltage;
+        double batteryVoltage = 0;
 
         const float capacityVoltages[21] = {9.82, 10.83, 11.06, 11.12,  //  0  5 10 15
                                             11.18, 11.24, 11.3, 11.36,  // 20 25 30 35
@@ -107,6 +119,17 @@ class Battery {
                                             11.62, 11.74, 11.86, 11.95, // 60 65 70 75
                                             12.07, 12.25, 12.33, 12.45, // 80 85 90 95
                                             12.6 };
+
+        const uint8_t rawAdcVoltagesCount = 60;
+        const double startVoltage = 7;
+        const double stepVoltage = 0.1;
+        const uint16_t rawAdcVoltages[60] = // from 7.0V to 12.9V in 0.1V steps
+                           {1820, 1851, 1880, 1910, 1937, 1967, 1992, 2020, 2048, 2080,
+                            2109, 2136, 2163, 2189, 2218, 2244, 2273, 2302, 2334, 2363,
+                            2391, 2415, 2441, 2471, 2499, 2531, 2557, 2587, 2617, 2646,
+                            2674, 2699, 2730, 2761, 2791, 2816, 2843, 2872, 2900, 2930,
+                            2958, 2991, 3017, 3049, 3080, 3115, 3144, 3178, 3208, 3242,
+                            3276, 3313, 3346, 3389, 3433, 3470, 3509, 3548, 3590, 3636};
 };
 
-#endif // AKKU_H
+#endif // BATTERY_H

src/main.cpp

@@ -77,7 +77,7 @@ void setup() {
     spiPort = new SPIClass(HSPI);
     spiPort->begin(UBLOX_GNSS_SPI_SCK, UBLOX_GNSS_SPI_CIPO, UBLOX_GNSS_SPI_COPI, UBLOX_GNSS_SPI_CS);
 
-    mainBattery = new Battery(35, 692000, 218500);
+    mainBattery = new Battery(35);
     controlPad = new ControlPad();
 
     Wire.begin(21, 19);
@@ -137,12 +137,18 @@ void loop() {
     main_m->update();
     lcdWrapper->loop();
 
-    mainBattery->loop();
+    bool newValues = mainBattery->loop();
-    if (mainBattery->isBatteryLow(10)) {
+
+    if (newValues) {
+        static uint8_t batteryLowCounter = 0;
+        if (mainBattery->isBatteryLow(10))
+            batteryLowCounter++;
+        else
+            batteryLowCounter = 0;
+
+        if (batteryLowCounter >= 10) {        
             moveController.emergencyStop();
-
             uint16_t voltage = (uint16_t) (mainBattery->getBatteryVoltage() * 100);
-
             lcd->setCursor(0, 0);
             lcd->printf("Low Battery: %u", voltage);
             lcd->setCursor(0, 1);
@@ -150,6 +156,7 @@ void loop() {
             
             while (true);
         }
+    }
 }
 
 void i2cScanner() {
@@ -202,7 +209,7 @@ void makeMenu() {
     MenuSysteminformation* sys_m = new MenuSysteminformation(mainBattery);
     MenuRoute* rout_m = new MenuRoute(driveManager->getNavigation()->getRoute());
 
-    // Set Menus on LCD
+    // Set Menus on LCD and set update time
     main_m->setLcd(lcdWrapper);
     mode_m->setLcd(lcdWrapper);
     pid_m->setLcd(lcdWrapper);
@@ -213,10 +220,13 @@ void makeMenu() {
     auto_m->setLcd(lcdWrapper);
     testM_m->setLcd(lcdWrapper);
     gps_m->setLcd(lcdWrapper);
+    gps_m->setUpdateDelay(1000);
     sys_m->setLcd(lcdWrapper);
+    sys_m->setUpdateDelay(1500);
     rout_m->setLcd(lcdWrapper);
 
 
+
     // Entry for the main menu
     MenuAction* mode_e = new MenuAction("Mode", mode_m);
     MenuAction* gps_e = new MenuAction("GPS", gps_m);