Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 01:35:35 +03:00
Code Issues Wiki Activity

Fix current BB logging and improve battery code

Browse source
This commit is contained in:
Michel Pastor 2018-07-01 17:39:18 +02:00
parent 7c301e67d4
commit e311f4ac1e
3 changed files with 35 additions and 56 deletions
Download patch file Download diff file Expand all files Collapse all files

6
src/main/blackbox/blackbox.c
View file

@ -1139,7 +1139,7 @@ void blackboxStart(void)
blackboxHistory[1] = &blackboxHistoryRing[1]; blackboxHistory[1] = &blackboxHistoryRing[1];
blackboxHistory[2] = &blackboxHistoryRing[2]; blackboxHistory[2] = &blackboxHistoryRing[2];
vbatReference = getBatteryVoltageLatestADC(); vbatReference = getBatteryRawVoltage();
//No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it //No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it
@ -1301,8 +1301,8 @@ static void loadMainState(timeUs_t currentTimeUs)
blackboxCurrent->motor[i] = motor[i]; blackboxCurrent->motor[i] = motor[i];
} }
blackboxCurrent->vbatLatest = getBatteryVoltageLatestADC(); blackboxCurrent->vbatLatest = getBatteryRawVoltage();
blackboxCurrent->amperageLatest = getAmperageLatestADC(); blackboxCurrent->amperageLatest = getAmperage();
#ifdef USE_BARO #ifdef USE_BARO
blackboxCurrent->BaroAlt = baro.BaroAlt; blackboxCurrent->BaroAlt = baro.BaroAlt;

83
src/main/sensors/battery.c
View file

