forked from Archive/PX4-Autopilot
Use discharged current to estimate remaining battery charge if capacity is known
This commit is contained in:
parent
1a318ee2a6
commit
6b085e8ced
|
@ -896,8 +896,9 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
/* only consider battery voltage if system has been running 2s and battery voltage is higher than 4V */
|
/* only consider battery voltage if system has been running 2s and battery voltage is higher than 4V */
|
||||||
if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 4.0f) {
|
if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 4.0f) {
|
||||||
status.battery_voltage = battery.voltage_v;
|
status.battery_voltage = battery.voltage_v;
|
||||||
|
status.battery_current = battery.current_a;
|
||||||
status.condition_battery_voltage_valid = true;
|
status.condition_battery_voltage_valid = true;
|
||||||
status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage);
|
status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_v, battery.discharged_mah);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -251,36 +251,44 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern)
|
||||||
ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern);
|
ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern);
|
||||||
}
|
}
|
||||||
|
|
||||||
float battery_remaining_estimate_voltage(float voltage)
|
float battery_remaining_estimate_voltage(float voltage, float discharged)
|
||||||
{
|
{
|
||||||
float ret = 0;
|
float ret = 0;
|
||||||
static param_t bat_volt_empty;
|
static param_t bat_v_empty_h;
|
||||||
static param_t bat_volt_full;
|
static param_t bat_v_full_h;
|
||||||
static param_t bat_n_cells;
|
static param_t bat_n_cells_h;
|
||||||
|
static param_t bat_capacity_h;
|
||||||
|
static float bat_v_empty = 3.2f;
|
||||||
|
static float bat_v_full = 4.0f;
|
||||||
|
static int bat_n_cells = 3;
|
||||||
|
static float bat_capacity = -1.0f;
|
||||||
static bool initialized = false;
|
static bool initialized = false;
|
||||||
static unsigned int counter = 0;
|
static unsigned int counter = 0;
|
||||||
static float ncells = 3;
|
|
||||||
// XXX change cells to int (and param to INT32)
|
|
||||||
|
|
||||||
if (!initialized) {
|
if (!initialized) {
|
||||||
bat_volt_empty = param_find("BAT_V_EMPTY");
|
bat_v_empty_h = param_find("BAT_V_EMPTY");
|
||||||
bat_volt_full = param_find("BAT_V_FULL");
|
bat_v_full_h = param_find("BAT_V_FULL");
|
||||||
bat_n_cells = param_find("BAT_N_CELLS");
|
bat_n_cells_h = param_find("BAT_N_CELLS");
|
||||||
|
bat_capacity_h = param_find("BAT_CAPACITY");
|
||||||
initialized = true;
|
initialized = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
static float chemistry_voltage_empty = 3.2f;
|
|
||||||
static float chemistry_voltage_full = 4.05f;
|
|
||||||
|
|
||||||
if (counter % 100 == 0) {
|
if (counter % 100 == 0) {
|
||||||
param_get(bat_volt_empty, &chemistry_voltage_empty);
|
param_get(bat_v_empty_h, &bat_v_empty);
|
||||||
param_get(bat_volt_full, &chemistry_voltage_full);
|
param_get(bat_v_full_h, &bat_v_full);
|
||||||
param_get(bat_n_cells, &ncells);
|
param_get(bat_n_cells_h, &bat_n_cells);
|
||||||
|
param_get(bat_capacity_h, &bat_capacity);
|
||||||
}
|
}
|
||||||
|
|
||||||
counter++;
|
counter++;
|
||||||
|
|
||||||
ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty));
|
if (bat_capacity > 0.0f) {
|
||||||
|
/* if battery capacity is known, use it to estimate remaining charge */
|
||||||
|
ret = 1.0f - discharged / bat_capacity;
|
||||||
|
} else {
|
||||||
|
/* else use voltage */
|
||||||
|
ret = (voltage - bat_n_cells * bat_v_empty) / (bat_n_cells * (bat_v_full - bat_v_empty));
|
||||||
|
}
|
||||||
|
|
||||||
/* limit to sane values */
|
/* limit to sane values */
|
||||||
ret = (ret < 0.0f) ? 0.0f : ret;
|
ret = (ret < 0.0f) ? 0.0f : ret;
|
||||||
|
|
|
@ -75,12 +75,13 @@ void rgbled_set_mode(rgbled_mode_t mode);
|
||||||
void rgbled_set_pattern(rgbled_pattern_t *pattern);
|
void rgbled_set_pattern(rgbled_pattern_t *pattern);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Provides a coarse estimate of remaining battery power.
|
* Estimate remaining battery charge.
|
||||||
*
|
*
|
||||||
* The estimate is very basic and based on decharging voltage curves.
|
* Use integral of current if battery capacity known (BAT_CAPACITY parameter set),
|
||||||
|
* else use simple estimate based on voltage.
|
||||||
*
|
*
|
||||||
* @return the estimated remaining capacity in 0..1
|
* @return the estimated remaining capacity in 0..1
|
||||||
*/
|
*/
|
||||||
float battery_remaining_estimate_voltage(float voltage);
|
float battery_remaining_estimate_voltage(float voltage, float discharged);
|
||||||
|
|
||||||
#endif /* COMMANDER_HELPER_H_ */
|
#endif /* COMMANDER_HELPER_H_ */
|
||||||
|
|
|
@ -52,4 +52,5 @@ PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
|
||||||
PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
|
PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
|
||||||
PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f);
|
PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f);
|
||||||
PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f);
|
PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f);
|
||||||
PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3);
|
PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
|
||||||
|
PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
|
||||||
|
|
Loading…
Reference in New Issue