diff options
author | Julian Oes <julian@oes.ch> | 2013-07-16 09:35:31 +0200 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2013-07-16 09:35:31 +0200 |
commit | 08926019ea4203760a225e957d27328862182ce1 (patch) | |
tree | d4c38d4e286202fc3baac11438f16093e8becc15 /src/modules/commander/commander_helper.c | |
parent | 3e161049ac4e953f8c0084b1872b544de6189f5d (diff) | |
download | px4-firmware-08926019ea4203760a225e957d27328862182ce1.tar.gz px4-firmware-08926019ea4203760a225e957d27328862182ce1.tar.bz2 px4-firmware-08926019ea4203760a225e957d27328862182ce1.zip |
Just some reordering in commander
Diffstat (limited to 'src/modules/commander/commander_helper.c')
-rw-r--r-- | src/modules/commander/commander_helper.c | 43 |
1 files changed, 43 insertions, 0 deletions
diff --git a/src/modules/commander/commander_helper.c b/src/modules/commander/commander_helper.c index fb5c47885..199f73e6c 100644 --- a/src/modules/commander/commander_helper.c +++ b/src/modules/commander/commander_helper.c @@ -49,6 +49,7 @@ #include <uORB/topics/actuator_controls.h> #include <uORB/topics/vehicle_control_mode.h> #include <systemlib/err.h> +#include <systemlib/param/param.h> #include <drivers/drv_hrt.h> #include <drivers/drv_tone_alarm.h> #include <drivers/drv_led.h> @@ -172,4 +173,46 @@ int led_on(int led) int led_off(int led) { return ioctl(leds, LED_OFF, led); +} + + +PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f); +PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f); +PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3); + +float battery_remaining_estimate_voltage(float voltage) +{ + float ret = 0; + static param_t bat_volt_empty; + static param_t bat_volt_full; + static param_t bat_n_cells; + 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"); + 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); + } + + counter++; + + ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty)); + + /* limit to sane values */ + ret = (ret < 0) ? 0 : ret; + ret = (ret > 1) ? 1 : ret; + return ret; }
\ No newline at end of file |