commander: move battery calculations to systemlib

The commander used to consume the battery_status topic and write the
contents after some calculations into the system state. Instead, the
calculations now happen in library calls in systemlib/battery.

This moves the battery fields out of the vehicle_status message into the
battery_status topic.

This brought quite some changes in all modules that need battery
information. The current state is compiling but untested.

src/drivers/blinkm/blinkm.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012-2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -119,12 +119,12 @@
 
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/battery_status.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/safety.h>
 
-static const float MAX_CELL_VOLTAGE	= 4.3f;
 static const int LED_ONTIME = 120;
 static const int LED_OFFTIME = 120;
 static const int LED_BLINK = 1;
@@ -195,6 +195,7 @@ private:
 	bool systemstate_run;
 
 	int vehicle_status_sub_fd;
+	int battery_status_sub_fd;
 	int vehicle_control_mode_sub_fd;
 	int vehicle_gps_position_sub_fd;
 	int actuator_armed_sub_fd;
@@ -202,7 +203,6 @@ private:
 
 	int num_of_cells;
 	int detected_cells_runcount;
-
 	int t_led_color[8];
 	int t_led_blink;
 	int led_thread_runcount;
@@ -291,6 +291,7 @@ BlinkM::BlinkM(int bus, int blinkm) :
 	led_blink(LED_NOBLINK),
 	systemstate_run(false),
 	vehicle_status_sub_fd(-1),
+	battery_status_sub_fd(-1),
 	vehicle_control_mode_sub_fd(-1),
 	vehicle_gps_position_sub_fd(-1),
 	actuator_armed_sub_fd(-1),
@@ -434,6 +435,9 @@ BlinkM::led()
 		vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
 		orb_set_interval(vehicle_status_sub_fd, 250);
 
+		battery_status_sub_fd = orb_subscribe(ORB_ID(battery_status));
+		orb_set_interval(battery_status_sub_fd, 250);
+
 		vehicle_control_mode_sub_fd = orb_subscribe(ORB_ID(vehicle_control_mode));
 		orb_set_interval(vehicle_control_mode_sub_fd, 250);
 
@@ -510,29 +514,33 @@ BlinkM::led()
 
 	if (led_thread_runcount == 15) {
 		/* obtained data for the first file descriptor */
-		struct vehicle_status_s vehicle_status_raw;
-		struct vehicle_control_mode_s vehicle_control_mode;
-		struct actuator_armed_s actuator_armed;
-		struct vehicle_gps_position_s vehicle_gps_position_raw;
-		struct safety_s safety;
+		struct vehicle_status_s vehicle_status_raw = {};
+		struct battery_status_s battery_status = {};
+		struct vehicle_control_mode_s vehicle_control_mode = {};
+		struct actuator_armed_s actuator_armed = {};
+		struct vehicle_gps_position_s vehicle_gps_position_raw = {};
+		struct safety_s safety = {};
 
 		memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw));
 		memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw));
 		memset(&safety, 0, sizeof(safety));
 
 		bool new_data_vehicle_status;
+		bool new_data_battery_status;
 		bool new_data_vehicle_control_mode;
 		bool new_data_actuator_armed;
 		bool new_data_vehicle_gps_position;
 		bool new_data_safety;
 
-		orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);
 
 		int no_data_vehicle_status = 0;
+		int no_data_battery_status = 0;
 		int no_data_vehicle_control_mode = 0;
 		int no_data_actuator_armed = 0;
 		int no_data_vehicle_gps_position = 0;
 
+		orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);
+
 		if (new_data_vehicle_status) {
 			orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw);
 			no_data_vehicle_status = 0;
@@ -545,6 +553,19 @@ BlinkM::led()
 			}
 		}
 