@ -78,8 +78,6 @@ static bool batteryFullWhenPluggedIn = false;
static bool profileAutoswitchDisable = false; static bool profileAutoswitchDisable = false;
static uint16_t vbat = 0; // battery voltage in 0.1V steps (filtered) static uint16_t vbat = 0; // battery voltage in 0.1V steps (filtered)
static uint16_t vbatLatestADC = 0; // most recent unsmoothed raw reading from vbat ADC
static uint16_t amperageLatestADC = 0; // most recent raw reading from current ADC
static uint16_t powerSupplyImpedance = 0; // calculated impedance in milliohm static uint16_t powerSupplyImpedance = 0; // calculated impedance in milliohm
static uint16_t sagCompensatedVBat = 0; // calculated no load vbat static uint16_t sagCompensatedVBat = 0; // calculated no load vbat
static bool powerSupplyImpedanceIsValid = false; static bool powerSupplyImpedanceIsValid = false;
@ -137,19 +135,6 @@ PG_RESET_TEMPLATE(batteryMetersConfig_t, batteryMetersConfig,
); );
uint16_t batteryAdcToVoltage(uint16_t src)
{
// calculate battery voltage based on ADC reading
// result is Vbatt in 0.01V steps. 3.3V = ADC Vref, 0xFFF = 12bit adc, 1100 = 11:1 voltage divider (10k:1k)
return((uint64_t)src * batteryMetersConfig()->voltage_scale * ADCVREF / (0xFFF * 1000));
}
int16_t currentSensorToCentiamps(uint16_t src)
{
int32_t microvolts = ((uint32_t)src * ADCVREF * 100) / 0xFFF * 10 - (int32_t)batteryMetersConfig()->current.offset * 100;
return microvolts / batteryMetersConfig()->current.scale; // current in 0.01A steps
}
void batteryInit(void) void batteryInit(void)
{ {
batteryState = BATTERY_NOT_PRESENT; batteryState = BATTERY_NOT_PRESENT;
@ -184,9 +169,8 @@ static int8_t profileDetect() {
qsort(profile_comp_array, MAX_BATTERY_PROFILE_COUNT, sizeof(*profile_comp_array), (int (*)(const void *, const void *))profile_compare); qsort(profile_comp_array, MAX_BATTERY_PROFILE_COUNT, sizeof(*profile_comp_array), (int (*)(const void *, const void *))profile_compare);
// Return index of the first profile where vbat <= profile_max_voltage // Return index of the first profile where vbat <= profile_max_voltage
uint16_t vbatLatest = batteryAdcToVoltage(vbatLatestADC);
for (uint8_t i = 0; i < MAX_BATTERY_PROFILE_COUNT; ++i) for (uint8_t i = 0; i < MAX_BATTERY_PROFILE_COUNT; ++i)
if ((profile_comp_array[i].max_voltage > 0) && (vbatLatest <= profile_comp_array[i].max_voltage)) if ((profile_comp_array[i].max_voltage > 0) && (vbat <= profile_comp_array[i].max_voltage))
return profile_comp_array[i].profile_index; return profile_comp_array[i].profile_index;
// No matching profile found // No matching profile found
@ -206,24 +190,26 @@ void activateBatteryProfile(void)
batteryInit(); batteryInit();
} }
static void updateBatteryVoltage(timeUs_t timeDelta) static void updateBatteryVoltage(timeUs_t timeDelta, bool justConnected)
{ {
uint16_t vbatSample;
static pt1Filter_t vbatFilterState; static pt1Filter_t vbatFilterState;
// store the battery voltage with some other recent battery voltage readings // calculate battery voltage based on ADC reading
vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY); // result is Vbatt in 0.01V steps. 3.3V = ADC Vref, 0xFFF = 12bit adc, 1100 = 11:1 voltage divider (10k:1k)
vbatSample = pt1FilterApply4(&vbatFilterState, vbatSample, VBATT_LPF_FREQ, timeDelta * 1e-6f); vbat = (uint64_t)adcGetChannel(ADC_BATTERY) * batteryMetersConfig()->voltage_scale * ADCVREF / (0xFFF * 1000);
vbat = batteryAdcToVoltage(vbatSample);
if (justConnected) {
pt1FilterReset(&vbatFilterState, vbat);
} else {
vbat = pt1FilterApply4(&vbatFilterState, vbat, VBATT_LPF_FREQ, timeDelta * 1e-6f);
}
} }
void batteryUpdate(timeUs_t timeDelta) void batteryUpdate(timeUs_t timeDelta)
{ {
updateBatteryVoltage(timeDelta);
/* battery has just been connected*/ /* battery has just been connected*/
if (batteryState == BATTERY_NOT_PRESENT && vbat > VBATT_PRESENT_THRESHOLD) if (batteryState == BATTERY_NOT_PRESENT && vbat > VBATT_PRESENT_THRESHOLD) {
{
/* Actual battery state is calculated below, this is really BATTERY_PRESENT */ /* Actual battery state is calculated below, this is really BATTERY_PRESENT */
batteryState = BATTERY_OK; batteryState = BATTERY_OK;
/* wait for VBatt to stabilise then we can calc number of cells /* wait for VBatt to stabilise then we can calc number of cells
@ -231,7 +217,7 @@ void batteryUpdate(timeUs_t timeDelta)
We only do this on the ground so don't care if we do block, not We only do this on the ground so don't care if we do block, not
worse than original code anyway*/ worse than original code anyway*/
delay(VBATT_STABLE_DELAY); delay(VBATT_STABLE_DELAY);
updateBatteryVoltage(timeDelta); updateBatteryVoltage(timeDelta, true);
int8_t detectedProfileIndex = -1; int8_t detectedProfileIndex = -1;
if (feature(FEATURE_BAT_PROFILE_AUTOSWITCH) && (!profileAutoswitchDisable)) if (feature(FEATURE_BAT_PROFILE_AUTOSWITCH) && (!profileAutoswitchDisable))
@ -244,7 +230,7 @@ void batteryUpdate(timeUs_t timeDelta)
} else if (currentBatteryProfile->cells > 0) } else if (currentBatteryProfile->cells > 0)
batteryCellCount = currentBatteryProfile->cells; batteryCellCount = currentBatteryProfile->cells;
else { else {
batteryCellCount = (batteryAdcToVoltage(vbatLatestADC) / currentBatteryProfile->voltage.cellDetect) + 1; batteryCellCount = (vbat / currentBatteryProfile->voltage.cellDetect) + 1;
if (batteryCellCount > 8) batteryCellCount = 8; // something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells) if (batteryCellCount > 8) batteryCellCount = 8; // something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells)
} }
@ -252,17 +238,20 @@ void batteryUpdate(timeUs_t timeDelta)
batteryWarningVoltage = batteryCellCount * currentBatteryProfile->voltage.cellWarning; batteryWarningVoltage = batteryCellCount * currentBatteryProfile->voltage.cellWarning;
batteryCriticalVoltage = batteryCellCount * currentBatteryProfile->voltage.cellMin; batteryCriticalVoltage = batteryCellCount * currentBatteryProfile->voltage.cellMin;
batteryFullWhenPluggedIn = batteryAdcToVoltage(vbatLatestADC) >= (batteryFullVoltage - batteryCellCount * VBATT_CELL_FULL_MAX_DIFF); batteryFullWhenPluggedIn = vbat >= (batteryFullVoltage - batteryCellCount * VBATT_CELL_FULL_MAX_DIFF);
batteryUseCapacityThresholds = isAmperageConfigured() && batteryFullWhenPluggedIn && (currentBatteryProfile->capacity.value > 0) && batteryUseCapacityThresholds = isAmperageConfigured() && batteryFullWhenPluggedIn && (currentBatteryProfile->capacity.value > 0) &&
(currentBatteryProfile->capacity.warning > 0) && (currentBatteryProfile->capacity.critical > 0); (currentBatteryProfile->capacity.warning > 0) && (currentBatteryProfile->capacity.critical > 0);
} } else {
/* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of VBATT_PRESENT_THRESHOLD */ updateBatteryVoltage(timeDelta, false);
else if (batteryState != BATTERY_NOT_PRESENT && vbat <= VBATT_PRESENT_THRESHOLD) {
batteryState = BATTERY_NOT_PRESENT; /* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of VBATT_PRESENT_THRESHOLD */
batteryCellCount = 0; if (batteryState != BATTERY_NOT_PRESENT && vbat <= VBATT_PRESENT_THRESHOLD) {
batteryWarningVoltage = 0; batteryState = BATTERY_NOT_PRESENT;
batteryCriticalVoltage = 0; batteryCellCount = 0;
batteryWarningVoltage = 0;
batteryCriticalVoltage = 0;
}
} }
if (batteryState != BATTERY_NOT_PRESENT) { if (batteryState != BATTERY_NOT_PRESENT) {
@ -361,11 +350,6 @@ float calculateThrottleCompensationFactor(void)
return batteryFullVoltage / sagCompensatedVBat; return batteryFullVoltage / sagCompensatedVBat;
} }
uint16_t getBatteryVoltageLatestADC(void)
{
return vbatLatestADC;
}
uint16_t getBatteryWarningVoltage(void) uint16_t getBatteryWarningVoltage(void)
{ {
return batteryWarningVoltage; return batteryWarningVoltage;
@ -415,11 +399,6 @@ int16_t getAmperage(void)
return amperage; return amperage;
} }
int16_t getAmperageLatestADC(void)
{
return amperageLatestADC;
}
int32_t getPower(void) int32_t getPower(void)
{ {
return power; return power;
@ -443,10 +422,12 @@ void currentMeterUpdate(timeUs_t timeDelta)
switch (batteryMetersConfig()->current.type) { switch (batteryMetersConfig()->current.type) {
case CURRENT_SENSOR_ADC: case CURRENT_SENSOR_ADC:
amperageLatestADC = adcGetChannel(ADC_CURRENT); {
amperageLatestADC = pt1FilterApply4(&amperageFilterState, amperageLatestADC, AMPERAGE_LPF_FREQ, timeDelta * 1e-6f); int32_t microvolts = ((uint32_t)adcGetChannel(ADC_CURRENT) * ADCVREF * 100) / 0xFFF * 10 - (int32_t)batteryMetersConfig()->current.offset * 100;
amperage = currentSensorToCentiamps(amperageLatestADC); amperage = microvolts / batteryMetersConfig()->current.scale; // current in 0.01A steps
break; amperage = pt1FilterApply4(&amperageFilterState, amperage, AMPERAGE_LPF_FREQ, timeDelta * 1e-6f);
break;
}
case CURRENT_SENSOR_VIRTUAL: case CURRENT_SENSOR_VIRTUAL:
amperage = batteryMetersConfig()->current.offset; amperage = batteryMetersConfig()->current.offset;
if (ARMING_FLAG(ARMED)) { if (ARMING_FLAG(ARMED)) {

2
src/main/sensors/battery.h
View file

@ -125,7 +125,6 @@ bool isPowerSupplyImpedanceValid(void);
uint16_t getBatteryVoltage(void); uint16_t getBatteryVoltage(void);
uint16_t getBatteryRawVoltage(void); uint16_t getBatteryRawVoltage(void);
uint16_t getBatterySagCompensatedVoltage(void); uint16_t getBatterySagCompensatedVoltage(void);
uint16_t getBatteryVoltageLatestADC(void);
uint16_t getBatteryWarningVoltage(void); uint16_t getBatteryWarningVoltage(void);
uint8_t getBatteryCellCount(void); uint8_t getBatteryCellCount(void);
uint16_t getBatteryRawAverageCellVoltage(void); uint16_t getBatteryRawAverageCellVoltage(void);
@ -136,7 +135,6 @@ uint16_t getPowerSupplyImpedance(void);
bool isAmperageConfigured(void); bool isAmperageConfigured(void);
int16_t getAmperage(void); int16_t getAmperage(void);
int16_t getAmperageLatestADC(void);
int32_t getPower(void); int32_t getPower(void);
int32_t getMAhDrawn(void); int32_t getMAhDrawn(void);
int32_t getMWhDrawn(void); int32_t getMWhDrawn(void);