mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-19 11:53:33 +08:00
commander: move battery calculations to systemlib
The commander used to consume the battery_status topic and write the contents after some calculations into the system state. Instead, the calculations now happen in library calls in systemlib/battery. This moves the battery fields out of the vehicle_status message into the battery_status topic. This brought quite some changes in all modules that need battery information. The current state is compiling but untested.
This commit is contained in:
Julian Oes
@@ -255,7 +255,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
||||
*/
|
||||
int commander_thread_main(int argc, char *argv[]);
|
||||
|
||||
void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed);
|
||||
void control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed,
|
||||
battery_status_s *battery);
|
||||
|
||||
void get_circuit_breaker_params();
|
||||
|
||||
@@ -1224,10 +1225,6 @@ int commander_thread_main(int argc, char *argv[])
|
||||
mavlink_and_console_log_critical(&mavlink_log_pub, "ERROR: BUZZER INIT FAIL");
|
||||
}
|
||||
|
||||
if (battery_init() != OK) {
|
||||
mavlink_and_console_log_critical(&mavlink_log_pub, "ERROR: BATTERY INIT FAIL");
|
||||
}
|
||||
|
||||
/* vehicle status topic */
|
||||
memset(&status, 0, sizeof(status));
|
||||
status.condition_landed = true; // initialize to safe value
|
||||
@@ -1255,10 +1252,6 @@ int commander_thread_main(int argc, char *argv[])
|
||||
status.offboard_control_signal_lost = true;
|
||||
status.data_link_lost = true;
|
||||
|
||||
/* set battery warning flag */
|
||||
battery_warning = vehicle_battery_warning::NONE;
|
||||
status.condition_battery_voltage_valid = false;
|
||||
|
||||
// XXX for now just set sensors as initialized
|
||||
status.condition_system_sensors_initialized = true;
|
||||
|
||||
@@ -1471,7 +1464,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||
vtol_status.vtol_in_rw_mode = true; //default for vtol is rotary wing
|
||||
|
||||
|
||||
control_status_leds(&status, &armed, true);
|
||||
control_status_leds(&status, &armed, true, &battery);
|
||||
|
||||
/* now initialized */
|
||||
commander_initialized = true;
|
||||
@@ -1967,20 +1960,37 @@ int commander_thread_main(int argc, char *argv[])
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
|
||||
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
|
||||
|
||||
/* only consider battery voltage if system has been running 2s and battery voltage is valid */
|
||||
if (hrt_absolute_time() > commander_boot_timestamp + 2000000 && battery.voltage_filtered_v > 0.0f) {
|
||||
status.battery_voltage = battery.voltage_filtered_v;
|
||||
status.battery_current = battery.current_a;
|
||||
status.battery_discharged_mah = battery.discharged_mah;
|
||||
status.condition_battery_voltage_valid = true;
|
||||
status.battery_cell_count = battery_get_n_cells();
|
||||
if (hrt_absolute_time() > commander_boot_timestamp + 2000000
|
||||
&& battery.voltage_filtered_v > FLT_EPSILON) {
|
||||
|
||||
/* get throttle (if armed), as we only care about energy negative throttle also counts */
|
||||
float throttle = (armed.armed) ? fabsf(actuator_controls.control[3]) : 0.0f;
|
||||
status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah,
|
||||
throttle);
|
||||
/* if battery voltage is getting lower, warn using buzzer, etc. */
|
||||
if (battery.warning == battery_status_s::BATTERY_WARNING_LOW &&
|
||||
!low_battery_voltage_actions_done) {
|
||||
low_battery_voltage_actions_done = true;
|
||||
if (armed.armed) {
|
||||
mavlink_log_critical(&mavlink_log_pub, "LOW BATTERY, RETURN TO LAND ADVISED");
|
||||
} else {
|
||||
mavlink_log_critical(&mavlink_log_pub, "LOW BATTERY, TAKEOFF DISCOURAGED");
|
||||
}
|
||||
|
||||
} else if (!status.usb_connected &&
|
||||
battery.warning == battery_status_s::BATTERY_WARNING_CRITICAL &&
|
||||
!critical_battery_voltage_actions_done &&
|
||||
low_battery_voltage_actions_done) {
|
||||
critical_battery_voltage_actions_done = true;
|
||||
|
||||
if (armed.armed) {
|
||||
mavlink_and_console_log_critical(&mavlink_log_pub, "CRITICAL BATTERY, SHUT SYSTEM DOWN");
|
||||
} else {
|
||||
mavlink_and_console_log_emergency(&mavlink_log_pub, "CRITICAL BATTERY, LAND IMMEDIATELY");
|
||||
}
|
||||
|
||||
status_changed = true;
|
||||
}
|
||||
|
||||
/* End battery voltage check */
|
||||
}
|
||||
}
|
||||
|
||||
@@ -2037,34 +2047,6 @@ int commander_thread_main(int argc, char *argv[])
|
||||
last_idle_time = system_load.tasks[0].total_runtime;
|
||||
}
|
||||
|
||||
/* if battery voltage is getting lower, warn using buzzer, etc. */
|
||||
if (status.condition_battery_voltage_valid && status.battery_remaining < 0.18f && !low_battery_voltage_actions_done) {
|
||||
low_battery_voltage_actions_done = true;
|
||||
if (armed.armed) {
|
||||
mavlink_log_critical(&mavlink_log_pub, "LOW BATTERY, RETURN TO LAND ADVISED");
|
||||
} else {
|
||||
mavlink_log_critical(&mavlink_log_pub, "LOW BATTERY, TAKEOFF DISCOURAGED");
|
||||
}
|
||||
battery_warning = vehicle_battery_warning::LOW;
|
||||
status_changed = true;
|
||||
|
||||
} else if (!status.usb_connected && status.condition_battery_voltage_valid && status.battery_remaining < 0.09f
|
||||
&& !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
|
||||
/* critical battery voltage, this is rather an emergency, change state machine */
|
||||
critical_battery_voltage_actions_done = true;
|
||||
battery_warning = vehicle_battery_warning::CRITICAL;
|
||||
|
||||
if (armed.armed) {
|
||||
mavlink_and_console_log_critical(&mavlink_log_pub, "CRITICAL BATTERY, SHUT SYSTEM DOWN");
|
||||
} else {
|
||||
mavlink_and_console_log_emergency(&mavlink_log_pub, "CRITICAL BATTERY, LAND IMMEDIATELY");
|
||||
}
|
||||
|
||||
status_changed = true;
|
||||
}
|
||||
|
||||
/* End battery voltage check */
|
||||
|
||||
/* If in INIT state, try to proceed to STANDBY state */
|
||||
if (!status.calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
|
||||
arming_ret = arming_state_transition(&status,
|
||||
@@ -2518,33 +2500,41 @@ int commander_thread_main(int argc, char *argv[])
|
||||
}
|
||||
}
|
||||
|
||||
/* Check engine failure
|
||||
* only for fixed wing for now
|
||||
*/
|
||||
if (!circuit_breaker_engaged_enginefailure_check &&
|
||||
status.is_rotary_wing == false &&
|
||||
armed.armed &&
|
||||
((actuator_controls.control[3] > ef_throttle_thres &&
|
||||
battery.current_a / actuator_controls.control[3] <
|
||||
ef_current2throttle_thres) ||
|
||||
(status.engine_failure))) {
|
||||
/* potential failure, measure time */
|
||||
if (timestamp_engine_healthy > 0 &&
|
||||
hrt_elapsed_time(×tamp_engine_healthy) >
|
||||
ef_time_thres * 1e6 &&
|
||||
!status.engine_failure) {
|
||||
status.engine_failure = true;
|
||||
status_changed = true;
|
||||
mavlink_log_critical(&mavlink_log_pub, "Engine Failure");
|
||||
}
|
||||
/* handle commands last, as the system needs to be updated to handle them */
|
||||
orb_check(actuator_controls_sub, &updated);
|
||||
|
||||
} else {
|
||||
/* no failure reset flag */
|
||||
timestamp_engine_healthy = hrt_absolute_time();
|
||||
if (updated) {
|
||||
/* got command */
|
||||
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
|
||||
|
||||
if (status.engine_failure) {
|
||||
status.engine_failure = false;
|
||||
status_changed = true;
|
||||
/* Check engine failure
|
||||
* only for fixed wing for now
|
||||
*/
|
||||
if (!circuit_breaker_engaged_enginefailure_check &&
|
||||
status.is_rotary_wing == false &&
|
||||
armed.armed &&
|
||||
((actuator_controls.control[3] > ef_throttle_thres &&
|
||||
battery.current_a / actuator_controls.control[3] <
|
||||
ef_current2throttle_thres) ||
|
||||
(status.engine_failure))) {
|
||||
/* potential failure, measure time */
|
||||
if (timestamp_engine_healthy > 0 &&
|
||||
hrt_elapsed_time(×tamp_engine_healthy) >
|
||||
ef_time_thres * 1e6 &&
|
||||
!status.engine_failure) {
|
||||
status.engine_failure = true;
|
||||
status_changed = true;
|
||||
mavlink_log_critical(mavlink_fd, "Engine Failure");
|
||||
}
|
||||
|
||||
} else {
|
||||
/* no failure reset flag */
|
||||
timestamp_engine_healthy = hrt_absolute_time();
|
||||
|
||||
if (status.engine_failure) {
|
||||
status.engine_failure = false;
|
||||
status_changed = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -2711,12 +2701,12 @@ int commander_thread_main(int argc, char *argv[])
|
||||
arm_tune_played = true;
|
||||
|
||||
} else if ((status.hil_state != vehicle_status_s::HIL_STATE_ON) &&
|
||||
battery_warning == vehicle_battery_warning::CRITICAL) {
|
||||
(battery.warning == battery_status_s::BATTERY_WARNING_CRITICAL)) {
|
||||
/* play tune on battery critical */
|
||||
set_tune(TONE_BATTERY_WARNING_FAST_TUNE);
|
||||
|
||||
} else if ((status.hil_state != vehicle_status_s::HIL_STATE_ON) &&
|
||||
(battery_warning == vehicle_battery_warning::LOW || status.failsafe)) {
|
||||
(battery.warning == battery_status_s::BATTERY_WARNING_LOW)) {
|
||||
/* play tune on battery warning or failsafe */
|
||||
set_tune(TONE_BATTERY_WARNING_SLOW_TUNE);
|
||||
|
||||
@@ -2758,12 +2748,12 @@ int commander_thread_main(int argc, char *argv[])
|
||||
/* blinking LED message, don't touch LEDs */
|
||||
if (blink_state == 2) {
|
||||
/* blinking LED message completed, restore normal state */
|
||||
control_status_leds(&status, &armed, true);
|
||||
control_status_leds(&status, &armed, true, &battery);
|
||||
}
|
||||
|
||||
} else {
|
||||
/* normal state */
|
||||
control_status_leds(&status, &armed, status_changed);
|
||||
control_status_leds(&status, &armed, status_changed, &battery);
|
||||
}
|
||||
|
||||
status_changed = false;
|
||||
@@ -2829,7 +2819,7 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val
|
||||
}
|
||||
|
||||
void
|
||||
control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed)
|
||||
control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed, battery_status_s *battery)
|
||||
{
|
||||
/* driving rgbled */
|
||||
if (changed) {
|
||||
@@ -2862,9 +2852,9 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
|
||||
/* set color */
|
||||
if (status_local->failsafe) {
|
||||
rgbled_set_color(RGBLED_COLOR_PURPLE);
|
||||
} else if (battery_warning == vehicle_battery_warning::LOW) {
|
||||
} else if (battery->warning == battery_status_s::BATTERY_WARNING_LOW) {
|
||||
rgbled_set_color(RGBLED_COLOR_AMBER);
|
||||
} else if (battery_warning == vehicle_battery_warning::CRITICAL) {
|
||||
} else if (battery->warning == battery_status_s::BATTERY_WARNING_CRITICAL) {
|
||||
rgbled_set_color(RGBLED_COLOR_RED);
|
||||
} else {
|
||||
if (status_local->condition_home_position_valid && status_local->condition_global_position_valid) {
|
||||
|
||||
@@ -105,30 +105,6 @@ static DevHandle h_leds;
|
||||
static DevHandle h_rgbleds;
|
||||
static DevHandle h_buzzer;
|
||||
|
||||
static param_t bat_v_empty_h;
|
||||
static param_t bat_v_full_h;
|
||||
static param_t bat_n_cells_h;
|
||||
static param_t bat_capacity_h;
|
||||
static param_t bat_v_load_drop_h;
|
||||
static float bat_v_empty = 3.4f;
|
||||
static float bat_v_full = 4.2f;
|
||||
static float bat_v_load_drop = 0.06f;
|
||||
static int bat_n_cells = 3;
|
||||
static float bat_capacity = -1.0f;
|
||||
static unsigned int counter = 0;
|
||||
static float throttle_lowpassed = 0.0f;
|
||||
|
||||
int battery_init()
|
||||
{
|
||||
bat_v_empty_h = param_find("BAT_V_EMPTY");
|
||||
bat_v_full_h = param_find("BAT_V_CHARGED");
|
||||
bat_n_cells_h = param_find("BAT_N_CELLS");
|
||||
bat_capacity_h = param_find("BAT_CAPACITY");
|
||||
bat_v_load_drop_h = param_find("BAT_V_LOAD_DROP");
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
int buzzer_init()
|
||||
{
|
||||
tune_end = 0;
|
||||
@@ -346,51 +322,3 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern)
|
||||
|
||||
h_rgbleds.ioctl(RGBLED_SET_PATTERN, (unsigned long)pattern);
|
||||
}
|
||||
|
||||
unsigned battery_get_n_cells() {
|
||||
return bat_n_cells;
|
||||
}
|
||||
|
||||
float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized)
|
||||
{
|
||||
float ret = 0;
|
||||
|
||||
if (counter % 100 == 0) {
|
||||
param_get(bat_v_empty_h, &bat_v_empty);
|
||||
param_get(bat_v_full_h, &bat_v_full);
|
||||
param_get(bat_v_load_drop_h, &bat_v_load_drop);
|
||||
param_get(bat_n_cells_h, &bat_n_cells);
|
||||
param_get(bat_capacity_h, &bat_capacity);
|
||||
}
|
||||
|
||||
counter++;
|
||||
|
||||
// XXX this time constant needs to become tunable
|
||||
// but really, the right fix are smart batteries.
|
||||
float val = throttle_lowpassed * 0.97f + throttle_normalized * 0.03f;
|
||||
if (PX4_ISFINITE(val)) {
|
||||
throttle_lowpassed = val;
|
||||
}
|
||||
|
||||
/* remaining charge estimate based on voltage and internal resistance (drop under load) */
|
||||
float bat_v_empty_dynamic = bat_v_empty - (bat_v_load_drop * throttle_lowpassed);
|
||||
/* the range from full to empty is the same for batteries under load and without load,
|
||||
* since the voltage drop applies to both the full and empty state
|
||||
*/
|
||||
float voltage_range = (bat_v_full - bat_v_empty);
|
||||
float remaining_voltage = (voltage - (bat_n_cells * bat_v_empty_dynamic)) / (bat_n_cells * voltage_range);
|
||||
|
||||
if (bat_capacity > 0.0f) {
|
||||
/* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */
|
||||
ret = fminf(remaining_voltage, 1.0f - discharged / bat_capacity);
|
||||
|
||||
} else {
|
||||
/* else use voltage */
|
||||
ret = remaining_voltage;
|
||||
}
|
||||
|
||||
/* limit to sane values */
|
||||
ret = (ret < 0.0f) ? 0.0f : ret;
|
||||
ret = (ret > 1.0f) ? 1.0f : ret;
|
||||
return ret;
|
||||
}
|
||||
|
||||
@@ -77,21 +77,4 @@ void rgbled_set_color(rgbled_color_t color);
|
||||
void rgbled_set_mode(rgbled_mode_t mode);
|
||||
void rgbled_set_pattern(rgbled_pattern_t *pattern);
|
||||
|
||||
int battery_init();
|
||||
|
||||
/**
|
||||
* Estimate remaining battery charge.
|
||||
*
|
||||
* Use integral of current if battery capacity known (BAT_CAPACITY parameter set),
|
||||
* else use simple estimate based on voltage.
|
||||
*
|
||||
* @param voltage the current battery voltage
|
||||
* @param discharged the discharged capacity
|
||||
* @param throttle_normalized the normalized throttle magnitude from 0 to 1. Negative throttle should be converted to this range as well, as it consumes energy.
|
||||
* @return the estimated remaining capacity in 0..1
|
||||
*/
|
||||
float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized);
|
||||
|
||||
unsigned battery_get_n_cells();
|
||||
|
||||
#endif /* COMMANDER_HELPER_H_ */
|
||||
|
||||
@@ -93,87 +93,7 @@ PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
|
||||
|
||||
/**
|
||||
* Empty cell voltage.
|
||||
*
|
||||
* Defines the voltage where a single cell of the battery is considered empty.
|
||||
*
|
||||
* @group Battery Calibration
|
||||
* @unit V
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f);
|
||||
|
||||
/**
|
||||
* Full cell voltage.
|
||||
*
|
||||
* Defines the voltage where a single cell of the battery is considered full.
|
||||
*
|
||||
* @group Battery Calibration
|
||||
* @unit V
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f);
|
||||
|
||||
/**
|
||||
* Voltage drop per cell on 100% load
|
||||
*
|
||||
* This implicitely defines the internal resistance
|
||||
* to maximum current ratio and assumes linearity.
|
||||
*
|
||||
* @group Battery Calibration
|
||||
* @unit V
|
||||
* @min 0.0
|
||||
* @max 1.5
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f);
|
||||
|
||||
/**
|
||||
* Number of cells.
|
||||
*
|
||||
* Defines the number of cells the attached battery consists of.
|
||||
*
|
||||
* @group Battery Calibration
|
||||
* @unit S
|
||||
* @min 2
|
||||
* @max 10
|
||||
* @value 2 2S Battery
|
||||
* @value 3 3S Battery
|
||||
* @value 4 4S Battery
|
||||
* @value 5 5S Battery
|
||||
* @value 6 6S Battery
|
||||
* @value 7 7S Battery
|
||||
* @value 8 8S Battery
|
||||
* @value 9 9S Battery
|
||||
* @value 10 10S Battery
|
||||
* @value 11 11S Battery
|
||||
* @value 12 12S Battery
|
||||
* @value 13 13S Battery
|
||||
* @value 14 14S Battery
|
||||
* @value 15 15S Battery
|
||||
* @value 16 16S Battery
|
||||
*/
|
||||
PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
|
||||
|
||||
/**
|
||||
* Battery capacity.
|
||||
*
|
||||
* Defines the capacity of the attached battery.
|
||||
*
|
||||
* @group Battery Calibration
|
||||
* @unit mA
|
||||
* @decimal 0
|
||||
* @min -1.0
|
||||
* @max 100000
|
||||
* @increment 50
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
|
||||
|
||||
/**
|
||||
* Datalink loss failsafe.
|
||||
* Datalink loss mode enabled.
|
||||
*
|
||||
* Set to 1 to enable actions triggered when the datalink is lost.
|
||||
*
|
||||
|
||||
Reference in New Issue
Block a user