From c2048ae949c1a8a0d85203bb2322bc5e8d7cd8b9 Mon Sep 17 00:00:00 2001 From: Alexander Klein Date: Wed, 26 Apr 2023 15:55:34 +0200 Subject: [PATCH] extend battery lib with messuared values and new calculation --- lib/Battery/battery.cpp | 82 ++++++++++++++++++++++++++++++++++++----- lib/Battery/battery.h | 45 ++++++++++++++++------ src/main.cpp | 34 +++++++++++------ 3 files changed, 129 insertions(+), 32 deletions(-) diff --git a/lib/Battery/battery.cpp b/lib/Battery/battery.cpp index 1bb1af5..2b701db 100644 --- a/lib/Battery/battery.cpp +++ b/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) { + this->pin = pin; + this->initBuffer(); +} + +bool Battery::loop() { if (millis() - this->lastMillis < this->delay) - return; - - this->calculateBatteryVoltage(); - this->calculateBatteryPercent(); + return false; + this->readAdcToBuf(); + this->loopCounter++; this->lastMillis = millis(); + + if (this->loopCounter == this->calulationDelayMultiplier) { + this->calculateBatteryVoltage(); + this->calculateBatteryPercent(); + return true; + 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); +} diff --git a/lib/Battery/battery.h b/lib/Battery/battery.h index bea1ec5..c4db57c 100644 --- a/lib/Battery/battery.h +++ b/lib/Battery/battery.h @@ -9,8 +9,8 @@ * */ -#ifndef AKKU_H -#define AKKU_H +#ifndef BATTERY_H +#define BATTERY_H #include #include @@ -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. + * @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; - uint32_t r1; - uint32_t r2; + uint8_t bufferPos = 0; + uint8_t delay = 100; + 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 diff --git a/src/main.cpp b/src/main.cpp index c5334e0..5d527ed 100644 --- a/src/main.cpp +++ b/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,18 +137,25 @@ void loop() { main_m->update(); lcdWrapper->loop(); - mainBattery->loop(); - if (mainBattery->isBatteryLow(10)) { - moveController.emergencyStop(); + bool newValues = mainBattery->loop(); - uint16_t voltage = (uint16_t) (mainBattery->getBatteryVoltage() * 100); + if (newValues) { + static uint8_t batteryLowCounter = 0; + if (mainBattery->isBatteryLow(10)) + batteryLowCounter++; + else + batteryLowCounter = 0; - lcd->setCursor(0, 0); - lcd->printf("Low Battery: %u", voltage); - lcd->setCursor(0, 1); - lcd->print("Please turn off."); - - while (true); + 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); + lcd->print("Please turn off."); + + while (true); + } } } @@ -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);