commander: Fix battery config

3 files changed, 32 insertions, 24 deletions

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.
  *