From 8ebf9755e44c45a957d51f1a75eb102bf1868c58 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Mar 2015 13:54:05 -0700 Subject: commander: Fix battery config --- src/modules/commander/commander.cpp | 10 +++++-- src/modules/commander/commander_helper.cpp | 44 ++++++++++++++++-------------- src/modules/commander/commander_helper.h | 2 ++ 3 files changed, 32 insertions(+), 24 deletions(-) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 35f538b23..21f1eacec 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -865,12 +865,16 @@ int commander_thread_main(int argc, char *argv[]) pthread_t commander_low_prio_thread; /* initialize */ - if (led_init() != 0) { - warnx("ERROR: LED INIT FAIL"); + if (led_init() != OK) { + mavlink_and_console_log_critical(mavlink_fd, "ERROR: LED INIT FAIL"); } if (buzzer_init() != OK) { - warnx("ERROR: BUZZER INIT FAIL"); + mavlink_and_console_log_critical(mavlink_fd, "ERROR: BUZZER INIT FAIL"); + } + + if (battery_init() != OK) { + mavlink_and_console_log_critical(mavlink_fd, "ERROR: BATTERY INIT FAIL"); } mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index a2e827a15..a5e4d1972 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -90,6 +90,29 @@ static hrt_abstime tune_end = 0; // end time of currently played tune, 0 for re static int tune_current = TONE_STOP_TUNE; // currently playing tune, can be interrupted after tune_end static unsigned int tune_durations[TONE_NUMBER_OF_TUNES]; +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; + +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 OK; +} + int buzzer_init() { tune_end = 0; @@ -303,27 +326,6 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern) 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 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; - static unsigned int counter = 0; - - if (!initialized) { - 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"); - initialized = true; - } if (counter % 100 == 0) { param_get(bat_v_empty_h, &bat_v_empty); diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index cd3db7324..0cefedba7 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -73,6 +73,8 @@ 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. * -- cgit v1.2.3