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.

msg/battery_status.msg

@@ -3,4 +3,13 @@ float32 voltage_v		# Battery voltage in volts, 0 if unknown
 float32 voltage_filtered_v	# Battery voltage in volts, filtered, 0 if unknown
 float32 current_a		# Battery current in amperes, -1 if unknown
 float32 discharged_mah		# Discharged amount in mAh, -1 if unknown
-bool is_powering_off		# Power off event imminent indication, false if unknown
+float32 remaining		# From 1 to 0, -1 if unknown
+int32 cell_count		# Number of cells
+#bool is_powering_off		# Power off event imminent indication, false if unknown
+
+
+uint8 BATTERY_WARNING_NONE = 0       # no battery low voltage warning active
+uint8 BATTERY_WARNING_LOW = 1        # warning of low voltage
+uint8 BATTERY_WARNING_CRITICAL = 2   # alerting of critical voltage
+
+uint8 warning    # current battery warning

msg/vehicle_status.msg

@@ -77,7 +77,6 @@ bool is_vtol				# True if the system is VTOL capable
 bool vtol_fw_permanent_stab		# True if vtol should stabilize attitude for fw in manual mode
 bool in_transition_mode			# True if VTOL is doing a transition
 
-bool condition_battery_voltage_valid
 bool condition_system_in_air_restore		# true if we can restore in mid air
 bool condition_system_sensors_initialized
 bool condition_system_prearm_error_reported	# true if errors have already been reported
@@ -125,11 +124,4 @@ uint32 onboard_control_sensors_enabled
 uint32 onboard_control_sensors_health
 
 float32 load					# processor load from 0 to 1
-float32 battery_voltage
-float32 battery_current
-float32 battery_remaining
-float32 battery_discharged_mah
-uint32 battery_cell_count
-
-uint8 battery_warning    # current battery warning mode, as defined by VEHICLE_BATTERY_WARNING enum
 

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_status_s vehicle_status_raw = {};
-		struct vehicle_control_mode_s vehicle_control_mode;
+		struct battery_status_s battery_status = {};
-		struct actuator_armed_s actuator_armed;
+		struct vehicle_control_mode_s vehicle_control_mode = {};
-		struct vehicle_gps_position_s vehicle_gps_position_raw;
+		struct actuator_armed_s actuator_armed = {};
-		struct safety_s safety;
+		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++) {
+				num_of_cells = battery_status.cell_count;
-					if (vehicle_status_raw.battery_voltage < num_of_cells * MAX_CELL_VOLTAGE) { break; }
-				}
-
-				printf("<blinkm> cells found:%d\n", num_of_cells);
 
 			} 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 int battery_status_sub = -1;
-static struct vehicle_status_s *vehicle_status;
 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 */
+	/* get a local copy of the battery data */
-	orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, vehicle_status);
+	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;
+	const float voltage_v = vbatt / 1000.0f;
-	battery_status.voltage_filtered_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;
+	float current_a = ibatt * (3.3f / 4096.0f) * _battery_amp_per_volt;
-	battery_status.current_a += _battery_amp_bias;
+	current_a += _battery_amp_bias;
 
-	/*
+	_battery.updateBatteryStatus(timestamp, voltage_v, current_a, _last_throttle, &battery_status);
-	  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;
 
 	/* the announced battery status would conflict with the simulated battery status in HIL */
 	if (!(_pub_blocked)) {

src/modules/commander/commander.cpp

@@ -255,7 +255,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
  */
 int commander_thread_main(int argc, char *argv[]);
 
-void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed);
+void control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed,
+			 battery_status_s *battery);
 
 void get_circuit_breaker_params();
 
@@ -1224,10 +1225,6 @@ int commander_thread_main(int argc, char *argv[])
 		mavlink_and_console_log_critical(&mavlink_log_pub, "ERROR: BUZZER INIT FAIL");
 	}
 
-	if (battery_init() != OK) {
-		mavlink_and_console_log_critical(&mavlink_log_pub, "ERROR: BATTERY INIT FAIL");
-	}
-
 	/* vehicle status topic */
 	memset(&status, 0, sizeof(status));
 	status.condition_landed = true;	// initialize to safe value
@@ -1255,10 +1252,6 @@ int commander_thread_main(int argc, char *argv[])
 	status.offboard_control_signal_lost = true;
 	status.data_link_lost = true;
 
-	/* set battery warning flag */
-	battery_warning = vehicle_battery_warning::NONE;
-	status.condition_battery_voltage_valid = false;
-
 	// XXX for now just set sensors as initialized
 	status.condition_system_sensors_initialized = true;
 
@@ -1471,7 +1464,7 @@ int commander_thread_main(int argc, char *argv[])
 	vtol_status.vtol_in_rw_mode = true;		//default for vtol is rotary wing
 
 
-	control_status_leds(&status, &armed, true);
+	control_status_leds(&status, &armed, true, &battery);
 
 	/* now initialized */
 	commander_initialized = true;
