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
This commit is contained in:
jonny_l480
parent
aab30abb80
commit
50ee4244d3
@ -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
|
||||
};
|
||||
|
||||
|
||||
@ -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<samples; j++){
|
||||
measure += adc1_get_raw(adc);
|
||||
ets_delay_us(50);
|
||||
}
|
||||
return (float)measure / samples / 4096 * 3.3;
|
||||
//return (float)measure / samples / 4096 * 3.3;
|
||||
return measure / samples;
|
||||
}
|
||||
|
||||
|
||||
@ -47,7 +48,6 @@ static displayStatusPage_t selectedStatusPage = STATUS_SCREEN_OVERVIEW;
|
||||
//======================
|
||||
//==== display_init ====
|
||||
//======================
|
||||
//note CONFIG_OFFSETX is used (from menuconfig)
|
||||
void display_init(display_config_t config){
|
||||
adc1_config_channel_atten(ADC1_CHANNEL_6, ADC_ATTEN_DB_11); //max voltage
|
||||
ESP_LOGI(TAG, "Initializing Display with config: sda=%d, sdl=%d, reset=%d, offset=%d, flip=%d, size: %dx%d",
|
||||
@ -123,7 +123,7 @@ void displayTextLineCentered(SSD1306_t *display, int line, bool isLarge, bool in
|
||||
|
||||
// add certain spaces around string (-> 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]);
|
||||
//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;
|
||||
}
|
||||
}
|
||||
ESP_LOGE(TAG, "getBatteryPercent - unknown voltage range");
|
||||
return 0.0; //unknown range
|
||||
}
|
||||
|
||||
|
||||
|
||||
//#############################
|
||||
//#### showScreen Overview ####
|
||||
//#############################
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -3,6 +3,7 @@ extern "C" {
|
||||
#include "esp_log.h"
|
||||
}
|
||||
|
||||
#include <math.h>
|
||||
#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;
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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;
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user