diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-21 20:51:34 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-21 20:51:34 +0200 |
commit | 5bb3e92d362c04ac57537a0ddaa515781076161e (patch) | |
tree | e673235d4b0c7a91420e609ae1bb18466feb8790 | |
parent | 2685e3cfa4fdcbe80e9fd89cc3b08ffafccebad7 (diff) | |
parent | 07d11583bb0926ac9fe1cf2f1c7e11c683606a4b (diff) | |
download | px4-firmware-5bb3e92d362c04ac57537a0ddaa515781076161e.tar.gz px4-firmware-5bb3e92d362c04ac57537a0ddaa515781076161e.tar.bz2 px4-firmware-5bb3e92d362c04ac57537a0ddaa515781076161e.zip |
Merge pull request #1211 from PX4/battload
Consider the throttle load for battery voltage calculation
-rw-r--r-- | Tools/tests-host/data/fit_linear_voltage.m | 14 | ||||
-rw-r--r-- | Tools/tests-host/data/px4io_v1.3.csv | 70 | ||||
-rw-r--r-- | src/modules/commander/commander.cpp | 15 | ||||
-rw-r--r-- | src/modules/commander/commander_helper.cpp | 17 | ||||
-rw-r--r-- | src/modules/commander/commander_helper.h | 3 | ||||
-rw-r--r-- | src/modules/commander/commander_params.c | 12 | ||||
-rw-r--r-- | src/modules/px4iofirmware/registers.c | 25 |
7 files changed, 127 insertions, 29 deletions
diff --git a/Tools/tests-host/data/fit_linear_voltage.m b/Tools/tests-host/data/fit_linear_voltage.m new file mode 100644 index 000000000..7d0c2c27f --- /dev/null +++ b/Tools/tests-host/data/fit_linear_voltage.m @@ -0,0 +1,14 @@ +close all; +clear all; +M = importdata('px4io_v1.3.csv'); +voltage = M.data(:, 1); +counts = M.data(:, 2); +plot(counts, voltage, 'b*-', 'LineWidth', 2, 'MarkerSize', 15); +coeffs = polyfit(counts, voltage, 1); +fittedC = linspace(min(counts), max(counts), 500); +fittedV = polyval(coeffs, fittedC); +hold on +plot(fittedC, fittedV, 'r-', 'LineWidth', 3); + +slope = coeffs(1) +y_intersection = coeffs(2) diff --git a/Tools/tests-host/data/px4io_v1.3.csv b/Tools/tests-host/data/px4io_v1.3.csv new file mode 100644 index 000000000..b41ee8f1f --- /dev/null +++ b/Tools/tests-host/data/px4io_v1.3.csv @@ -0,0 +1,70 @@ +voltage, counts +4.3, 950 +4.4, 964 +4.5, 986 +4.6, 1009 +4.7, 1032 +4.8, 1055 +4.9, 1078 +5.0, 1101 +5.2, 1124 +5.3, 1148 +5.4, 1171 +5.5, 1195 +6.0, 1304 +6.1, 1329 +6.2, 1352 +7.0, 1517 +7.1, 1540 +7.2, 1564 +7.3, 1585 +7.4, 1610 +7.5, 1636 +8.0, 1728 +8.1, 1752 +8.2, 1755 +8.3, 1798 +8.4, 1821 +9.0, 1963 +9.1, 1987 +9.3, 2010 +9.4, 2033 +10.0, 2174 +10.1, 2198 +10.2, 2221 +10.3, 2245 +10.4, 2268 +11.0, 2385 +11.1, 2409 +11.2, 2432 +11.3, 2456 +11.4, 2480 +11.5, 2502 +11.6, 2526 +11.7, 2550 +11.8, 2573 +11.9, 2597 +12.0, 2621 +12.1, 2644 +12.3, 2668 +12.4, 2692 +12.5, 2716 +12.6, 2737 +12.7, 2761 +13.0, 2832 +13.5, 2950 +13.6, 2973 +14.1, 3068 +14.2, 3091 +14.7, 3209 +15.0, 3280 +15.1, 3304 +15.5, 3374 +15.6, 3397 +15.7, 3420 +16.0, 3492 +16.1, 3514 +16.2, 3538 +16.9, 3680 +17.0, 3704 +17.1, 3728 diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 48cbc254f..a0c896178 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -913,6 +913,11 @@ int commander_thread_main(int argc, char *argv[]) struct system_power_s system_power; memset(&system_power, 0, sizeof(system_power)); + /* Subscribe to actuator controls (outputs) */ + int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + struct actuator_controls_s actuator_controls; + memset(&actuator_controls, 0, sizeof(actuator_controls)); + control_status_leds(&status, &armed, true); /* now initialized */ @@ -1194,13 +1199,17 @@ 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() > 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(battery.voltage_filtered_v, battery.discharged_mah); + + /* 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); } } @@ -1262,13 +1271,13 @@ int commander_thread_main(int argc, char *argv[]) } /* 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) { + if (status.condition_battery_voltage_valid && status.battery_remaining < 0.18f && !low_battery_voltage_actions_done) { low_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED"); status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status_changed = true; - } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { + } else if (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; mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY"); diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index d5fe122cb..2022e99fb 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -281,15 +281,17 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern) } } -float battery_remaining_estimate_voltage(float voltage, float discharged) +float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized) { float ret = 0; 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 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 bool initialized = false; @@ -297,23 +299,26 @@ float battery_remaining_estimate_voltage(float voltage, float discharged) if (!initialized) { bat_v_empty_h = param_find("BAT_V_EMPTY"); - bat_v_full_h = param_find("BAT_V_FULL"); + 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"); initialized = true; } 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++; - /* 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)); + /* remaining charge estimate based on voltage and internal resistance (drop under load) */ + float bat_v_full_dynamic = bat_v_full - (bat_v_load_drop * throttle_normalized); + float remaining_voltage = (voltage - (bat_n_cells * bat_v_empty)) / (bat_n_cells * (bat_v_full_dynamic - 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 */ diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index a49c9e263..4a77fe487 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -80,8 +80,9 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern); * * @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 battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized); #endif /* COMMANDER_HELPER_H_ */ diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 4750f9d5c..dba68700b 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -65,7 +65,17 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); * * @group Battery Calibration */ -PARAM_DEFINE_FLOAT(BAT_V_FULL, 3.9f); +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 + */ +PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f); /** * Number of cells. diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index fcd53f1f1..43161aa70 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -739,30 +739,19 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val { /* * Coefficients here derived by measurement of the 5-16V - * range on one unit: + * range on one unit, validated on sample points of another unit * - * V counts - * 5 1001 - * 6 1219 - * 7 1436 - * 8 1653 - * 9 1870 - * 10 2086 - * 11 2303 - * 12 2522 - * 13 2738 - * 14 2956 - * 15 3172 - * 16 3389 + * Data in Tools/tests-host/data folder. * - * slope = 0.0046067 - * intercept = 0.3863 + * measured slope = 0.004585267878277 (int: 4585) + * nominal theoretic slope: 0.00459340659 (int: 4593) + * intercept = 0.016646394188076 (int: 16646) + * nominal theoretic intercept: 0.00 (int: 0) * - * Intercept corrected for best results @ 12V. */ unsigned counts = adc_measure(ADC_VBATT); if (counts != 0xffff) { - unsigned mV = (4150 + (counts * 46)) / 10 - 200; + unsigned mV = (166460 + (counts * 45934)) / 10000; unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000; r_page_status[PX4IO_P_STATUS_VBATT] = corrected; |