@@ -1967,20 +1960,37 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (updated) {
 			orb_copy(ORB_ID(battery_status), battery_sub, &battery);
-			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
 
 			/* only consider battery voltage if system has been running 2s and battery voltage is valid */
-			if (hrt_absolute_time() > commander_boot_timestamp + 2000000 && battery.voltage_filtered_v > 0.0f) {
+			if (hrt_absolute_time() > commander_boot_timestamp + 2000000
-				status.battery_voltage = battery.voltage_filtered_v;
+			    && battery.voltage_filtered_v > FLT_EPSILON) {
-				status.battery_current = battery.current_a;
-				status.battery_discharged_mah = battery.discharged_mah;
-				status.condition_battery_voltage_valid = true;
-				status.battery_cell_count = battery_get_n_cells();
 
-				/* get throttle (if armed), as we only care about energy negative throttle also counts */
+				/* if battery voltage is getting lower, warn using buzzer, etc. */
-				float throttle = (armed.armed) ? fabsf(actuator_controls.control[3]) : 0.0f;
+				if (battery.warning == battery_status_s::BATTERY_WARNING_LOW &&
-				status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah,
+				    !low_battery_voltage_actions_done) {
-							   throttle);
+					low_battery_voltage_actions_done = true;
+					if (armed.armed) {
+						mavlink_log_critical(&mavlink_log_pub, "LOW BATTERY, RETURN TO LAND ADVISED");
+					} else {
+						mavlink_log_critical(&mavlink_log_pub, "LOW BATTERY, TAKEOFF DISCOURAGED");
+					}
+
+				} else if (!status.usb_connected &&
+					   battery.warning == battery_status_s::BATTERY_WARNING_CRITICAL &&
+					   !critical_battery_voltage_actions_done &&
+					   low_battery_voltage_actions_done) {
+					critical_battery_voltage_actions_done = true;
+
+					if (armed.armed) {
+						mavlink_and_console_log_critical(&mavlink_log_pub, "CRITICAL BATTERY, SHUT SYSTEM DOWN");
+					} else {
+						mavlink_and_console_log_emergency(&mavlink_log_pub, "CRITICAL BATTERY, LAND IMMEDIATELY");
+					}
+
+					status_changed = true;
+				}
+
+				/* End battery voltage check */
 			}
 		}
 
@@ -2037,34 +2047,6 @@ int commander_thread_main(int argc, char *argv[])
 			last_idle_time = system_load.tasks[0].total_runtime;
 		}
 
-		/* if battery voltage is getting lower, warn using buzzer, etc. */
-		if (status.condition_battery_voltage_valid && status.battery_remaining < 0.18f && !low_battery_voltage_actions_done) {
-			low_battery_voltage_actions_done = true;
-			if (armed.armed) {
-				mavlink_log_critical(&mavlink_log_pub, "LOW BATTERY, RETURN TO LAND ADVISED");
-			} else {
-				mavlink_log_critical(&mavlink_log_pub, "LOW BATTERY, TAKEOFF DISCOURAGED");
-			}
-			battery_warning = vehicle_battery_warning::LOW;
-			status_changed = true;
-
-		} else if (!status.usb_connected && status.condition_battery_voltage_valid && status.battery_remaining < 0.09f
-			   && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
-			/* critical battery voltage, this is rather an emergency, change state machine */
-			critical_battery_voltage_actions_done = true;
-			battery_warning = vehicle_battery_warning::CRITICAL;
-
-			if (armed.armed) {
-				mavlink_and_console_log_critical(&mavlink_log_pub, "CRITICAL BATTERY, SHUT SYSTEM DOWN");
-			} else {
-				mavlink_and_console_log_emergency(&mavlink_log_pub, "CRITICAL BATTERY, LAND IMMEDIATELY");
-			}
-
-			status_changed = true;
-		}
-
-		/* End battery voltage check */
-
 		/* If in INIT state, try to proceed to STANDBY state */
 		if (!status.calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
 			arming_ret = arming_state_transition(&status,
@@ -2518,33 +2500,41 @@ int commander_thread_main(int argc, char *argv[])
 			}
 		}
 
-		/* Check engine failure
+		/* handle commands last, as the system needs to be updated to handle them */
-		 * only for fixed wing for now
+		orb_check(actuator_controls_sub, &updated);
-		 */
-		if (!circuit_breaker_engaged_enginefailure_check &&
-		    status.is_rotary_wing == false &&
-		    armed.armed &&
-		    ((actuator_controls.control[3] > ef_throttle_thres &&
-		      battery.current_a / actuator_controls.control[3] <
-		      ef_current2throttle_thres) ||
-		     (status.engine_failure))) {
-			/* potential failure, measure time */
-			if (timestamp_engine_healthy > 0 &&
-			    hrt_elapsed_time(&timestamp_engine_healthy) >
-			    ef_time_thres * 1e6 &&
-			    !status.engine_failure) {
-				status.engine_failure = true;
-				status_changed = true;
-				mavlink_log_critical(&mavlink_log_pub, "Engine Failure");
-			}
 
-		} else {
+		if (updated) {
-			/* no failure reset flag */
+			/* got command */
-			timestamp_engine_healthy = hrt_absolute_time();
+			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
 
-			if (status.engine_failure) {
+			/* Check engine failure
-				status.engine_failure = false;
+			 * only for fixed wing for now
-				status_changed = true;
+			 */
+			if (!circuit_breaker_engaged_enginefailure_check &&
+			    status.is_rotary_wing == false &&
+			    armed.armed &&
+			    ((actuator_controls.control[3] > ef_throttle_thres &&
+			      battery.current_a / actuator_controls.control[3] <
+			      ef_current2throttle_thres) ||
+			     (status.engine_failure))) {
+				/* potential failure, measure time */
+				if (timestamp_engine_healthy > 0 &&
+				    hrt_elapsed_time(&timestamp_engine_healthy) >
+				    ef_time_thres * 1e6 &&
+				    !status.engine_failure) {
+					status.engine_failure = true;
+					status_changed = true;
+					mavlink_log_critical(mavlink_fd, "Engine Failure");
+				}
+
+			} else {
+				/* no failure reset flag */
+				timestamp_engine_healthy = hrt_absolute_time();
+
+				if (status.engine_failure) {
+					status.engine_failure = false;
+					status_changed = true;
+				}
 			}
 		}
 
@@ -2711,12 +2701,12 @@ int commander_thread_main(int argc, char *argv[])
 			arm_tune_played = true;
 
 		} else if ((status.hil_state != vehicle_status_s::HIL_STATE_ON) &&
-				battery_warning == vehicle_battery_warning::CRITICAL) {
+			   (battery.warning == battery_status_s::BATTERY_WARNING_CRITICAL)) {
 			/* play tune on battery critical */
 			set_tune(TONE_BATTERY_WARNING_FAST_TUNE);
 
 		} else if ((status.hil_state != vehicle_status_s::HIL_STATE_ON) &&
-				(battery_warning == vehicle_battery_warning::LOW || status.failsafe)) {
+			   (battery.warning == battery_status_s::BATTERY_WARNING_LOW)) {
 			/* play tune on battery warning or failsafe */
 			set_tune(TONE_BATTERY_WARNING_SLOW_TUNE);
 
@@ -2758,12 +2748,12 @@ int commander_thread_main(int argc, char *argv[])
 			/* blinking LED message, don't touch LEDs */
 			if (blink_state == 2) {
 				/* blinking LED message completed, restore normal state */
-				control_status_leds(&status, &armed, true);
+				control_status_leds(&status, &armed, true, &battery);
 			}
 
 		} else {
 			/* normal state */
-			control_status_leds(&status, &armed, status_changed);
+			control_status_leds(&status, &armed, status_changed, &battery);
 		}
 
 		status_changed = false;
@@ -2829,7 +2819,7 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val
 }
 
 void
