Merged, compiling

1 files changed, 20 insertions, 11 deletions

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index ddbfc19d6..f7e93db6c 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -69,6 +69,7 @@
 #include <poll.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/battery_status.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/offboard_control_setpoint.h>
 #include <uORB/topics/vehicle_global_position.h>
@@ -1315,8 +1316,6 @@ int commander_thread_main(int argc, char *argv[])
 	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
 	struct sensor_combined_s sensors;
 	memset(&sensors, 0, sizeof(sensors));
-	sensors.battery_voltage_v = 0.0f;
-	sensors.battery_voltage_valid = false;
 
 	/* Subscribe to command topic */
 	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
@@ -1328,6 +1327,12 @@ int commander_thread_main(int argc, char *argv[])
 	struct parameter_update_s param_changed;
 	memset(&param_changed, 0, sizeof(param_changed));
 
+	/* subscribe to battery topic */
+	int battery_sub = orb_subscribe(ORB_ID(battery_status));
+	struct battery_status_s battery;
+	memset(&battery, 0, sizeof(battery));
+	battery.voltage_v = 0.0f;
+
 	// uint8_t vehicle_state_previous = current_status.state_machine;
 	float voltage_previous = 0.0f;
 
@@ -1413,15 +1418,19 @@ int commander_thread_main(int argc, char *argv[])
 			last_local_position_time = local_position.timestamp;
 		}
 
-		battery_voltage = sensors.battery_voltage_v;
-		battery_voltage_valid = sensors.battery_voltage_valid;
-
-		/*
-		 * Only update battery voltage estimate if voltage is
-		 * valid and system has been running for two and a half seconds
-		 */
-		if (battery_voltage_valid && (hrt_absolute_time() - start_time > 2500000)) {
-			bat_remain = battery_remaining_estimate_voltage(battery_voltage);
+		orb_check(battery_sub, &new_data);
+		if (new_data) {
+			orb_copy(ORB_ID(battery_status), battery_sub, &battery);
+			battery_voltage = battery.voltage_v;
+			battery_voltage_valid = true;
+
+			/*
+			 * Only update battery voltage estimate if system has
+			 * been running for two and a half seconds.
+			 */
+			if (hrt_absolute_time() - start_time > 2500000) {
+				bat_remain = battery_remaining_estimate_voltage(battery_voltage);
+			}
 		}
 
 		/* Slow but important 8 Hz checks */