From 7958810e7dc9a54f29cd6e0a1c9b0840ca50eddd Mon Sep 17 00:00:00 2001 From: norbert-walter Date: Fri, 25 Mar 2022 18:19:14 +0100 Subject: [PATCH] Add moving average for battery values in OBPSensorTask --- lib/obp60task/OBPSensorTask.cpp | 51 +++++++++-- lib/obp60task/PageBattery.cpp | 158 +++++++++++++++++++++++++++----- lib/obp60task/Pagedata.h | 9 ++ lib/obp60task/config.json | 2 +- lib/obp60task/movingAvg.cpp | 67 ++++++++++++++ lib/obp60task/movingAvg.h | 29 ++++++ lib/obp60task/obp60task.h | 2 +- 7 files changed, 284 insertions(+), 34 deletions(-) create mode 100644 lib/obp60task/movingAvg.cpp create mode 100644 lib/obp60task/movingAvg.h diff --git a/lib/obp60task/OBPSensorTask.cpp b/lib/obp60task/OBPSensorTask.cpp index dff75c6..6ad776a 100644 --- a/lib/obp60task/OBPSensorTask.cpp +++ b/lib/obp60task/OBPSensorTask.cpp @@ -12,7 +12,8 @@ #include "N2kMessages.h" #include "NMEA0183.h" #include "ObpNmea0183.h" -#include "OBP60ExtensionPort.h" +#include "OBP60ExtensionPort.h" +#include "movingAvg.h" // Lib for moving average building // Timer Interrupts for hardware functions void underVoltageDetection(); @@ -79,6 +80,15 @@ void sensorTask(void *param){ bool AS5600_ready = false; // AS5600 initialized and ready to use bool INA226_1_ready = false; // INA226_1 initialized and ready to use + // Create integer arrays for average building + int avgsize = 300; + constexpr int arrayBatV{300}; + constexpr int arrayBatC{300}; + movingAvg batV(arrayBatV); + movingAvg batC(arrayBatC); + batV.begin(); + batC.begin(); + // Start timer interrupts bool uvoltage = api->getConfig()->getConfigItem(api->getConfig()->underVoltage,true)->asBoolean(); if(uvoltage == true){ @@ -210,7 +220,7 @@ void sensorTask(void *param){ String shunt1 = api->getConfig()->getConfigItem(api->getConfig()->shunt1, true)->asString(); float shuntResistor = 1.0; // Default value for shunt resistor - float current = 10.0; // Default value for max. current + float maxCurrent = 10.0; // Default value for max. current float corrFactor = 1; // Correction factor for fix calibration if(String(powsensor1) == "INA226"){ @@ -220,14 +230,19 @@ void sensorTask(void *param){ else{ api->getLogger()->logDebug(GwLog::LOG,"Modul 1 INA226 found"); shuntResistor = SHUNT_VOLTAGE / float(shunt1.toInt()); // Calculate shunt resisitor for max. shunt voltage 75mV - current = float(shunt1.toInt()); - api->getLogger()->logDebug(GwLog::LOG,"Calibation INA226, Imax:%3.0fA Rs:%7.5fOhm Us:%5.3f", current, shuntResistor, SHUNT_VOLTAGE); -// ina226_1.setMaxCurrentShunt(current, shuntResistor); + maxCurrent = float(shunt1.toInt()); + api->getLogger()->logDebug(GwLog::LOG,"Calibation INA226, Imax:%3.0fA Rs:%7.5fOhm Us:%5.3f", maxCurrent, shuntResistor, SHUNT_VOLTAGE); +// ina226_1.setMaxCurrentShunt(maxCurrent, shuntResistor); ina226_1.setMaxCurrentShunt(10, 0.01); // Calibration with fix values (because the original values outer range) - corrFactor = (current / 10) * (0.001 / shuntResistor) / (current / 100); // correction factor for fix calibration + corrFactor = (maxCurrent / 10) * (0.001 / shuntResistor) / (maxCurrent / 100); // Correction factor for fix calibration sensors.batteryVoltage = ina226_1.getBusVoltage(); sensors.batteryCurrent = ina226_1.getCurrent() * corrFactor; sensors.batteryPower = ina226_1.getPower() * corrFactor; + // Fill average arrays with start values + for (int i=1; i<=avgsize+1; ++i) { + batV.reading(int(sensors.batteryVoltage * 100)); + batC.reading(int(sensors.batteryCurrent * 10)); + } INA226_1_ready = true; } } @@ -394,12 +409,30 @@ void sensorTask(void *param){ if(String(powsensor1) == "INA226" && INA226_1_ready == true){ sensors.batteryVoltage = ina226_1.getBusVoltage(); sensors.batteryCurrent = ina226_1.getCurrent() * corrFactor; - sensors.batteryPower = ina226_1.getPower() * corrFactor; + // Eliminates bit jitter by zero current values + float factor = maxCurrent / 100; + if(sensors.batteryCurrent >= (-0.015 * factor) && sensors.batteryCurrent <= (0.015 * factor)){ + sensors.batteryCurrent = 0; + } + // Save actual values in average arrays as integer values + batV.reading(int(sensors.batteryVoltage * 100)); + batC.reading(int(sensors.batteryCurrent * 10)); + // Calculate the average values for different time lines from integer values + sensors.batteryVoltage10 = batV.getAvg(10) / 100.0; + sensors.batteryVoltage60 = batV.getAvg(60) / 100.0; + sensors.batteryVoltage300 = batV.getAvg(300) / 100.0; + sensors.batteryCurrent10 = batC.getAvg(10) / 10.0; + sensors.batteryCurrent60 = batC.getAvg(60) / 10.0; + sensors.batteryCurrent300 = batC.getAvg(300) / 10.0; + sensors.batteryPower10 = sensors.batteryVoltage10 * sensors.batteryCurrent10; + sensors.batteryPower60 = sensors.batteryVoltage60 * sensors.batteryCurrent60; + sensors.batteryPower300 = sensors.batteryVoltage300 * sensors.batteryCurrent300; +// sensors.batteryPower = ina226_1.getPower() * corrFactor; // Real value + sensors.batteryPower = sensors.batteryVoltage * sensors.batteryCurrent; // Calculated power value (more stable) } - // Send battery data to NMEA200 bus + // Send battery live data to NMEA200 bus if(!isnan(sensors.batteryVoltage) && !isnan(sensors.batteryCurrent)){ SetN2kDCBatStatus(N2kMsg, 0, sensors.batteryVoltage, sensors.batteryCurrent, N2kDoubleNA, 1); -// SetN2kDCBatStatus(N2kMsg, 0, sensors.batteryVoltage, sensors.batteryCurrent, sensors.batteryPower, 1); api->sendN2kMessage(N2kMsg); } } diff --git a/lib/obp60task/PageBattery.cpp b/lib/obp60task/PageBattery.cpp index 4aa46e7..6ef7105 100644 --- a/lib/obp60task/PageBattery.cpp +++ b/lib/obp60task/PageBattery.cpp @@ -6,6 +6,7 @@ class PageBattery : public Page { bool keylock = false; // Keylock + int average = 0; // Average type [0...3], 0=off, 1=10s, 2=60s, 3=300s public: PageBattery(CommonData &comon){ @@ -13,7 +14,15 @@ class PageBattery : public Page } virtual int handleKey(int key){ - if(key == 11){ // Code for keylock + // Change average + if(key == 1){ + average ++; + average = average % 4; // Modulo 4 + return 0; // Commit the key + } + + // Code for keylock + if(key == 11){ keylock = !keylock; // Toggle keylock return 0; // Commit the key } @@ -23,7 +32,7 @@ class PageBattery : public Page virtual void displayPage(CommonData &commonData, PageData &pageData){ GwConfigHandler *config = commonData.config; GwLog *logger=commonData.logger; - + // Old values for hold function double value1 = 0; static String svalue1old = ""; @@ -47,13 +56,27 @@ class PageBattery : public Page // Get voltage value String name1 = "BatVolt"; // Value name if(String(powsensor1) == "INA219" || String(powsensor1) == "INA226"){ - value1 = commonData.data.batteryVoltage; // Value as double in SI unit + // Switch average values + switch (average) { + case 0: + value1 = commonData.data.batteryVoltage; // Real data + break; + case 1: + value1 = commonData.data.batteryVoltage10; // Average 10s + break; + case 2: + value1 = commonData.data.batteryVoltage60; // Average 60s + break; + case 3: + value1 = commonData.data.batteryVoltage300; // Average 300s + break; + default: + value1 = commonData.data.batteryVoltage; // Default + break; + } } else{ - if(simulation == false){ - value1 = 0; // No sensor data - } - else{ + if(simulation == true){ value1 = 12 + float(random(0, 5)) / 10; // Simulation data } } @@ -63,13 +86,26 @@ class PageBattery : public Page // Get current value String name2 = "BatCurr"; // Value name if(String(powsensor1) == "INA219" || String(powsensor1) == "INA226"){ - value2 = commonData.data.batteryCurrent; // Value as double in SI unit + switch (average) { + case 0: + value2 = commonData.data.batteryCurrent; // Real data + break; + case 1: + value2 = commonData.data.batteryCurrent10; // Average 10s + break; + case 2: + value2 = commonData.data.batteryCurrent60; // Average 60s + break; + case 3: + value2 = commonData.data.batteryCurrent300; // Average 300s + break; + default: + value2 = commonData.data.batteryCurrent; // Default + break; + } } else{ - if(simulation == false){ - value2 = 0; // No sensor data - } - else{ + if(simulation == true){ value2 = 8 + float(random(0, 10)) / 10; // Simulation data } } @@ -79,13 +115,26 @@ class PageBattery : public Page // Get power value String name3 = "BatPow"; // Value name if(String(powsensor1) == "INA219" || String(powsensor1) == "INA226"){ - value3 = commonData.data.batteryPower; // Value as double in SI unit + switch (average) { + case 0: + value3 = commonData.data.batteryPower; // Real data + break; + case 1: + value3 = commonData.data.batteryPower10; // Average 10s + break; + case 2: + value3 = commonData.data.batteryPower60; // Average 60s + break; + case 3: + value3 = commonData.data.batteryPower300; // Average 300s + break; + default: + value3 = commonData.data.batteryPower; // Default + break; + } } else{ - if(simulation == false){ - value3 = 0; // No sensor data - } - else{ + if(simulation == true){ value3 = value1 * value2; // Simulation data } } @@ -99,7 +148,7 @@ class PageBattery : public Page } // Logging boat values - LOG_DEBUG(GwLog::LOG,"Drawing at PageBattery, %s: %f, %s: %f, %s: %f", name1, value1, name2, value2, name3, value3); + LOG_DEBUG(GwLog::LOG,"Drawing at PageBattery, %s: %f, %s: %f, %s: %f, Avg: %d", name1, value1, name2, value2, name3, value3, average); // Draw page //*********************************************************** @@ -120,6 +169,52 @@ class PageBattery : public Page } // Clear display by call in obp60task.cpp in main loop + // Show average settings + display.setTextColor(textcolor); + display.setFont(&Ubuntu_Bold8pt7b); + switch (average) { + case 0: + display.setCursor(60, 90); + display.print("Avg: 1s"); + display.setCursor(60, 180); + display.print("Avg: 1s"); + display.setCursor(60, 270); + display.print("Avg: 1s"); + break; + case 1: + display.setCursor(60, 90); + display.print("Avg: 10s"); + display.setCursor(60, 180); + display.print("Avg: 10s"); + display.setCursor(60, 270); + display.print("Avg: 10s"); + break; + case 2: + display.setCursor(60, 90); + display.print("Avg: 60s"); + display.setCursor(60, 180); + display.print("Avg: 60s"); + display.setCursor(60, 270); + display.print("Avg: 60s"); + break; + case 3: + display.setCursor(60, 90); + display.print("Avg: 300s"); + display.setCursor(60, 180); + display.print("Avg: 300s"); + display.setCursor(60, 270); + display.print("Avg: 300s"); + break; + default: + display.setCursor(60, 90); + display.print("Avg: 1s"); + display.setCursor(60, 180); + display.print("Avg: 1s"); + display.setCursor(60, 270); + display.print("Avg: 1s"); + break; + } + // ############### Value 1 ################ // Show name @@ -134,13 +229,17 @@ class PageBattery : public Page display.setCursor(20, 90); display.print(unit1); // Unit - // Show value display.setFont(&DSEG7Classic_BoldItalic30pt7b); display.setCursor(180, 90); // Show bus data - display.print(value1,2); // Real value as formated string + if(String(powsensor1) != "off"){ + display.print(value1,2); // Real value as formated string + } + else{ + display.print("---"); // No sensor data (sensor is off) + } // ############### Horizontal Line ################ @@ -166,7 +265,12 @@ class PageBattery : public Page display.setCursor(180, 180); // Show bus data - display.print(value2,1); // Real value as formated string + if(String(powsensor1) != "off"){ + display.print(value2,1); // Real value as formated string + } + else{ + display.print("---"); // No sensor data (sensor is off) + } // ############### Horizontal Line ################ @@ -192,7 +296,12 @@ class PageBattery : public Page display.setCursor(180, 270); // Show bus data - display.print(value3,1); // Real value as formated string + if(String(powsensor1) != "off"){ + display.print(value3,1); // Real value as formated string + } + else{ + display.print("---"); // No sensor data (sensor is off) + } // ############### Key Layout ################ @@ -200,8 +309,10 @@ class PageBattery : public Page // Key Layout display.setTextColor(textcolor); display.setFont(&Ubuntu_Bold8pt7b); - display.setCursor(130, 290); if(keylock == false){ + display.setCursor(10, 290); + display.print("[AVG]"); + display.setCursor(130, 290); display.print("[ <<<< " + String(commonData.data.actpage) + "/" + String(commonData.data.maxpage) + " >>>> ]"); if(String(backlightMode) == "Control by Key"){ // Key for illumination display.setCursor(343, 290); @@ -209,6 +320,7 @@ class PageBattery : public Page } } else{ + display.setCursor(130, 290); display.print(" [ Keylock active ]"); } diff --git a/lib/obp60task/Pagedata.h b/lib/obp60task/Pagedata.h index fc2b1ee..63bd44d 100644 --- a/lib/obp60task/Pagedata.h +++ b/lib/obp60task/Pagedata.h @@ -17,6 +17,15 @@ typedef struct{ double batteryVoltage = 0; double batteryCurrent = 0; double batteryPower = 0; + double batteryVoltage10 = 0; + double batteryCurrent10 = 0; + double batteryPower10 = 0; + double batteryVoltage60 = 0; + double batteryCurrent60 = 0; + double batteryPower60 = 0; + double batteryVoltage300 = 0; + double batteryCurrent300 = 0; + double batteryPower300 = 0; double solarVoltage = 0; double solarCurrent = 0; double solarPower = 0; diff --git a/lib/obp60task/config.json b/lib/obp60task/config.json index bd6c947..00fdfb8 100644 --- a/lib/obp60task/config.json +++ b/lib/obp60task/config.json @@ -351,7 +351,7 @@ }, { "name": "shunt3", - "label": "Gen. Sensor", + "label": "Gen. Shunt", "type": "list", "default": "10", "description": "Shunt current value [10A|50A|100A|200A|300A|400A|500A]", diff --git a/lib/obp60task/movingAvg.cpp b/lib/obp60task/movingAvg.cpp new file mode 100644 index 0000000..b9a46ab --- /dev/null +++ b/lib/obp60task/movingAvg.cpp @@ -0,0 +1,67 @@ +// Arduino Moving Average Library +// https://github.com/JChristensen/movingAvg +// Copyright (C) 2018 by Jack Christensen and licensed under +// GNU GPL v3.0, https://www.gnu.org/licenses/gpl.html + +#include + +// initialize - allocate the interval array +void movingAvg::begin() +{ + m_readings = new int[m_interval]; +} + +// add a new reading and return the new moving average +int movingAvg::reading(int newReading) +{ + // add each new data point to the sum until the m_readings array is filled + if (m_nbrReadings < m_interval) { + ++m_nbrReadings; + m_sum += newReading; + } + // once the array is filled, subtract the oldest data point and add the new one + else { + m_sum = m_sum - m_readings[m_next] + newReading; + } + + m_readings[m_next] = newReading; + if (++m_next >= m_interval) m_next = 0; + return (m_sum + m_nbrReadings / 2) / m_nbrReadings; +} + +// just return the current moving average +int movingAvg::getAvg() +{ + return (m_sum + m_nbrReadings / 2) / m_nbrReadings; +} + +// return the average for a subset of the data, the most recent nPoints readings. +// for invalid values of nPoints, return zero. +int movingAvg::getAvg(int nPoints) +{ + if (nPoints < 1 || nPoints > m_interval || nPoints > m_nbrReadings) { + return 0; + } + else { + long sum{0}; + int i = m_next; + for (int n=0; n