-control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed)
+control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed, battery_status_s *battery)
 {
 	/* driving rgbled */
 	if (changed) {
@@ -2862,9 +2852,9 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
 			/* set color */
 			if (status_local->failsafe) {
 				rgbled_set_color(RGBLED_COLOR_PURPLE);
-			} else if (battery_warning == vehicle_battery_warning::LOW) {
+			} else if (battery->warning == battery_status_s::BATTERY_WARNING_LOW) {
 				rgbled_set_color(RGBLED_COLOR_AMBER);
-			} else if (battery_warning == vehicle_battery_warning::CRITICAL) {
+			} else if (battery->warning == battery_status_s::BATTERY_WARNING_CRITICAL) {
 				rgbled_set_color(RGBLED_COLOR_RED);
 			} else {
 				if (status_local->condition_home_position_valid && status_local->condition_global_position_valid) {

src/modules/commander/commander_helper.cpp

@@ -105,30 +105,6 @@ static DevHandle h_leds;
 static DevHandle h_rgbleds;
 static DevHandle h_buzzer;
 
-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;
-static float throttle_lowpassed = 0.0f;
-
-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 PX4_OK;
-}
-
 int buzzer_init()
 {
 	tune_end = 0;
@@ -346,51 +322,3 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern)
 
 	h_rgbleds.ioctl(RGBLED_SET_PATTERN, (unsigned long)pattern);
 }
-
-unsigned battery_get_n_cells() {
-	return bat_n_cells;
-}
-
-float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized)
-{
-	float ret = 0;
-
-	if (counter % 100 == 0) {
-		param_get(bat_v_empty_h, &bat_v_empty);
-		param_get(bat_v_full_h, &bat_v_full);
-		param_get(bat_v_load_drop_h, &bat_v_load_drop);
-		param_get(bat_n_cells_h, &bat_n_cells);
-		param_get(bat_capacity_h, &bat_capacity);
-	}
-
-	counter++;
-
-	// XXX this time constant needs to become tunable
-	// but really, the right fix are smart batteries.
-	float val = throttle_lowpassed * 0.97f + throttle_normalized * 0.03f;
-	if (PX4_ISFINITE(val)) {
-		throttle_lowpassed = val;
-	}
-
-	/* remaining charge estimate based on voltage and internal resistance (drop under load) */
-	float bat_v_empty_dynamic = bat_v_empty - (bat_v_load_drop * throttle_lowpassed);
-	/* the range from full to empty is the same for batteries under load and without load,
-	 * since the voltage drop applies to both the full and empty state
-	 */
-	float voltage_range = (bat_v_full - bat_v_empty);
-	float remaining_voltage = (voltage - (bat_n_cells * bat_v_empty_dynamic)) / (bat_n_cells * voltage_range);
-
-	if (bat_capacity > 0.0f) {
-		/* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */
-		ret = fminf(remaining_voltage, 1.0f - discharged / bat_capacity);
-
-	} else {
-		/* else use voltage */
-		ret = remaining_voltage;
-	}
-
-	/* limit to sane values */
-	ret = (ret < 0.0f) ? 0.0f : ret;
-	ret = (ret > 1.0f) ? 1.0f : ret;
-	return ret;
-}

src/modules/commander/commander_helper.h

@@ -77,21 +77,4 @@ 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.
- *
- * Use integral of current if battery capacity known (BAT_CAPACITY parameter set),
- * else use simple estimate based on voltage.
- *
- * @param voltage the current battery voltage
- * @param discharged the discharged capacity
- * @param throttle_normalized the normalized throttle magnitude from 0 to 1. Negative throttle should be converted to this range as well, as it consumes energy.
- * @return the estimated remaining capacity in 0..1
- */
-float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized);
-
-unsigned battery_get_n_cells();
-
 #endif /* COMMANDER_HELPER_H_ */

src/modules/commander/commander_params.c

