From 50ee4244d33c9aae95f6d9e58731e2fb830e6292 Mon Sep 17 00:00:00 2001 From: jonny_l480 Date: Tue, 27 Feb 2024 12:24:38 +0100 Subject: [PATCH] Fix bat voltage: lookup table, currentSensors: Add snap to zero display.cpp: Optimize battery voltage measurement While calibrating noticed it is very non-linear - outsource function to scale using lookup table - add lookup table to batvoltage measurement inserted many values to lookup table while testing currentsensors: Fix current value jumping around between 0 and 0.2 on display while standstill - add parameter snapToZeroThreshold --- board_single/main/config.cpp | 2 + board_single/main/display.cpp | 130 ++++++++++++++++++++++------------ board_single/main/display.hpp | 3 + common/currentsensor.cpp | 14 +++- common/currentsensor.hpp | 3 +- common/motorctl.cpp | 2 +- common/types.hpp | 1 + 7 files changed, 104 insertions(+), 51 deletions(-) diff --git a/board_single/main/config.cpp b/board_single/main/config.cpp index 9a7d777..c029fa5 100644 --- a/board_single/main/config.cpp +++ b/board_single/main/config.cpp @@ -98,6 +98,7 @@ motorctl_config_t configMotorControlLeft = { .currentSensor_ratedCurrent = 50, .currentMax = 30, .currentInverted = true, + .currentSnapToZeroThreshold = 0.15, .deadTimeMs = 0 // minimum time motor is off between direction change }; @@ -111,6 +112,7 @@ motorctl_config_t configMotorControlRight = { .currentSensor_ratedCurrent = 50, .currentMax = 30, .currentInverted = false, + .currentSnapToZeroThreshold = 0.25, .deadTimeMs = 0 // minimum time motor is off between direction change }; diff --git a/board_single/main/display.cpp b/board_single/main/display.cpp index e0c8a3a..c664e06 100644 --- a/board_single/main/display.cpp +++ b/board_single/main/display.cpp @@ -20,14 +20,15 @@ extern "C"{ //-------------------------- //TODO duplicate code: getVoltage also defined in currentsensor.cpp -> outsource this //local function to get average voltage from adc -float getVoltage1(adc1_channel_t adc, uint32_t samples){ +int readAdc(adc1_channel_t adc, uint32_t samples){ //measure voltage - int measure = 0; + uint32_t measure = 0; for (int j=0; j buf) snprintf(buf, MAX_LEN_NORMAL*2, "%*s%s%*s", numSpaces, "", tmp, maxLen - numSpaces - len, ""); - ESP_LOGD(TAG, "print center - isLarge=%d, value='%s', needed-spaces=%d, resulted-string='%s'", isLarge, tmp, numSpaces, buf); + ESP_LOGV(TAG, "print center - isLarge=%d, value='%s', needed-spaces=%d, resulted-string='%s'", isLarge, tmp, numSpaces, buf); // show line on display if (isLarge) @@ -134,62 +134,100 @@ void displayTextLineCentered(SSD1306_t *display, int line, bool isLarge, bool in -//---------------------------------- -//------- getBatteryVoltage -------- -//---------------------------------- -float getBatteryVoltage(){ -#define BAT_VOLTAGE_CONVERSION_FACTOR 11.9 - float voltageRead = getVoltage1(ADC_BATT_VOLTAGE, 1000); - float battVoltage = voltageRead * 11.9; //note: factor comes from simple test with voltmeter - ESP_LOGD(TAG, "batteryVoltage - voltageAdc=%f, voltageConv=%f, factor=%.2f", voltageRead, battVoltage, BAT_VOLTAGE_CONVERSION_FACTOR); - return battVoltage; +//================================= +//===== scaleUsingLookupTable ===== +//================================= +//scale/inpolate an input value to output value between several known points (two arrays) +//notes: the lookup values must be in ascending order. If the input value is lower/larger than smalles/largest value, output is set to first/last element of output elements +float scaleUsingLookupTable(const float lookupInput[], const float lookupOutput[], int count, float input){ + // check limit case (set to min/max) + if (input <= lookupInput[0]) { + ESP_LOGV(TAG, "lookup: %.2f is lower than lowest value -> returning min", input); + return lookupOutput[0]; + } else if (input >= lookupInput[count -1]) { + ESP_LOGV(TAG, "lookup: %.2f is larger than largest value -> returning max", input); + return lookupOutput[count -1]; + } + + // find best matching range and + // scale input linear to output in matched range + for (int i = 1; i < count; ++i) + { + if (input <= lookupInput[i]) //best match + { + float voltageRange = lookupInput[i] - lookupInput[i - 1]; + float voltageOffset = input - lookupInput[i - 1]; + float percentageRange = lookupOutput[i] - lookupOutput[i - 1]; + float percentageOffset = lookupOutput[i - 1]; + float output = percentageOffset + (voltageOffset / voltageRange) * percentageRange; + ESP_LOGV(TAG, "lookup: - input=%.3f => output=%.3f", input, output); + ESP_LOGV(TAG, "lookup - matched range: %.2fV-%.2fV => %.1f-%.1f", lookupInput[i - 1], lookupInput[i], lookupOutput[i - 1], lookupOutput[i]); + return output; + } + } + ESP_LOGE(TAG, "lookup - unknown range"); + return 0.0; //unknown range } +//================================== +//======= getBatteryVoltage ======== +//================================== +// apparently the ADC in combination with the added filter and voltage +// divider is slightly non-linear -> using lookup table +const float batteryAdcValues[] = {1732, 2418, 2509, 2600, 2753, 2853, 2889, 2909, 2936, 2951, 3005, 3068, 3090, 3122}; +const float batteryVoltages[] = {14.01, 20, 21, 22, 24, 25.47, 26, 26.4, 26.84, 27, 28, 29.05, 29.4, 30}; + +float getBatteryVoltage(){ + // check if lookup table is configured correctly + int countAdc = sizeof(batteryAdcValues) / sizeof(float); + int countVoltages = sizeof(batteryVoltages) / sizeof(float); + if (countAdc != countVoltages) + { + ESP_LOGE(TAG, "getBatteryVoltage - count of configured adc-values do not match count of voltages"); + return 0; + } + + //read adc + int adcRead = readAdc(ADC_BATT_VOLTAGE, 1000); + + //convert adc to voltage using lookup table + float battVoltage = scaleUsingLookupTable(batteryAdcValues, batteryVoltages, countAdc, adcRead); + ESP_LOGD(TAG, "batteryVoltage - adcRaw=%d => voltage=%.3f, scaled using lookuptable with %d elements", adcRead, battVoltage, countAdc); + return battVoltage; +} + //---------------------------------- //------- getBatteryPercent -------- //---------------------------------- -//TODO find better/more accurate table? -//configure discharge curve of one cell with corresponding known voltage->chargePercent values -const float voltageLevels[] = {3.00, 3.45, 3.68, 3.74, 3.77, 3.79, 3.82, 3.87, 3.92, 3.98, 4.06, 4.20}; -const float percentageLevels[] = {0.0, 5.0, 10.0, 20.0, 30.0, 40.0, 50.0, 60.0, 70.0, 80.0, 90.0, 100.0}; +// TODO find better/more accurate table? +// configure discharge curve of one cell with corresponding known voltage->chargePercent values +const float cellVoltageLevels[] = {3.00, 3.45, 3.68, 3.74, 3.77, 3.79, 3.82, 3.87, 3.92, 3.98, 4.06, 4.20}; +const float cellPercentageLevels[] = {0.0, 5.0, 10.0, 20.0, 30.0, 40.0, 50.0, 60.0, 70.0, 80.0, 90.0, 100.0}; -float getBatteryPercent(){ - float voltage = getBatteryVoltage(); - float cellVoltage = voltage/BAT_CELL_COUNT; - int size = sizeof(voltageLevels) / sizeof(voltageLevels[0]); - int sizePer = sizeof(percentageLevels) / sizeof(percentageLevels[0]); - //check if configured correctly - if (size != sizePer) { +float getBatteryPercent() +{ + // check if lookup table is configured correctly + int sizeVoltage = sizeof(cellVoltageLevels) / sizeof(cellVoltageLevels[0]); + int sizePer = sizeof(cellPercentageLevels) / sizeof(cellPercentageLevels[0]); + if (sizeVoltage != sizePer) + { ESP_LOGE(TAG, "getBatteryPercent - count of configured percentages do not match count of voltages"); return 0; } - if (cellVoltage <= voltageLevels[0]) { - return 0.0; - } else if (cellVoltage >= voltageLevels[size - 1]) { - return 100.0; - } - //scale voltage linear to percent in matched range - for (int i = 1; i < size; ++i) { - if (cellVoltage <= voltageLevels[i]) { - float voltageRange = voltageLevels[i] - voltageLevels[i - 1]; - float voltageOffset = cellVoltage - voltageLevels[i - 1]; - float percentageRange = percentageLevels[i] - percentageLevels[i - 1]; - float percentageOffset = percentageLevels[i - 1]; - float percent = percentageOffset + (voltageOffset / voltageRange) * percentageRange; - ESP_LOGD(TAG, "getBatPercent - cellVoltage=%.3f => percentage=%.3f", cellVoltage, percent); - ESP_LOGD(TAG, "getBatPercent - matched range: %.2fV-%.2fV => %.1f%%-%.1f%%", voltageLevels[i-1], voltageLevels[i], percentageLevels[i-1], percentageLevels[i]); - return percent; - } - } - ESP_LOGE(TAG, "getBatteryPercent - unknown voltage range"); - return 0.0; //unknown range + //get current battery voltage + float voltage = getBatteryVoltage(); + float cellVoltage = voltage / BAT_CELL_COUNT; + + //convert voltage to battery percentage using lookup table + float percent = scaleUsingLookupTable(cellVoltageLevels, cellPercentageLevels, sizeVoltage, cellVoltage); + ESP_LOGD(TAG, "batteryPercentage - Battery=%.3fV, Cell=%.3fV => percentage=%.3f, scaled using lookuptable with %d elements", voltage, cellVoltage, percent, sizePer); + return percent; } - //############################# //#### showScreen Overview #### //############################# diff --git a/board_single/main/display.hpp b/board_single/main/display.hpp index 6b64caa..13c554a 100644 --- a/board_single/main/display.hpp +++ b/board_single/main/display.hpp @@ -51,6 +51,9 @@ typedef struct display_task_parameters_t { // enum for selecting the currently shown status page (display content when not in MENU mode) typedef enum displayStatusPage_t {STATUS_SCREEN_OVERVIEW=0, STATUS_SCREEN_SPEED, STATUS_SCREEN_JOYSTICK, STATUS_SCREEN_MOTORS} displayStatusPage_t; +// get precise battery voltage (using lookup table) +float getBatteryVoltage(); + // function to select one of the defined status screens which are shown on display when not in MENU mode void display_selectStatusPage(displayStatusPage_t newStatusPage); diff --git a/common/currentsensor.cpp b/common/currentsensor.cpp index 3fd3a95..359a73b 100644 --- a/common/currentsensor.cpp +++ b/common/currentsensor.cpp @@ -3,6 +3,7 @@ extern "C" { #include "esp_log.h" } +#include #include "currentsensor.hpp" //tag for logging @@ -29,11 +30,12 @@ float getVoltage(adc1_channel_t adc, uint32_t samples){ //============================= //======== constructor ======== //============================= -currentSensor::currentSensor (adc1_channel_t adcChannel_f, float ratedCurrent_f, bool isInverted_f){ +currentSensor::currentSensor (adc1_channel_t adcChannel_f, float ratedCurrent_f, float snapToZeroThreshold_f, bool isInverted_f){ //copy config adcChannel = adcChannel_f; ratedCurrent = ratedCurrent_f; isInverted = isInverted_f; + snapToZeroThreshold = snapToZeroThreshold_f; //init adc adc1_config_width(ADC_WIDTH_BIT_12); //max resolution 4096 adc1_config_channel_atten(adcChannel, ADC_ATTEN_DB_11); //max voltage @@ -59,8 +61,14 @@ float currentSensor::read(void){ current = 0; } - //invert calculated current if necessary - if (isInverted) current = -current; + if (fabs(current) < snapToZeroThreshold) + { + ESP_LOGD(TAG, "current=%.3f < threshold=%.3f -> snap to 0", current, snapToZeroThreshold); + current = 0; + } + // invert calculated current if necessary + else if (isInverted) + current = -current; ESP_LOGI(TAG, "read sensor adc=%d: voltage=%.3fV, centerVoltage=%.3fV => current=%.3fA", (int)adcChannel, voltage, centerVoltage, current); return current; diff --git a/common/currentsensor.hpp b/common/currentsensor.hpp index 3110512..d24eb3d 100644 --- a/common/currentsensor.hpp +++ b/common/currentsensor.hpp @@ -7,13 +7,14 @@ class currentSensor{ public: - currentSensor (adc1_channel_t adcChannel_f, float ratedCurrent, bool inverted = false); + currentSensor (adc1_channel_t adcChannel_f, float ratedCurrent, float snapToZeroThreshold, bool inverted = false); void calibrateZeroAmpere(void); //set current voltage to voltage representing 0A float read(void); //get current ampere private: adc1_channel_t adcChannel; float ratedCurrent; bool isInverted; + float snapToZeroThreshold; uint32_t measure; float voltage; float current; diff --git a/common/motorctl.cpp b/common/motorctl.cpp index f437417..38fc804 100644 --- a/common/motorctl.cpp +++ b/common/motorctl.cpp @@ -29,7 +29,7 @@ void task_motorctl( void * ptrControlledMotor ){ //============================= //constructor, simultaniously initialize instance of motor driver 'motor' and current sensor 'cSensor' with provided config (see below lines after ':') controlledMotor::controlledMotor(motorSetCommandFunc_t setCommandFunc, motorctl_config_t config_control, nvs_handle_t * nvsHandle_f): - cSensor(config_control.currentSensor_adc, config_control.currentSensor_ratedCurrent, config_control.currentInverted) { + cSensor(config_control.currentSensor_adc, config_control.currentSensor_ratedCurrent, config_control.currentSnapToZeroThreshold, config_control.currentInverted) { //copy parameters for controlling the motor config = config_control; //pointer to update motot dury method diff --git a/common/types.hpp b/common/types.hpp index dcc9480..45d7f80 100644 --- a/common/types.hpp +++ b/common/types.hpp @@ -49,6 +49,7 @@ typedef struct motorctl_config_t { float currentSensor_ratedCurrent; float currentMax; bool currentInverted; + float currentSnapToZeroThreshold; uint32_t deadTimeMs; //time motor stays in IDLE before direction change } motorctl_config_t;