+		orb_check(battery_status_sub_fd, &new_data_battery_status);
+
+		if (new_data_battery_status) {
+			orb_copy(ORB_ID(battery_status), battery_status_sub_fd, &battery_status);
+
+		} else {
+			no_data_battery_status++;
+
+			if (no_data_battery_status >= 3) {
+				no_data_battery_status = 3;
+			}
+		}
+
 		orb_check(vehicle_control_mode_sub_fd, &new_data_vehicle_control_mode);
 
 		if (new_data_vehicle_control_mode) {
@@ -597,19 +618,13 @@ BlinkM::led()
 		/* get number of used satellites in navigation */
 		num_of_used_sats = vehicle_gps_position_raw.satellites_used;
 
-		if (new_data_vehicle_status || no_data_vehicle_status < 3) {
+		if (new_data_battery_status || no_data_battery_status < 3) {
 			if (num_of_cells == 0) {
-				/* looking for lipo cells that are connected */
-				printf("<blinkm> checking cells\n");
 
-				for (num_of_cells = 2; num_of_cells < 7; num_of_cells++) {
-					if (vehicle_status_raw.battery_voltage < num_of_cells * MAX_CELL_VOLTAGE) { break; }
-				}
-
-				printf("<blinkm> cells found:%d\n", num_of_cells);
+				num_of_cells = battery_status.cell_count;
 
 			} else {
-				if (vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) {
+				if (battery_status.warning == battery_status_s::BATTERY_WARNING_CRITICAL) {
 					/* LED Pattern for battery critical alerting */
 					led_color_1 = LED_RED;
 					led_color_2 = LED_RED;
@@ -633,7 +648,7 @@ BlinkM::led()
 					led_color_8 = LED_BLUE;
 					led_blink = LED_BLINK;
 
-				} else if (vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW) {
+				} else if (battery_status.warning == battery_status_s::BATTERY_WARNING_LOW) {
 					/* LED Pattern for battery low warning */
 					led_color_1 = LED_YELLOW;
 					led_color_2 = LED_YELLOW;

src/drivers/frsky_telemetry/frsky_data.c

@@ -53,7 +53,6 @@
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_status.h>
 
 #include <drivers/drv_hrt.h>
 
@@ -95,12 +94,15 @@
 static struct battery_status_s *battery_status;
 static struct vehicle_global_position_s *global_pos;
 static struct sensor_combined_s *sensor_data;
-static struct vehicle_status_s *vehicle_status;
 
-static int battery_sub = -1;
+static int battery_status_sub = -1;
 static int global_position_sub = -1;
 static int sensor_sub = -1;
-static int vehicle_status_sub = -1;
+
+static struct battery_status_s battery_status;
+static struct sensor_combined_s raw;
+static struct vehicle_global_position_s global_pos;
+
 
 /**
  * Initializes the uORB subscriptions.
@@ -110,16 +112,14 @@ bool frsky_init()
 	battery_status = malloc(sizeof(struct battery_status_s));
 	global_pos = malloc(sizeof(struct vehicle_global_position_s));
 	sensor_data = malloc(sizeof(struct sensor_combined_s));
-	vehicle_status = malloc(sizeof(struct vehicle_status_s));
 
-	if (battery_status == NULL || global_pos == NULL || sensor_data == NULL || vehicle_status == NULL) {
+	if (battery_status == NULL || global_pos == NULL || sensor_data == NULL) {
 		return false;
 	}
 
-	battery_sub = orb_subscribe(ORB_ID(battery_status));
+	battery_status_sub = orb_subscribe(ORB_ID(battery_status));
 	global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 	sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
-	vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
 	return true;
 }
 
@@ -128,7 +128,6 @@ void frsky_deinit()
 	free(battery_status);
 	free(global_pos);
 	free(sensor_data);
-	free(vehicle_status);
 }
 
 /**
@@ -178,18 +177,35 @@ static void frsky_send_data(int uart, uint8_t id, int16_t data)
 	frsky_send_byte(uart, udata >> 8); /* MSB */
 }
 
+void frsky_update_topics()
+{
+	bool updated;
+
+	/* get a local copy of the current sensor values */
+	orb_check(sensor_sub, &updated);
+	if (updated) {
+		orb_copy(ORB_ID(sensor_combined), sensor_sub, raw);
+	}
+
+	/* get a local copy of the battery data */
+	orb_check(battery_status_sub, &updated);
+	if (updated) {
+		orb_copy(ORB_ID(battery_status), battery_status_sub, battery_status);
+	}
+
+	/* get a local copy of the global position data */
+	orb_check(global_position_sub, &updated);
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_global_position), global_position_sub, global_pos);
+	}
+}
+
 /**
  * Sends frame 1 (every 200ms):
  *   acceleration values, barometer altitude, temperature, battery voltage & current
  */
 void frsky_send_frame1(int uart)
 {
-	/* get a local copy of the current sensor values */
-	orb_copy(ORB_ID(sensor_combined), sensor_sub, sensor_data);
-
-	/* get a local copy of the battery data */
-	orb_copy(ORB_ID(battery_status), battery_sub, battery_status);
-
 	/* send formatted frame */
 	frsky_send_data(uart, FRSKY_ID_ACCEL_X,
 			roundf(sensor_data->accelerometer_m_s2[0] * 1000.0f));
@@ -229,12 +245,6 @@ static float frsky_format_gps(float dec)
  */
 void frsky_send_frame2(int uart)
 {
-	/* get a local copy of the global position data */
-	orb_copy(ORB_ID(vehicle_global_position), global_position_sub, global_pos);
-
-	/* get a local copy of the vehicle status data */
-	orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, vehicle_status);
-
 	/* send formatted frame */
 	float course = 0, lat = 0, lon = 0, speed = 0, alt = 0;
 	char lat_ns = 0, lon_ew = 0;
@@ -273,7 +283,7 @@ void frsky_send_frame2(int uart)
 	frsky_send_data(uart, FRSKY_ID_GPS_ALT_AP, frac(alt) * 100.0f);
 
 	frsky_send_data(uart, FRSKY_ID_FUEL,
-			roundf(vehicle_status->battery_remaining * 100.0f));
+			roundf(battery_status->remaining * 100.0f));
 
 	frsky_send_data(uart, FRSKY_ID_GPS_SEC, sec);
 
@@ -286,9 +296,6 @@ void frsky_send_frame2(int uart)
  */
 void frsky_send_frame3(int uart)
 {
-	/* get a local copy of the battery data */
-	orb_copy(ORB_ID(vehicle_global_position), global_position_sub, global_pos);
-
 	/* send formatted frame */
 	time_t time_gps = global_pos->time_utc_usec / 1000000ULL;
 	struct tm *tm_gps = gmtime(&time_gps);

src/drivers/frsky_telemetry/frsky_data.h

@@ -47,6 +47,7 @@
 // Public functions
 bool frsky_init(void);
 void frsky_deinit(void);
+void frsky_update_topics(void);
 void frsky_send_frame1(int uart);
 void frsky_send_frame2(int uart);
 void frsky_send_frame3(int uart);

src/drivers/frsky_telemetry/frsky_telemetry.c

@@ -456,6 +456,8 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
 				      host_frame.ad1, host_frame.ad2, host_frame.linkq);
 			}
 
+			frsky_update_topics();
+
 			/* Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM */
 			if (iteration % 2 == 0) {
 				frsky_send_frame1(uart);

src/drivers/frsky_telemetry/sPort_data.c

@@ -51,7 +51,7 @@
 
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/battery_status.h>
 
 #include <drivers/drv_hrt.h>
 
@@ -59,35 +59,35 @@
 
 static int sensor_sub = -1;
 static int global_position_sub = -1;
-static int vehicle_status_sub = -1;
-static struct vehicle_status_s *vehicle_status;
+static int battery_status_sub = -1;
 static struct sensor_combined_s *sensor_data;
 static struct vehicle_global_position_s *global_pos;
+static struct battery_status_s *battery_status;
 
 /**
  * Initializes the uORB subscriptions.
  */
 bool sPort_init()
 {
-	vehicle_status = malloc(sizeof(struct vehicle_status_s));
 	sensor_data = malloc(sizeof(struct sensor_combined_s));
 	global_pos = malloc(sizeof(struct vehicle_global_position_s));
+	battery_status = malloc(sizeof(struct battery_status_s));
 
-	if (vehicle_status == NULL || sensor_data == NULL || global_pos == NULL) {
+	if (sensor_data == NULL || global_pos == NULL || battery_status == NULL) {
 		return false;
 	}
 
-	global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 	sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
-	vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+	global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+	battery_status_sub = orb_subscribe(ORB_ID(battery_status));
 	return true;
 }
 
 void sPort_deinit()
 {
-	free(vehicle_status);
 	free(sensor_data);
 	free(global_pos);
+	free(battery_status);
 }
 
 static void update_crc(uint16_t *crc, unsigned char b)
@@ -158,19 +158,24 @@ void sPort_send_data(int uart, uint16_t id, uint32_t data)
 // scaling correct with OpenTX 2.1.7
 void sPort_send_BATV(int uart)
 {
-	/* get a local copy of the vehicle status data */
-	orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, vehicle_status);
+	/* get a local copy of the battery data */
+	orb_copy(ORB_ID(battery_status), battery_status_sub, battery_status);
 
 	/* send battery voltage as VFAS */
-	uint32_t voltage = (int)(100 * vehicle_status->battery_voltage);
+	uint32_t voltage = (int)(100 * battery_status->voltage_v);
+
 	sPort_send_data(uart, SMARTPORT_ID_VFAS, voltage);
 }
 
 // verified scaling
 void sPort_send_CUR(int uart)
 {
+	/* get a local copy of the battery data */
+	orb_copy(ORB_ID(battery_status), battery_status_sub, battery_status);
+
 	/* send data */
-	uint32_t current = (int)(10 * vehicle_status->battery_current);
+	uint32_t current = (int)(10 * battery_status->current_a);
+
 	sPort_send_data(uart, SMARTPORT_ID_CURR, current);
 }
 
@@ -207,8 +212,12 @@ void sPort_send_VSPD(int uart, float speed)
 // verified scaling
 void sPort_send_FUEL(int uart)
 {
+	/* get a local copy of the battery data */
+	orb_copy(ORB_ID(battery_status), battery_status_sub, battery_status);
+
 	/* send data */
-	uint32_t fuel = (int)(100 * vehicle_status->battery_remaining);
+	uint32_t fuel = (int)(100 * vehicle_status->remaining_mah);
+
 	sPort_send_data(uart, SMARTPORT_ID_FUEL, fuel);
 }
 

src/drivers/px4io/px4io.cpp

@@ -74,6 +74,7 @@
 #include <systemlib/param/param.h>
 #include <systemlib/circuit_breaker.h>
 #include <systemlib/mavlink_log.h>
+#include <systemlib/battery.h>
 
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_controls_0.h>
@@ -308,10 +309,14 @@ private:
 	bool			_cb_flighttermination;	///< true if the flight termination circuit breaker is enabled
 	bool 			_in_esc_calibration_mode;	///< do not send control outputs to IO (used for esc calibration)
 
+	Battery			_battery;
+
 	int32_t			_rssi_pwm_chan; ///< RSSI PWM input channel
 	int32_t			_rssi_pwm_max; ///< max RSSI input on PWM channel
 	int32_t			_rssi_pwm_min; ///< min RSSI input on PWM channel
 
+	float			_last_throttle; ///< last throttle value for battery calculation
+
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	bool			_dsm_vcc_ctl;		///< true if relay 1 controls DSM satellite RX power
 #endif
@@ -538,9 +543,11 @@ PX4IO::PX4IO(device::Device *interface) :
 	_battery_last_timestamp(0),
 	_cb_flighttermination(true),
 	_in_esc_calibration_mode(false),
+	_battery{},
 	_rssi_pwm_chan(0),
 	_rssi_pwm_max(0),
-	_rssi_pwm_min(0)
+	_rssi_pwm_min(0),
+	_last_throttle(0.0f)
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	, _dsm_vcc_ctl(false)
 #endif
@@ -1188,6 +1195,8 @@ PX4IO::task_main()
 								    (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT), 0);
 					}
 				}