@@ -93,87 +93,7 @@ PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
 PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
 
 /**
- * Empty cell voltage.
+ * Datalink loss mode enabled.
- *
- * Defines the voltage where a single cell of the battery is considered empty.
- *
- * @group Battery Calibration
- * @unit V
- * @decimal 2
- * @increment 0.01
- */
-PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f);
-
-/**
- * Full cell voltage.
- *
- * Defines the voltage where a single cell of the battery is considered full.
- *
- * @group Battery Calibration
- * @unit V
- * @decimal 2
- * @increment 0.01
- */
-PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f);
-
-/**
- * Voltage drop per cell on 100% load
- *
- * This implicitely defines the internal resistance
- * to maximum current ratio and assumes linearity.
- *
- * @group Battery Calibration
- * @unit V
- * @min 0.0
- * @max 1.5
- * @decimal 2
- * @increment 0.01
- */
-PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f);
-
-/**
- * Number of cells.
- *
- * Defines the number of cells the attached battery consists of.
- *
- * @group Battery Calibration
- * @unit S
- * @min 2
- * @max 10
- * @value 2 2S Battery
- * @value 3 3S Battery
- * @value 4 4S Battery
- * @value 5 5S Battery
- * @value 6 6S Battery
- * @value 7 7S Battery
- * @value 8 8S Battery
- * @value 9 9S Battery
- * @value 10 10S Battery
- * @value 11 11S Battery
- * @value 12 12S Battery
- * @value 13 13S Battery
- * @value 14 14S Battery
- * @value 15 15S Battery
- * @value 16 16S Battery
- */
-PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
-
-/**
- * Battery capacity.
- *
- * Defines the capacity of the attached battery.
- *
- * @group Battery Calibration
- * @unit mA
- * @decimal 0
- * @min -1.0
- * @max 100000
- * @increment 50
- */
-PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
-
-/**
- * Datalink loss failsafe.
  *
  * Set to 1 to enable actions triggered when the datalink is lost.
  *

src/modules/gpio_led/gpio_led.c

@@ -51,6 +51,7 @@
 #include <systemlib/err.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/battery_status.h>
 #include <poll.h>
 #include <drivers/drv_gpio.h>
 #include <modules/px4iofirmware/protocol.h>
@@ -60,8 +61,10 @@ struct gpio_led_s {
 	int gpio_fd;
 	bool use_io;
 	int pin;
-	struct vehicle_status_s status;
+	struct vehicle_status_s vehicle_status;
+	struct battery_status_s battery_status;
 	int vehicle_status_sub;
+	int battery_status_sub;
 	bool led_state;
 	int counter;
 };
@@ -238,11 +241,17 @@ void gpio_led_start(FAR void *arg)
 	ioctl(priv->gpio_fd, GPIO_SET_OUTPUT, priv->pin);
 
 	/* initialize vehicle status structure */
-	memset(&priv->status, 0, sizeof(priv->status));
+	memset(&priv->vehicle_status, 0, sizeof(priv->vehicle_status));
+
+	/* initialize battery status structure */
+	memset(&priv->battery_status, 0, sizeof(priv->battery_status));
 
 	/* subscribe to vehicle status topic */
 	priv->vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
 
+	/* subscribe to battery status topic */
+	priv->battery_status_sub = orb_subscribe(ORB_ID(battery_status));
+
 	/* add worker to queue */
 	int ret = work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, 0);
 
@@ -258,32 +267,39 @@ void gpio_led_cycle(FAR void *arg)
 {
 	FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg;
 
-	/* check for status updates*/
+	/* check for vehicle status updates*/
 	bool updated;
 	orb_check(priv->vehicle_status_sub, &updated);
 
 	if (updated) {
-		orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status);
+		orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->vehicle_status);
 	}
 
-	/* select pattern for current status */
+	orb_check(priv->battery_status_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(battery_status), priv->battery_status_sub, &priv->battery_status);
+	}
+
+	/* select pattern for current vehiclestatus */
 	int pattern = 0;
 
