diff options
Diffstat (limited to 'src/modules/commander')
-rw-r--r-- | src/modules/commander/commander.cpp | 65 | ||||
-rw-r--r-- | src/modules/commander/commander_helper.cpp | 44 | ||||
-rw-r--r-- | src/modules/commander/commander_helper.h | 7 | ||||
-rw-r--r-- | src/modules/commander/commander_params.c | 7 |
4 files changed, 55 insertions, 68 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0357542f0..add7312de 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -107,14 +107,9 @@ static const int ERROR = -1; extern struct system_load_s system_load; -#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f -#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f - /* Decouple update interval and hysteris counters, all depends on intervals */ #define COMMANDER_MONITORING_INTERVAL 50000 #define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f)) -#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) #define MAVLINK_OPEN_INTERVAL 50000 @@ -666,8 +661,6 @@ int commander_thread_main(int argc, char *argv[]) /* Start monitoring loop */ unsigned counter = 0; - unsigned low_voltage_counter = 0; - unsigned critical_voltage_counter = 0; unsigned stick_off_counter = 0; unsigned stick_on_counter = 0; @@ -745,7 +738,6 @@ int commander_thread_main(int argc, char *argv[]) int battery_sub = orb_subscribe(ORB_ID(battery_status)); struct battery_status_s battery; memset(&battery, 0, sizeof(battery)); - battery.voltage_v = 0.0f; /* Subscribe to subsystem info topic */ int subsys_sub = orb_subscribe(ORB_ID(subsystem_info)); @@ -890,14 +882,12 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); - - // warnx("bat v: %2.2f", battery.voltage_v); - - /* 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) { - status.battery_voltage = battery.voltage_v; + /* only consider battery voltage if system has been running 2s and battery voltage is valid */ + if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) { + status.battery_voltage = battery.voltage_filtered_v; + status.battery_current = battery.current_a; 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_filtered_v, battery.discharged_mah); } } @@ -950,46 +940,29 @@ int commander_thread_main(int argc, char *argv[]) //on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0); } - // XXX remove later - //warnx("bat remaining: %2.2f", status.battery_remaining); - /* if battery voltage is getting lower, warn using buzzer, etc. */ if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) { - //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS - if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { - low_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY"); - status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; - status_changed = true; - battery_tune_played = false; - } - - low_voltage_counter++; + low_battery_voltage_actions_done = true; + mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY"); + status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; + status_changed = true; + battery_tune_played = false; } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { /* critical battery voltage, this is rather an emergency, change state machine */ - if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { - critical_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY"); - status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; - battery_tune_played = false; - - if (armed.armed) { - arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed); + critical_battery_voltage_actions_done = true; + mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY"); + status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; + battery_tune_played = false; - } else { - arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed); - } + if (armed.armed) { + arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed); - status_changed = true; + } else { + arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed); } - critical_voltage_counter++; - - } else { - - low_voltage_counter = 0; - critical_voltage_counter = 0; + status_changed = true; } /* End battery voltage check */ diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 565b4b66a..21a1c4c2c 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -44,6 +44,7 @@ #include <stdint.h> #include <stdbool.h> #include <fcntl.h> +#include <math.h> #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> @@ -251,36 +252,47 @@ void rgbled_set_pattern(rgbled_pattern_t *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; - static param_t bat_volt_empty; - static param_t bat_volt_full; - static param_t bat_n_cells; + 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 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 unsigned int counter = 0; - static float ncells = 3; - // XXX change cells to int (and param to INT32) if (!initialized) { - bat_volt_empty = param_find("BAT_V_EMPTY"); - bat_volt_full = param_find("BAT_V_FULL"); - bat_n_cells = param_find("BAT_N_CELLS"); + bat_v_empty_h = param_find("BAT_V_EMPTY"); + bat_v_full_h = param_find("BAT_V_FULL"); + bat_n_cells_h = param_find("BAT_N_CELLS"); + bat_capacity_h = param_find("BAT_CAPACITY"); initialized = true; } - static float chemistry_voltage_empty = 3.2f; - static float chemistry_voltage_full = 4.05f; - if (counter % 100 == 0) { - param_get(bat_volt_empty, &chemistry_voltage_empty); - param_get(bat_volt_full, &chemistry_voltage_full); - param_get(bat_n_cells, &ncells); + param_get(bat_v_empty_h, &bat_v_empty); + param_get(bat_v_full_h, &bat_v_full); + param_get(bat_n_cells_h, &bat_n_cells); + param_get(bat_capacity_h, &bat_capacity); } counter++; - ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty)); + /* remaining charge estimate based on voltage */ + float remaining_voltage = (voltage - bat_n_cells * bat_v_empty) / (bat_n_cells * (bat_v_full - bat_v_empty)); + + 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; diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index e9514446c..d0393f45a 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -75,12 +75,13 @@ void rgbled_set_mode(rgbled_mode_t mode); 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 */ -float battery_remaining_estimate_voltage(float voltage); +float battery_remaining_estimate_voltage(float voltage, float discharged); #endif /* COMMANDER_HELPER_H_ */ diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 40d0386d5..e10b7f18d 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -50,6 +50,7 @@ PARAM_DEFINE_FLOAT(NAV_TAKEOFF_GAP, 3.0f); PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); -PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f); -PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f); -PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3); +PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); +PARAM_DEFINE_FLOAT(BAT_V_FULL, 3.9f); +PARAM_DEFINE_INT32(BAT_N_CELLS, 3); +PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); |