+				// Also trigger param update in Battery instance.
+				_battery.updateParams();
 			}
 
 		}
@@ -1297,6 +1306,11 @@ PX4IO::io_set_control_state(unsigned group)
 		regs[i] = FLOAT_TO_REG(ctrl);
 	}
 
+	// save last throttle for battery calculation
+	if (group == 0) {
+		_last_throttle = controls.control[3];
+	}
+
 	/* copy values to registers in IO */
 	return io_reg_set(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT, regs, _max_controls);
 }
@@ -1648,29 +1662,19 @@ PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt)
 	}
 
 	battery_status_s	battery_status;
-	battery_status.timestamp = hrt_absolute_time();
 
+	const hrt_abstime timestamp = hrt_absolute_time();
 	/* voltage is scaled to mV */
-	battery_status.voltage_v = vbatt / 1000.0f;
-	battery_status.voltage_filtered_v = vbatt / 1000.0f;
+	const float voltage_v = vbatt / 1000.0f;
 
 	/*
 	  ibatt contains the raw ADC count, as 12 bit ADC
 	  value, with full range being 3.3v
 	*/
-	battery_status.current_a = ibatt * (3.3f / 4096.0f) * _battery_amp_per_volt;
-	battery_status.current_a += _battery_amp_bias;
+	float current_a = ibatt * (3.3f / 4096.0f) * _battery_amp_per_volt;
+	current_a += _battery_amp_bias;
 
-	/*
-	  integrate battery over time to get total mAh used
-	*/
-	if (_battery_last_timestamp != 0) {
-		_battery_mamphour_total += battery_status.current_a *
-					   (battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600;
-	}
-
-	battery_status.discharged_mah = _battery_mamphour_total;
-	_battery_last_timestamp = battery_status.timestamp;
+	_battery.updateBatteryStatus(timestamp, voltage_v, current_a, _last_throttle, &battery_status);
 
 	/* the announced battery status would conflict with the simulated battery status in HIL */
 	if (!(_pub_blocked)) {