-	if (priv->status.arming_state == ARMING_STATE_ARMED_ERROR) {
+	if (priv->vehicle_status.arming_state == ARMING_STATE_ARMED_ERROR) {
 		pattern = 0x2A;	// *_*_*_ fast blink (armed, error)
 
-	} else if (priv->status.arming_state == ARMING_STATE_ARMED) {
+	} else if (priv->vehicle_status.arming_state == ARMING_STATE_ARMED) {
-		if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE && !priv->status.failsafe) {
+		if (priv->battery_status.warning == BATTERY_WARNING_NONE
+		    && !priv->vehicle_status.failsafe) {
 			pattern = 0x3f;	// ****** solid (armed)
 
 		} else {
 			pattern = 0x3e;	// *****_ slow blink (armed, battery low or failsafe)
 		}
 
-	} else if (priv->status.arming_state == ARMING_STATE_STANDBY) {
+	} else if (priv->vehicle_status.arming_state == ARMING_STATE_STANDBY) {
 		pattern = 0x38;	// ***___ slow blink (disarmed, ready)
 
-	} else if (priv->status.arming_state == ARMING_STATE_STANDBY_ERROR) {
+	} else if (priv->vehicle_status.arming_state == ARMING_STATE_STANDBY_ERROR) {
 		pattern = 0x28;	// *_*___ slow double blink (disarmed, error)
 
 	}

src/modules/mavlink/mavlink_messages.cpp

@@ -557,6 +557,7 @@ public:
 
 private:
 	MavlinkOrbSubscription *_status_sub;
+	MavlinkOrbSubscription *_battery_status_sub;
 
 	/* do not allow top copying this class */
 	MavlinkStreamSysStatus(MavlinkStreamSysStatus &);
@@ -564,24 +565,28 @@ private:
 
 protected:
 	explicit MavlinkStreamSysStatus(Mavlink *mavlink) : MavlinkStream(mavlink),
-		_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status)))
+		_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))),
+		_battery_status_sub(_mavlink->add_orb_subscription(ORB_ID(battery_status)))
 	{}
 
 	void send(const hrt_abstime t)
 	{
 		struct vehicle_status_s status;
+		struct battery_status_s battery_status;
 
-		if (_status_sub->update(&status)) {
+		bool updated = _status_sub->update(&status);
+		updated = (updated || _battery_status_sub->update(&battery_status));
+
+		if (updated) {
 			mavlink_sys_status_t msg;
 
 			msg.onboard_control_sensors_present = status.onboard_control_sensors_present;
 			msg.onboard_control_sensors_enabled = status.onboard_control_sensors_enabled;
 			msg.onboard_control_sensors_health = status.onboard_control_sensors_health;
 			msg.load = status.load * 1000.0f;
-			msg.voltage_battery = status.battery_voltage * 1000.0f;
+			msg.voltage_battery = battery_status.voltage_v * 1000.0f;
-			msg.current_battery = status.battery_current * 100.0f;
+			msg.current_battery = battery_status.current_a * 100.0f;
-			msg.battery_remaining = (status.condition_battery_voltage_valid) ?
+			msg.battery_remaining = battery_status.remaining >= 0 ? battery_status.remaining * 100.0f : -1;
-							status.battery_remaining * 100.0f : -1;
 			// TODO: fill in something useful in the fields below
 			msg.drop_rate_comm = 0;
 			msg.errors_comm = 0;
@@ -595,28 +600,19 @@ protected:
 			/* battery status message with higher resolution */
 			mavlink_battery_status_t bat_msg;
 			bat_msg.id = 0;
+			bat_msg.id = 0;
 			bat_msg.battery_function = MAV_BATTERY_FUNCTION_ALL;
 			bat_msg.type = MAV_BATTERY_TYPE_LIPO;
 			bat_msg.temperature = INT16_MAX;
 			for (unsigned i = 0; i < (sizeof(bat_msg.voltages) / sizeof(bat_msg.voltages[0])); i++) {
-				if (i < status.battery_cell_count) {
+				if (i < battery_status.cell_count) {
-					bat_msg.voltages[i] = (status.battery_voltage / status.battery_cell_count) * 1000.0f;
+					bat_msg.voltages[i] = (battery_status.voltage_v / battery_status.cell_count) * 1000.0f;
 				} else {
 					bat_msg.voltages[i] = UINT16_MAX;
 				}
 			}
 
-			if (status.condition_battery_voltage_valid) {
+			// TODO: calculate this
-				bat_msg.current_battery = (bat_msg.current_battery >= 0.0f) ?
-								status.battery_current * 100.0f : -1;
-				bat_msg.current_consumed = (bat_msg.current_consumed >= 0.0f) ?
-								status.battery_discharged_mah : -1;
-				bat_msg.battery_remaining = status.battery_remaining * 100.0f;
-			} else {
-				bat_msg.current_battery = -1.0f;
-				bat_msg.current_consumed = -1.0f;
-				bat_msg.battery_remaining = -1.0f;
-			}
 			bat_msg.energy_consumed = -1.0f;
 
 			_mavlink->send_message(MAVLINK_MSG_ID_BATTERY_STATUS, &bat_msg);

src/modules/sdlog2/sdlog2.c

@@ -1447,8 +1447,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_STAT.main_state = buf_status.main_state;
 			log_msg.body.log_STAT.arming_state = buf_status.arming_state;
 			log_msg.body.log_STAT.failsafe = (uint8_t) buf_status.failsafe;
-			log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
-			log_msg.body.log_STAT.battery_warning = buf_status.battery_warning;
 			log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed;
 			log_msg.body.log_STAT.load = buf_status.load;
 			LOGBUFFER_WRITE_AND_COUNT(STAT);
@@ -1794,6 +1792,18 @@ int sdlog2_thread_main(int argc, char *argv[])
 				LOGBUFFER_WRITE_AND_COUNT(GPOS);
 			}
 
+			/* --- BATTERY --- */
+			if (copy_if_updated(ORB_ID(battery_status), &subs.battery_sub, &buf.battery)) {
+				log_msg.msg_type = LOG_BATT_MSG;
+				log_msg.body.log_BATT.voltage = buf.battery.voltage_v;
+				log_msg.body.log_BATT.voltage_filtered = buf.battery.voltage_filtered_v;
+				log_msg.body.log_BATT.current = buf.battery.current_a;
+				log_msg.body.log_BATT.discharged = buf.battery.discharged_mah;
+				log_msg.body.log_BATT.remaining = buf.battery.remaining;
+				log_msg.body.log_BATT.warning = buf.battery.warning;
+				LOGBUFFER_WRITE_AND_COUNT(BATT);
+			}
+
 			/* --- GLOBAL POSITION SETPOINT --- */
 			if (copy_if_updated(ORB_ID(position_setpoint_triplet), &subs.triplet_sub, &buf.triplet)) {
 

src/modules/sdlog2/sdlog2_messages.h

@@ -180,8 +180,6 @@ struct log_STAT_s {
 	uint8_t main_state;
 	uint8_t arming_state;
 	uint8_t failsafe;
-	float battery_remaining;
-	uint8_t battery_warning;
 	uint8_t landed;
 	float load;
 };
@@ -295,6 +293,8 @@ struct log_BATT_s {
 	float voltage_filtered;
 	float current;
 	float discharged;
+	float remaining;
+	uint8_t warning;
 };
 
 /* --- DIST - RANGE SENSOR DISTANCE --- */
@@ -623,7 +623,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(GPS, "QBffLLfffffBHHH",	"GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"),
 	LOG_FORMAT_S(ATTC, ATTC, "ffff",		"Roll,Pitch,Yaw,Thrust"),
 	LOG_FORMAT_S(ATC1, ATTC, "ffff",		"Roll,Pitch,Yaw,Thrust"),
-	LOG_FORMAT(STAT, "BBBfBBf",		"MainState,ArmS,Failsafe,BatRem,BatWarn,Landed,Load"),
+	LOG_FORMAT(STAT, "BBBBf",		"MainState,ArmS,Failsafe,Landed,Load"),
 	LOG_FORMAT(VTOL, "fBBB",		"Arsp,RwMode,TransMode,Failsafe"),
 	LOG_FORMAT(CTS, "fffffff", "Vx_b,Vy_b,Vz_b,Vinf,P,Q,R"),
 	LOG_FORMAT(RC, "ffffffffffffBBBL",		"C0,C1,C2,C3,C4,C5,C6,C7,C8,C9,C10,C11,RSSI,CNT,Lost,Drop"),
@@ -636,7 +636,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(GPSP, "BLLffBfbf",		"NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
 	LOG_FORMAT(ESC, "HBBBHHffiffH",		"count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
 	LOG_FORMAT(GVSP, "fff",			"VX,VY,VZ"),
-	LOG_FORMAT(BATT, "ffff",		"V,VFilt,C,Discharged"),
+	LOG_FORMAT(BATT, "fffffB",		"V,VFilt,C,Discharged,Remaining,Warning"),
 	LOG_FORMAT(DIST, "BBBff",			"Id,Type,Orientation,Distance,Covariance"),
 	LOG_FORMAT_S(TEL0, TEL, "BBBBHHBQ",		"RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
 	LOG_FORMAT_S(TEL1, TEL, "BBBBHHBQ",		"RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),

src/modules/sensors/sensors.cpp

@@ -83,6 +83,7 @@
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/perf_counter.h>
+#include <systemlib/battery.h>
 #include <conversion/rotation.h>
 
 #include <systemlib/airspeed.h>
@@ -128,9 +129,6 @@ using namespace DriverFramework;
  */
 
 
-#define BATT_V_LOWPASS			0.001f
-#define BATT_V_IGNORE_THRESHOLD		2.1f
-
 /**
  * HACK - true temperature is much less than indicated temperature in baro,
  * subtract 5 degrees in an attempt to account for the electrical upheating of the PCB
@@ -257,6 +255,8 @@ private:
 	uint64_t _battery_discharged;			/**< battery discharged current in mA*ms */
 	hrt_abstime _battery_current_timestamp;		/**< timestamp of last battery current reading */
 
+	Battery		_battery;			/**< Helper lib to publish battery_status topic. */
+
 	struct {
 		float min[_rc_max_chan_count];
 		float trim[_rc_max_chan_count];
@@ -1518,6 +1518,7 @@ Sensors::parameter_update_poll(bool forced)
 
 		/* do not output this for now, as its covered in preflight checks */
 		// warnx("valid configs: %u gyros, %u mags, %u accels", gyro_count, mag_count, accel_count);
+		_battery.updateParams();
 	}
 }
 
@@ -1644,6 +1645,10 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 		/* read all channels available */
 		int ret = _h_adc.read(&buf_adc, sizeof(buf_adc));
 
+		float bat_voltage_v = 0.0f;
+		float bat_current_a = 0.0f;
+		bool updated_battery = false;
+
 		if (ret >= (int)sizeof(buf_adc[0])) {
 
 			/* Read add channels we got */
@@ -1657,48 +1662,12 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 				/* look for specific channels and process the raw voltage to measurement data */
 				if (ADC_BATTERY_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
 					/* Voltage in volts */
-					float voltage = (buf_adc[i].am_data * _parameters.battery_voltage_scaling);
+					bat_voltage_v = (buf_adc[i].am_data * _parameters.battery_voltage_scaling);
-
+					updated_battery = true;
-					if (voltage > BATT_V_IGNORE_THRESHOLD) {
-						_battery_status.voltage_v = voltage;
-
-						/* one-time initialization of low-pass value to avoid long init delays */
-						if (_battery_status.voltage_filtered_v < BATT_V_IGNORE_THRESHOLD) {
-							_battery_status.voltage_filtered_v = voltage;
-						}
-
-						_battery_status.timestamp = t;
-						_battery_status.voltage_filtered_v += (voltage - _battery_status.voltage_filtered_v) * BATT_V_LOWPASS;
-
-					} else {
-						/* mark status as invalid */
-						_battery_status.voltage_v = -1.0f;
-						_battery_status.voltage_filtered_v = -1.0f;
-					}
 
 				} else if (ADC_BATTERY_CURRENT_CHANNEL == buf_adc[i].am_channel) {
-					/* handle current only if voltage is valid */
+					bat_current_a = (buf_adc[i].am_data * _parameters.battery_current_scaling);
-					if (_battery_status.voltage_v > 0.0f) {
+					updated_battery = true;
-						float current = ((buf_adc[i].am_data - _parameters.battery_current_offset) * _parameters.battery_current_scaling);
-
-						/* check measured current value */
-						if (current >= 0.0f) {
-							_battery_status.timestamp = t;
-							_battery_status.current_a = current;
-
-							if (_battery_current_timestamp != 0) {
-								/* initialize discharged value */
-								if (_battery_status.discharged_mah < 0.0f) {
-									_battery_status.discharged_mah = 0.0f;
-								}
-
-								_battery_discharged += current * (t - _battery_current_timestamp);
-								_battery_status.discharged_mah = ((float) _battery_discharged) / 3600000.0f;
-							}
-						}
-					}
-
-					_battery_current_timestamp = t;
 
 #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
 
@@ -1735,16 +1704,21 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 				}
 			}
 
+			if (updated_battery) {
+				// XXX TODO: throttle is hardcoded here. The dependency to throttle would need to be
+				// removed, or it needs to be subscribed to actuator controls.
+				const float throttle = 0.0f;
+				_battery.updateBatteryStatus(t, bat_voltage_v, bat_current_a, throttle, &_battery_status);
+			}
+
 			_last_adc = t;
 
-			if (_battery_status.voltage_filtered_v > BATT_V_IGNORE_THRESHOLD) {
+			/* announce the battery status if needed, just publish else */
-				/* announce the battery status if needed, just publish else */
+			if (_battery_pub != nullptr) {
-				if (_battery_pub != nullptr) {
+				orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
-					orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
 
-				} else {
+			} else {
-					_battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status);
+				_battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status);
-				}
 			}
 		}
 	}
@@ -2158,11 +2132,7 @@ Sensors::task_main()
 	raw.adc_voltage_v[2] = 0.0f;
 	raw.adc_voltage_v[3] = 0.0f;
 
-	memset(&_battery_status, 0, sizeof(_battery_status));
+	_battery.reset(&_battery_status);
-	_battery_status.voltage_v = -1.0f;
-	_battery_status.voltage_filtered_v = -1.0f;
-	_battery_status.current_a = -1.0f;
-	_battery_status.discharged_mah = -1.0f;
 
 	/* get a set of initial values */
 	accel_poll(raw);

src/modules/systemlib/CMakeLists.txt

@@ -48,6 +48,7 @@ set(SRCS
 	mcu_version.c
 	bson/tinybson.c
 	circuit_breaker.cpp
+	battery.cpp
 	)
 
 if(${OS} STREQUAL "nuttx")
@@ -55,7 +56,7 @@ if(${OS} STREQUAL "nuttx")
 		err.c
 		printload.c
 		param/param.c
-		up_cxxinitialize.c 
+		up_cxxinitialize.c
 		)
 elseif ("${CONFIG_SHMEM}" STREQUAL "1")
 	list(APPEND SRCS
@@ -66,7 +67,7 @@ else()
 	list(APPEND SRCS
 		param/param.c
 		print_load_posix.c
-		)	
+		)
 endif()
 
 if(NOT ${OS} STREQUAL "qurt")
@@ -85,4 +86,4 @@ px4_add_module(
 		platforms__common
 		modules__param
 	)
-# vim: set noet ft=cmake fenc=utf-8 ff=unix : 
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :

src/modules/systemlib/battery.cpp

@@ -0,0 +1,162 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file battery.cpp
+ *
+ * Library calls for battery functionality.
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include "battery.h"
+
+Battery::Battery() :
+	SuperBlock(NULL, NULL),
+	_param_v_empty(this, "BAT_V_EMPTY", false),
+	_param_v_full(this, "BAT_V_CHARGED", false),
+	_param_n_cells(this, "BAT_N_CELLS", false),
+	_param_capacity(this, "BAT_CAPACITY", false),
+	_param_v_load_drop(this, "BAT_V_LOAD_DROP", false),
+	_voltage_filtered_v(0.0f),
+	_throttle_filtered(0.0f),
+	_discharged_mah(0.0f),
+	_remaining(1.0f),
+	_warning(battery_status_s::BATTERY_WARNING_NONE),
+	_last_timestamp(0)
+{
+	/* load initial params */
+	updateParams();
+}
+
+Battery::~Battery()
+{
+}
+
+void
+Battery::reset(battery_status_s *battery_status)
+{
+	battery_status->voltage_v = 0.0f;
+	battery_status->voltage_filtered_v = 0.0f;
+	battery_status->current_a = -1.0f;
+	battery_status->discharged_mah = -1.0f;
+	battery_status->cell_count = _param_n_cells.get();
+	// TODO: check if it is sane to reset warning to NONE
+	battery_status->warning = battery_status_s::BATTERY_WARNING_NONE;
+}
+
+void
+Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a, float throttle_normalized,
+			     battery_status_s *battery_status)
+{
+	filterVoltage(voltage_v);
+	sumDischarged(timestamp, current_a);
+	estimateRemaining(voltage_v, throttle_normalized);
+	determineWarning();
+
+	if (_voltage_filtered_v > 2.1f) {
+		battery_status->voltage_v = voltage_v;
+		battery_status->voltage_filtered_v = _voltage_filtered_v;
+		battery_status->current_a = current_a;
+		battery_status->discharged_mah = _discharged_mah;
+		battery_status->cell_count = _param_n_cells.get();
+		battery_status->warning = _warning;
+	} else {
+		reset(battery_status);
+	}
+}
+
+void
+Battery::filterVoltage(float voltage_v)
+{
+	// TODO: inspect that filter performance
+	const float filtered_next = _voltage_filtered_v * 0.999f + voltage_v * 0.001f;
+	if (PX4_ISFINITE(filtered_next)) {
+		_voltage_filtered_v = filtered_next;
+	}
+}
+
+void
+Battery::sumDischarged(hrt_abstime timestamp, float current_a)
+{
+	// Ignore first update because we don't know dT.
+	if (_last_timestamp != 0) {
+		_discharged_mah = current_a * (timestamp - _last_timestamp) * 1.0e-3f / 3600.0f;
+	}
+	_last_timestamp = timestamp;
+}
+
+void
+Battery::estimateRemaining(float voltage_v, float throttle_normalized)
+{
+	// XXX this time constant needs to become tunable but really, the right fix are smart batteries.
+	const float filtered_next = _throttle_filtered * 0.97f + throttle_normalized * 0.03f;
+
+	if (PX4_ISFINITE(filtered_next)) {
+		_throttle_filtered = filtered_next;
+	}
+
+	/* remaining charge estimate based on voltage and internal resistance (drop under load) */
+	const float bat_v_empty_dynamic = _param_v_empty.get() - (_param_v_load_drop.get() * _throttle_filtered);
+	/* the range from full to empty is the same for batteries under load and without load,
+	 * since the voltage drop applies to both the full and empty state
+	 */
+	const float voltage_range = (_param_v_full.get() - _param_v_empty.get());
+	const float remaining_voltage = (voltage_v - (_param_n_cells.get() * bat_v_empty_dynamic))
+		/ (_param_n_cells.get() * voltage_range);
+
+	if (_param_capacity.get() > 0.0f) {
+		/* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */
+		_remaining = fminf(remaining_voltage, 1.0f - _discharged_mah / _param_capacity.get());
+
+	} else {
+		/* else use voltage */
+		_remaining = remaining_voltage;
+	}
+
+	/* limit to sane values */
+	_remaining = (_remaining < 0.0f) ? 0.0f : _remaining;
+	_remaining = (_remaining > 1.0f) ? 1.0f : _remaining;
+}
+
+void
+Battery::determineWarning()
+{
+	// TODO: Determine threshold or make params.
+	if (_remaining < 0.18f) {
+		_warning = battery_status_s::BATTERY_WARNING_LOW;
+
+	} else if (_remaining < 0.09f) {
+		_warning = battery_status_s::BATTERY_WARNING_CRITICAL;
+	}
+}

src/modules/systemlib/battery.h

@@ -0,0 +1,97 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file battery.h
+ *
+ * Library calls for battery functionality.
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#pragma once
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+#include <uORB/topics/battery_status.h>
+#include <drivers/drv_hrt.h>
+
+
+class Battery : public control::SuperBlock
+{
+public:
+	/**
+	 * Constructor
+	 */
+	Battery();
+
+	/**
+	 * Destructor
+	 */
+	~Battery();
+
+	/**
+	 * Reset all battery stats and report invalid/nothing.
+	 */
+	void reset(battery_status_s *battery_status);
+
+	/**
+	 * Update current battery status message.
+	 *
+	 * @param voltage_v: current voltage in V
+	 * @param current_a: current current in A
+	 * @param throttle_normalized: throttle from 0 to 1
+	 */
+	void updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a, float throttle_normalized,
+				 battery_status_s *status);
+
+private:
+	void filterVoltage(float voltage_v);
+	void sumDischarged(hrt_abstime timestamp, float current_a);
+	void estimateRemaining(float voltage_v, float throttle_normalized);
+	void determineWarning();
+
+	control::BlockParamFloat _param_v_empty;
+	control::BlockParamFloat _param_v_full;
+	control::BlockParamFloat _param_n_cells;
+	control::BlockParamFloat _param_capacity;
+	control::BlockParamFloat _param_v_load_drop;
+
+	float _voltage_filtered_v;
+	float _throttle_filtered;
+	float _discharged_mah;
+	float _remaining;
+	uint8_t _warning;
+	hrt_abstime _last_timestamp;
+};
+

src/modules/systemlib/battery_params.c

@@ -0,0 +1,123 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file battery_params.c
+ *
+ * Parameters defined by the battery lib.
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include <px4_config.h>
+#include <systemlib/param/param.h>
+
+/**
+ * Empty cell voltage.
+ *
+ * Defines the voltage where a single cell of the battery is considered empty.
+ *
+ * @group Battery Calibration
+ * @unit V
+ * @decimal 2
+ * @increment 0.01
+ */
+PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f);
+
+/**
+ * Full cell voltage.
+ *
+ * Defines the voltage where a single cell of the battery is considered full.
+ *
+ * @group Battery Calibration
+ * @unit V
+ * @decimal 2
+ * @increment 0.01
+ */
+PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f);
+
+/**
+ * Voltage drop per cell on 100% load
+ *
+ * This implicitely defines the internal resistance
+ * to maximum current ratio and assumes linearity.
+ *
+ * @group Battery Calibration
+ * @unit V
+ * @min 0.0
+ * @max 1.5
+ * @decimal 2
+ * @increment 0.01
+ */
+PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f);
+
+/**
+ * Number of cells.
+ *
+ * Defines the number of cells the attached battery consists of.
+ *
+ * @group Battery Calibration
+ * @unit S
+ * @min 2
+ * @max 10
+ * @value 2 2S Battery
+ * @value 3 3S Battery
+ * @value 4 4S Battery
+ * @value 5 5S Battery
+ * @value 6 6S Battery
+ * @value 7 7S Battery
+ * @value 8 8S Battery
+ * @value 9 9S Battery
+ * @value 10 10S Battery
+ * @value 11 11S Battery
+ * @value 12 12S Battery
+ * @value 13 13S Battery
+ * @value 14 14S Battery
+ * @value 15 15S Battery
+ * @value 16 16S Battery
+ */
+PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
+
+/**
+ * Battery capacity.
+ *
+ * Defines the capacity of the attached battery.
+ *
+ * @group Battery Calibration
+ * @unit mA
+ * @decimal 0
+ * @min -1.0
+ * @max 100000
+ * @increment 50
+ */
+PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);