Refactored battery library for multiple instances

boards/bitcraze/crazyflie/syslink/syslink_main.cpp

@@ -288,7 +288,7 @@ Syslink::task_main()
 	_memory = new SyslinkMemory(this);
 	_memory->init();
 
-	_battery.reset(&_battery_status);
+	_battery.reset();
 
 
 	//	int ret;
@@ -411,7 +411,7 @@ Syslink::handle_message(syslink_message_t *msg)
 		memcpy(&vbat, &msg->data[1], sizeof(float));
 		//memcpy(&iset, &msg->data[5], sizeof(float));
 
-		_battery.updateBatteryStatus(t, vbat, -1, true, true, 0, 0, false, &_battery_status);
+		_battery.updateBatteryStatus(vbat, -1, t, true, 0, 0, false);
 
 
 		// Update battery charge state
@@ -427,9 +427,6 @@ Syslink::handle_message(syslink_message_t *msg)
 			_bstate = BAT_DISCHARGING;
 		}
 
-		// announce the battery status if needed, just publish else
-		_battery_pub.publish(_battery_status);
-
 	} else if (msg->type == SYSLINK_RADIO_RSSI) {
 		uint8_t rssi = msg->data[0]; // Between 40 and 100 meaning -40 dBm to -100 dBm
 		_rssi = 140 - rssi * 100 / (100 - 40);

boards/bitcraze/crazyflie/syslink/syslink_main.h

@@ -135,12 +135,9 @@ private:
 	hrt_abstime _params_update[3]; // Time at which the parameters were updated
 	hrt_abstime _params_ack[3]; // Time at which the parameters were acknowledged by the nrf module
 
-	uORB::PublicationMulti<battery_status_s>	_battery_pub{ORB_ID(battery_status)};
 	uORB::PublicationMulti<input_rc_s>		_rc_pub{ORB_ID(input_rc)};
 
-	struct battery_status_s _battery_status;
-
-	Battery _battery;
+	Battery1 _battery;
 
 	int32_t _rssi;
 	battery_state _bstate;

src/drivers/driver_framework_wrapper/df_bebop_bus_wrapper/df_bebop_bus_wrapper.cpp

@@ -101,7 +101,7 @@ private:
 	orb_advert_t _battery_topic;
 	orb_advert_t _esc_topic;
 
-	Battery _battery;
+	Battery1 _battery;
 	bool _armed;
 	float _last_throttle;
 
@@ -201,14 +201,8 @@ int DfBebopBusWrapper::set_esc_speeds(const float speed_scaled[4])
 int DfBebopBusWrapper::_publish(struct bebop_state_data &data)
 {
 
-	battery_status_s battery_report;
 	const hrt_abstime timestamp = hrt_absolute_time();
 
-	// TODO Check if this is the right way for the Bebop
-	// We don't have current measurements
-	_battery.updateBatteryStatus(timestamp, data.battery_voltage_v, 0.0, true, true, 0, _last_throttle, _armed,
-				     &battery_report);
-
 	esc_status_s esc_status = {};
 
 	uint16_t esc_speed_setpoint_rpm[4] = {};
@@ -224,13 +218,9 @@ int DfBebopBusWrapper::_publish(struct bebop_state_data &data)
 	// TODO: when is this ever blocked?
 	if (!(m_pub_blocked)) {
 
-		if (_battery_topic == nullptr) {
-			_battery_topic = orb_advertise_multi(ORB_ID(battery_status), &battery_report,
-							     &_battery_orb_class_instance, ORB_PRIO_LOW);
-
-		} else {
-			orb_publish(ORB_ID(battery_status), _battery_topic, &battery_report);
-		}
+		// TODO Check if this is the right way for the Bebop
+		// We don't have current measurements
+		_battery.updateBatteryStatus(data.battery_voltage_v, 0.0, timestamp, true, 0, _last_throttle, _armed);
 
 		if (_esc_topic == nullptr) {
 			_esc_topic = orb_advertise(ORB_ID(esc_status), &esc_status);

src/drivers/driver_framework_wrapper/df_ltc2946_wrapper/df_ltc2946_wrapper.cpp

@@ -48,7 +48,6 @@
 #include <ltc2946/LTC2946.hpp>
 
 #include <uORB/uORB.h>
-#include <uORB/topics/battery_status.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/vehicle_control_mode.h>
 
@@ -70,9 +69,7 @@ public:
 private:
 	int _publish(const struct ltc2946_sensor_data &data);
 
-	orb_advert_t	_battery_pub{nullptr};
-	battery_status_s _battery_status{};
-	Battery _battery{};
+	Battery1 _battery{};
 
 	int _actuator_ctrl_0_sub{-1};
 	int _vcontrol_mode_sub{-1};
@@ -82,7 +79,7 @@ private:
 DfLtc2946Wrapper::DfLtc2946Wrapper() :
 	LTC2946(LTC2946_DEVICE_PATH)
 {
-	_battery.reset(&_battery_status);
+	_battery.reset();
 
 	// subscriptions
 	_actuator_ctrl_0_sub = orb_subscribe(ORB_ID(actuator_controls_0));
@@ -134,21 +131,16 @@ int DfLtc2946Wrapper::stop()
 int DfLtc2946Wrapper::_publish(const struct ltc2946_sensor_data &data)
 {
 	hrt_abstime t = hrt_absolute_time();
-	bool connected = data.battery_voltage_V > BOARD_ADC_OPEN_CIRCUIT_V;
 
 	actuator_controls_s ctrl;
 	orb_copy(ORB_ID(actuator_controls_0), _actuator_ctrl_0_sub, &ctrl);
 	vehicle_control_mode_s vcontrol_mode;
 	orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode);
 
-	_battery.updateBatteryStatus(t, data.battery_voltage_V, data.battery_current_A,
-				     connected, true, 1,
+	_battery.updateBatteryStatus(data.battery_voltage_V, data.battery_current_A, t,
+				     true, 1,
 				     ctrl.control[actuator_controls_s::INDEX_THROTTLE],
-				     vcontrol_mode.flag_armed,  &_battery_status);
-
-	int instance;
-	orb_publish_auto(ORB_ID(battery_status), &_battery_pub, &_battery_status, &instance, ORB_PRIO_DEFAULT);
-
+				     vcontrol_mode.flag_armed);
 	return 0;
 }
 

src/lib/battery/CMakeLists.txt

@@ -31,4 +31,4 @@
 #
 ############################################################################
 
-px4_add_library(battery battery.cpp)
+px4_add_library(battery battery_base.cpp)

src/lib/battery/battery.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2016, 2017 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019 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
@@ -31,69 +31,52 @@
  *
  ****************************************************************************/
 
-/**
- * @file battery.h
- *
- * Library calls for battery functionality.
- *
- * @author Julian Oes <julian@oes.ch>
- */
-
 #pragma once
 
-#include <uORB/topics/battery_status.h>
-#include <drivers/drv_hrt.h>
-#include <px4_platform_common/module_params.h>
+/**
+ * @file battery.h
+ * Implementations of BatteryBase
+ *
+ * The multiple batteries all share the same logic for calibration. The only difference is which parameters are used
+ * (Battery 1 uses `BAT_*`, while Battery 2 uses `BAT2_*`). To avoid code duplication, inheritance is being used.
+ * The problem is that the `ModuleParams` class depends on a macro which defines member variables. You can't override
+ * member variables in C++, so we have to declare virtual getter functions in BatteryBase, and implement them here.
+ *
+ * The alternative would be to avoid ModuleParams entirely, and build parameter names dynamically, like so:
+ * ```
+ * char param_name[17]; //16 max length of parameter name, + null terminator
+ * int battery_index = 1; // Or 2 or 3 or whatever
+ * snprintf(param_name, 17, "BAT%d_N_CELLS", battery_index);
+ * // A real implementation would have to handle the case where battery_index == 1 and there is no number in the param name.
+ * param_find(param_name); // etc
+ * ```
+ *
+ * This was decided against because the newer ModuleParams API provides more type safety and avoids code duplication.
+ *
+ * To add a new battery, just create a new implementation of BatteryBase and implement all of the _get_* methods,
+ * then add all of the new parameters necessary for calibration.
+ */
 
+#include "battery_base.h"
 
-class Battery : public ModuleParams
+class Battery1 : public BatteryBase
 {
 public:
-	Battery();
+	Battery1() : BatteryBase()
+	{
+		// Can't do this in the constructor because virtual functions
+		if (_get_adc_channel() >= 0) {
+			vChannel = _get_adc_channel();
 
-	/**
-	 * Reset all battery stats and report invalid/nothing.
-	 */
-	void reset(battery_status_s *battery_status);
+		} else {
+			vChannel = DEFAULT_V_CHANNEL[0];
+		}
 
-	/**
-	 * Get the battery cell count
-	 */
-	int cell_count() { return _param_bat_n_cells.get(); }
-
-	/**
-	 * Get the empty voltage per cell
-	 */
-	float empty_cell_voltage() { return _param_bat_v_empty.get(); }
-
-	/**
-	 * Get the full voltage per cell
-	 */
-	float full_cell_voltage() { return _param_bat_v_charged.get(); }
-
-	/**
-	 * Update current battery status message.
-	 *
-	 * @param voltage_v: current voltage in V
-	 * @param current_a: current current in A
-	 * @param connected: Battery is connected
-	 * @param selected_source: This battery is on the brick that the selected source for selected_source
-	 * @param priority: The brick number -1. The term priority refers to the Vn connection on the LTC4417
-	 * @param throttle_normalized: throttle from 0 to 1
-	 */
-	void updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a,
-				 bool connected, bool selected_source, int priority,
-				 float throttle_normalized,
-				 bool armed, battery_status_s *status);
+		// TODO: Add parameter, like with V
+		iChannel = DEFAULT_I_CHANNEL[0];
+	}
 
 private:
-	void filterVoltage(float voltage_v);
-	void filterThrottle(float throttle);
-	void filterCurrent(float current_a);
-	void sumDischarged(hrt_abstime timestamp, float current_a);
-	void estimateRemaining(float voltage_v, float current_a, float throttle, bool armed);
-	void determineWarning(bool connected);
-	void computeScale();
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::BAT_V_EMPTY>) _param_bat_v_empty,
@@ -102,20 +85,94 @@ private:
 		(ParamFloat<px4::params::BAT_CAPACITY>) _param_bat_capacity,
 		(ParamFloat<px4::params::BAT_V_LOAD_DROP>) _param_bat_v_load_drop,
 		(ParamFloat<px4::params::BAT_R_INTERNAL>) _param_bat_r_internal,
+		(ParamFloat<px4::params::BAT_V_DIV>) _param_v_div,
+		(ParamFloat<px4::params::BAT_A_PER_V>) _param_a_per_v,
+		(ParamInt<px4::params::BAT_ADC_CHANNEL>) _param_adc_channel,
+
 		(ParamFloat<px4::params::BAT_LOW_THR>) _param_bat_low_thr,
 		(ParamFloat<px4::params::BAT_CRIT_THR>) _param_bat_crit_thr,
-		(ParamFloat<px4::params::BAT_EMERGEN_THR>) _param_bat_emergen_thr
+		(ParamFloat<px4::params::BAT_EMERGEN_THR>) _param_bat_emergen_thr,
+		(ParamFloat<px4::params::BAT_CNT_V_VOLT>) _param_cnt_v_volt,
+		(ParamFloat<px4::params::BAT_CNT_V_CURR>) _param_cnt_v_curr,
+		(ParamFloat<px4::params::BAT_V_OFFS_CURR>) _param_v_offs_cur,
+		(ParamInt<px4::params::BAT_SOURCE>) _param_source
 	)
 
-	bool _battery_initialized = false;
-	float _voltage_filtered_v = -1.f;
-	float _throttle_filtered = -1.f;
-	float _current_filtered_a = -1.f;
-	float _discharged_mah = 0.f;
-	float _discharged_mah_loop = 0.f;
-	float _remaining_voltage = -1.f;		///< normalized battery charge level remaining based on voltage
-	float _remaining = -1.f;			///< normalized battery charge level, selected based on config param
-	float _scale = 1.f;
-	uint8_t _warning;
-	hrt_abstime _last_timestamp;
+	float _get_bat_v_empty() override {return _param_bat_v_empty.get(); }
+	float _get_bat_v_charged() override {return _param_bat_v_charged.get(); }
+	int _get_bat_n_cells() override {return _param_bat_n_cells.get(); }
+	float _get_bat_capacity() override {return _param_bat_capacity.get(); }
+	float _get_bat_v_load_drop() override {return _param_bat_v_load_drop.get(); }
+	float _get_bat_r_internal() override {return _param_bat_r_internal.get(); }
+	float _get_bat_low_thr() override {return _param_bat_low_thr.get(); }
+	float _get_bat_crit_thr() override {return _param_bat_crit_thr.get(); }
+	float _get_bat_emergen_thr() override {return _param_bat_emergen_thr.get(); }
+	float _get_cnt_v_volt_raw() override {return _param_cnt_v_volt.get(); }
+	float _get_cnt_v_curr_raw() override {return _param_cnt_v_curr.get(); }
+	float _get_v_offs_cur() override {return _param_v_offs_cur.get(); }
+	float _get_v_div_raw() override {return _param_v_div.get(); }
+	float _get_a_per_v_raw() override {return _param_a_per_v.get(); }
+	int _get_source() override {return _param_source.get(); }
+	int _get_adc_channel() override {return _param_adc_channel.get(); }
+
+	int _get_brick_index() override {return 0; }
+};
+
+class Battery2 : public BatteryBase
+{
+public:
+	Battery2() : BatteryBase()
+	{
+		// Can't do this in the constructor because virtual functions
+		if (_get_adc_channel() >= 0) {
+			vChannel = _get_adc_channel();
+
+		} else {
+			vChannel = DEFAULT_V_CHANNEL[1];
+		}
+
+		// TODO: Add parameter, like with V
+		iChannel = DEFAULT_I_CHANNEL[1];
+	}
+
+private:
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::BAT2_V_EMPTY>) _param_bat_v_empty,
+		(ParamFloat<px4::params::BAT2_V_CHARGED>) _param_bat_v_charged,
+		(ParamInt<px4::params::BAT2_N_CELLS>) _param_bat_n_cells,
+		(ParamFloat<px4::params::BAT2_CAPACITY>) _param_bat_capacity,
+		(ParamFloat<px4::params::BAT2_V_LOAD_DROP>) _param_bat_v_load_drop,
+		(ParamFloat<px4::params::BAT2_R_INTERNAL>) _param_bat_r_internal,
+		(ParamFloat<px4::params::BAT2_V_DIV>) _param_v_div,
+		(ParamFloat<px4::params::BAT2_A_PER_V>) _param_a_per_v,
+		(ParamInt<px4::params::BAT2_ADC_CHANNEL>) _param_adc_channel,
+
+		(ParamFloat<px4::params::BAT_LOW_THR>) _param_bat_low_thr,
+		(ParamFloat<px4::params::BAT_CRIT_THR>) _param_bat_crit_thr,
+		(ParamFloat<px4::params::BAT_EMERGEN_THR>) _param_bat_emergen_thr,
+		(ParamFloat<px4::params::BAT_CNT_V_VOLT>) _param_cnt_v_volt,
+		(ParamFloat<px4::params::BAT_CNT_V_CURR>) _param_cnt_v_curr,
+		(ParamFloat<px4::params::BAT_V_OFFS_CURR>) _param_v_offs_cur,
+		(ParamInt<px4::params::BAT_SOURCE>) _param_source
+	)
+
+	float _get_bat_v_empty() override {return _param_bat_v_empty.get(); }
+	float _get_bat_v_charged() override {return _param_bat_v_charged.get(); }
+	int _get_bat_n_cells() override {return _param_bat_n_cells.get(); }
+	float _get_bat_capacity() override {return _param_bat_capacity.get(); }
+	float _get_bat_v_load_drop() override {return _param_bat_v_load_drop.get(); }
+	float _get_bat_r_internal() override {return _param_bat_r_internal.get(); }
+	float _get_bat_low_thr() override {return _param_bat_low_thr.get(); }
+	float _get_bat_crit_thr() override {return _param_bat_crit_thr.get(); }
+	float _get_bat_emergen_thr() override {return _param_bat_emergen_thr.get(); }
+	float _get_cnt_v_volt_raw() override {return _param_cnt_v_volt.get(); }
+	float _get_cnt_v_curr_raw() override {return _param_cnt_v_curr.get(); }
+	float _get_v_offs_cur() override {return _param_v_offs_cur.get(); }
+	float _get_v_div_raw() override {return _param_v_div.get(); }
+	float _get_a_per_v_raw() override {return _param_a_per_v.get(); }
+	int _get_source() override {return _param_source.get(); }
+	int _get_adc_channel() override {return _param_adc_channel.get(); }
+
+	int _get_brick_index() override {return 1; }
 };

src/lib/battery/battery_base.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019 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
@@ -32,19 +32,20 @@
  ****************************************************************************/
 
 /**
- * @file battery.cpp
+ * @file battery_base.cpp
  *
  * Library calls for battery functionality.
  *
  * @author Julian Oes <julian@oes.ch>
+ * @author Timothy Scott <timothy@auterion.com>
  */
 
-#include "battery.h"
+#include "battery_base.h"
 #include <mathlib/mathlib.h>
 #include <cstring>
 #include <px4_platform_common/defines.h>
 
-Battery::Battery() :
+BatteryBase::BatteryBase() :
 	ModuleParams(nullptr),
 	_warning(battery_status_s::BATTERY_WARNING_NONE),
 	_last_timestamp(0)
@@ -52,26 +53,44 @@ Battery::Battery() :
 }
 
 void
-Battery::reset(battery_status_s *battery_status)
+BatteryBase::reset()
 {
-	memset(battery_status, 0, sizeof(*battery_status));
-	battery_status->current_a = -1.f;
-	battery_status->remaining = 1.f;
-	battery_status->scale = 1.f;
-	battery_status->cell_count = _param_bat_n_cells.get();
+	memset(&_battery_status, 0, sizeof(_battery_status));
+	_battery_status.current_a = -1.f;
+	_battery_status.remaining = 1.f;
+	_battery_status.scale = 1.f;
+	_battery_status.cell_count = _get_bat_n_cells();
 	// TODO: check if it is sane to reset warning to NONE
-	battery_status->warning = battery_status_s::BATTERY_WARNING_NONE;
-	battery_status->connected = false;
+	_battery_status.warning = battery_status_s::BATTERY_WARNING_NONE;
+	_battery_status.connected = false;
 }
 
 void
-Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a,
-			     bool connected, bool selected_source, int priority,
-			     float throttle_normalized,
-			     bool armed, battery_status_s *battery_status)
+BatteryBase::updateBatteryStatusRawADC(int32_t voltage_raw, int32_t current_raw, hrt_abstime timestamp,
+				       bool selected_source, int priority,
+				       float throttle_normalized,
+				       bool armed)
 {
-	reset(battery_status);
-	battery_status->timestamp = timestamp;
+
+	float voltage_v = (voltage_raw * _get_cnt_v_volt()) * _get_v_div();
+	float current_a = ((current_raw * _get_cnt_v_curr()) - _get_v_offs_cur()) * _get_a_per_v();
+
+	updateBatteryStatus(voltage_v, current_a, timestamp, selected_source, priority, throttle_normalized, armed);
+}
+
+void
+BatteryBase::updateBatteryStatus(float voltage_v, float current_a, hrt_abstime timestamp,
+				 bool selected_source, int priority,
+				 float throttle_normalized,
+				 bool armed)
+{
+	updateParams();
+
+	bool connected = voltage_v > BOARD_ADC_OPEN_CIRCUIT_V &&
+			 (BOARD_ADC_OPEN_CIRCUIT_V <= BOARD_VALID_UV || is_valid());
+
+	reset();
+	_battery_status.timestamp = timestamp;
 	filterVoltage(voltage_v);
 	filterThrottle(throttle_normalized);
 	filterCurrent(current_a);
@@ -85,24 +104,30 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre
 
 	if (_voltage_filtered_v > 2.1f) {
 		_battery_initialized = true;
-		battery_status->voltage_v = voltage_v;
-		battery_status->voltage_filtered_v = _voltage_filtered_v;
-		battery_status->scale = _scale;
-		battery_status->current_a = current_a;
-		battery_status->current_filtered_a = _current_filtered_a;
-		battery_status->discharged_mah = _discharged_mah;
-		battery_status->warning = _warning;
-		battery_status->remaining = _remaining;
-		battery_status->connected = connected;
-		battery_status->system_source = selected_source;
-		battery_status->priority = priority;
+		_battery_status.voltage_v = voltage_v;
+		_battery_status.voltage_filtered_v = _voltage_filtered_v;
+		_battery_status.scale = _scale;
+		_battery_status.current_a = current_a;
+		_battery_status.current_filtered_a = _current_filtered_a;
+		_battery_status.discharged_mah = _discharged_mah;
+		_battery_status.warning = _warning;
+		_battery_status.remaining = _remaining;
+		_battery_status.connected = connected;
+		_battery_status.system_source = selected_source;
+		_battery_status.priority = priority;
+	}
+
+	_battery_status.timestamp = timestamp;
+
+	if (_get_source() == 0) {
+		orb_publish_auto(ORB_ID(battery_status), &_orbAdvert, &_battery_status, &_orbInstance, ORB_PRIO_DEFAULT);
 	}
 
 	battery_status->temperature = NAN;
 }
 
 void
-Battery::filterVoltage(float voltage_v)
+BatteryBase::filterVoltage(float voltage_v)
 {
 	if (!_battery_initialized) {
 		_voltage_filtered_v = voltage_v;
@@ -117,7 +142,7 @@ Battery::filterVoltage(float voltage_v)
 }
 
 void
-Battery::filterCurrent(float current_a)
+BatteryBase::filterCurrent(float current_a)
 {
 	if (!_battery_initialized) {
 		_current_filtered_a = current_a;
@@ -131,7 +156,7 @@ Battery::filterCurrent(float current_a)
 	}
 }
 
-void Battery::filterThrottle(float throttle)
+void BatteryBase::filterThrottle(float throttle)
 {
 	if (!_battery_initialized) {
 		_throttle_filtered = throttle;
@@ -145,7 +170,7 @@ void Battery::filterThrottle(float throttle)
 }
 
 void
-Battery::sumDischarged(hrt_abstime timestamp, float current_a)
+BatteryBase::sumDischarged(hrt_abstime timestamp, float current_a)
 {
 	// Not a valid measurement
 	if (current_a < 0.f) {
@@ -167,24 +192,24 @@ Battery::sumDischarged(hrt_abstime timestamp, float current_a)
 }
 
 void
-Battery::estimateRemaining(float voltage_v, float current_a, float throttle, bool armed)
+BatteryBase::estimateRemaining(float voltage_v, float current_a, float throttle, bool armed)
 {
 	// remaining battery capacity based on voltage
-	float cell_voltage = voltage_v / _param_bat_n_cells.get();
+	float cell_voltage = voltage_v / _get_bat_n_cells();
 
 	// correct battery voltage locally for load drop to avoid estimation fluctuations
-	if (_param_bat_r_internal.get() >= 0.f) {
-		cell_voltage += _param_bat_r_internal.get() * current_a;
+	if (_get_bat_r_internal() >= 0.f) {
+		cell_voltage += _get_bat_r_internal() * current_a;
 
 	} else {
 		// assume linear relation between throttle and voltage drop
-		cell_voltage += throttle * _param_bat_v_load_drop.get();
+		cell_voltage += throttle * _get_bat_v_load_drop();
 	}
 
-	_remaining_voltage = math::gradual(cell_voltage, _param_bat_v_empty.get(), _param_bat_v_charged.get(), 0.f, 1.f);
+	_remaining_voltage = math::gradual(cell_voltage, _get_bat_v_empty(), _get_bat_v_charged(), 0.f, 1.f);
 
 	// choose which quantity we're using for final reporting
-	if (_param_bat_capacity.get() > 0.f) {
+	if (_get_bat_capacity() > 0.f) {
 		// if battery capacity is known, fuse voltage measurement with used capacity
 		if (!_battery_initialized) {
 			// initialization of the estimation state
@@ -195,7 +220,7 @@ Battery::estimateRemaining(float voltage_v, float current_a, float throttle, boo
 			const float weight_v = 3e-4f * (1 - _remaining_voltage);
 			_remaining = (1 - weight_v) * _remaining + weight_v * _remaining_voltage;
 			// directly apply current capacity slope calculated using current
-			_remaining -= _discharged_mah_loop / _param_bat_capacity.get();
+			_remaining -= _discharged_mah_loop / _get_bat_capacity();
 			_remaining = math::max(_remaining, 0.f);
 		}
 
@@ -206,17 +231,17 @@ Battery::estimateRemaining(float voltage_v, float current_a, float throttle, boo
 }
 
 void
-Battery::determineWarning(bool connected)
+BatteryBase::determineWarning(bool connected)
 {
 	if (connected) {
 		// propagate warning state only if the state is higher, otherwise remain in current warning state
-		if (_remaining < _param_bat_emergen_thr.get()) {
+		if (_remaining < _get_bat_emergen_thr()) {
 			_warning = battery_status_s::BATTERY_WARNING_EMERGENCY;
 
-		} else if (_remaining < _param_bat_crit_thr.get()) {
+		} else if (_remaining < _get_bat_crit_thr()) {
 			_warning = battery_status_s::BATTERY_WARNING_CRITICAL;
 
-		} else if (_remaining < _param_bat_low_thr.get()) {
+		} else if (_remaining < _get_bat_low_thr()) {
 			_warning = battery_status_s::BATTERY_WARNING_LOW;
 
 		} else {
@@ -226,14 +251,14 @@ Battery::determineWarning(bool connected)
 }
 
 void
-Battery::computeScale()
+BatteryBase::computeScale()
 {
-	const float voltage_range = (_param_bat_v_charged.get() - _param_bat_v_empty.get());
+	const float voltage_range = (_get_bat_v_charged() - _get_bat_v_empty());
 
 	// reusing capacity calculation to get single cell voltage before drop
-	const float bat_v = _param_bat_v_empty.get() + (voltage_range * _remaining_voltage);
+	const float bat_v = _get_bat_v_empty() + (voltage_range * _remaining_voltage);
 
-	_scale = _param_bat_v_charged.get() / bat_v;
+	_scale = _get_bat_v_charged() / bat_v;
 
 	if (_scale > 1.3f) { // Allow at most 30% compensation
 		_scale = 1.3f;
@@ -242,3 +267,55 @@ Battery::computeScale()
 		_scale = 1.f;
 	}
 }
+
+float
+BatteryBase::_get_cnt_v_volt()
+{
+	float val = _get_cnt_v_volt_raw();
+
+	if (val < 0.0f) {
+		return 3.3f / 4096.0f;
+
+	} else {
+		return val;
+	}
+}
+
+float
+BatteryBase::_get_cnt_v_curr()
+{
+	float val = _get_cnt_v_curr_raw();
+
+	if (val < 0.0f) {
+		return 3.3f / 4096.0f;
+
+	} else {
+		return val;
+	}
+}
+
+float
+BatteryBase::_get_v_div()
+{
+	float val = _get_v_div_raw();
+
+	if (val <= 0.0f) {
+		return BOARD_BATTERY1_V_DIV;
+
+	} else {
+		return val;
+	}
+}
+
+float
+BatteryBase::_get_a_per_v()
+{
+	float val = _get_a_per_v_raw();
+
+	if (val <= 0.0f) {
+		return BOARD_BATTERY1_A_PER_V;
+
+	} else {
+		return val;
+	}
+}

src/lib/battery/battery_base.h

@@ -0,0 +1,199 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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_base.h
+ *
+ * Library calls for battery functionality.
+ *
+ * @author Julian Oes <julian@oes.ch>
+ * @author Timothy Scott <timothy@auterion.com>
+ */
+
+#pragma once
+
+#include <uORB/uORB.h>
+#include <uORB/topics/battery_status.h>
+#include <drivers/drv_hrt.h>
+#include <px4_module_params.h>
+#include <drivers/drv_adc.h>
+#include <board_config.h>
+#include <drivers/boards/common/board_common.h>
+
+/**
+ * BatteryBase is a base class for any type of battery.
+ *
+ * See battery.h for example implementation, and for explanation of why this is designed like it is.
+ */
+class BatteryBase : ModuleParams
+{
+public:
+	BatteryBase();
+
+	/**
+	 * Reset all battery stats and report invalid/nothing.
+	 */
+	void reset();
+
+	/**
+	 * Get the battery cell count
+	 */
+	int cell_count() { return _get_bat_n_cells(); }
+
+	/**
+	 * Get the empty voltage per cell
+	 */
+	float empty_cell_voltage() { return _get_bat_v_empty(); }
+
+	/**
+	 * Get the full voltage per cell
+	 */
+	float full_cell_voltage() { return _get_bat_v_charged(); }
+
+	/**
+	 * Update current battery status message.
+	 *
+	 * @param voltage_raw Battery voltage read from ADC, in raw ADC counts
+	 * @param current_raw Voltage of current sense resistor, in raw ADC counts
+	 * @param timestamp Time at which the ADC was read (use hrt_absolute_time())
+	 * @param selected_source This battery is on the brick that the selected source for selected_source
+	 * @param priority: The brick number -1. The term priority refers to the Vn connection on the LTC4417
+	 * @param throttle_normalized Throttle of the vehicle, between 0 and 1
+	 * @param armed Arming state of the vehicle
+	 */
+	void updateBatteryStatusRawADC(int32_t voltage_raw, int32_t current_raw, hrt_abstime timestamp,
+				       bool selected_source, int priority, float throttle_normalized, bool armed);
+
+	/**
+	 * Update current battery status message.
+	 *
+	 * @param voltage_raw Battery voltage read from ADC, in Volts
+	 * @param current_raw Voltage of current sense resistor, in Amps
+	 * @param timestamp Time at which the ADC was read (use hrt_absolute_time())
+	 * @param selected_source This battery is on the brick that the selected source for selected_source
+	 * @param priority: The brick number -1. The term priority refers to the Vn connection on the LTC4417
+	 * @param throttle_normalized Throttle of the vehicle, between 0 and 1
+	 * @param armed Arming state of the vehicle
+	 */
+	void updateBatteryStatus(float voltage_v, float current_a, hrt_abstime timestamp,
+				 bool selected_source, int priority, float throttle_normalized, bool armed);
+
+	/**
+	 * Which ADC channel is used for voltage reading of this battery
+	 */
+	int vChannel{-1};
+	/**
+	 * Which ADC channel is used for current reading of this battery
+	 */
+	int iChannel{-1};
+
+	/**
+	 * Whether the ADC channel for the voltage of this battery is valid.
+	 * Corresponds to BOARD_BRICK_VALID_LIST
+	 */
+	bool is_valid()
+	{
+#ifdef BOARD_BRICK_VALID_LIST
+		bool valid[BOARD_NUMBER_BRICKS] = BOARD_BRICK_VALID_LIST;
+		return valid[_get_brick_index()];
+#else
+		return true;
+#endif
+	}
+
+protected:
+	// Defaults to use if the parameters are not set
+#if BOARD_NUMBER_BRICKS > 0
+#if defined(BOARD_BATT_V_LIST) && defined(BOARD_BATT_I_LIST)
+	static constexpr int   DEFAULT_V_CHANNEL[BOARD_NUMBER_BRICKS] = BOARD_BATT_V_LIST;
+	static constexpr int   DEFAULT_I_CHANNEL[BOARD_NUMBER_BRICKS] = BOARD_BATT_I_LIST;
+#else
+	static constexpr int   DEFAULT_V_CHANNEL[BOARD_NUMBER_BRICKS] = {0};
+	static constexpr int   DEFAULT_I_CHANNEL[BOARD_NUMBER_BRICKS] = {0};
+#endif
+#else
+	static constexpr int DEFAULT_V_CHANNEL[0] = {};
+	static constexpr int DEFAULT_I_CHANNEL[0] = {};
+#endif
+
+	// The following are all of the parameters needed for the batteries.
+	// See battery.h for example implementation.
+	virtual float _get_bat_v_empty() = 0;
+	virtual float _get_bat_v_charged() = 0;
+	virtual int _get_bat_n_cells() = 0;
+	virtual float _get_bat_capacity() = 0;
+	virtual float _get_bat_v_load_drop() = 0;
+	virtual float _get_bat_r_internal() = 0;
+	virtual float _get_bat_low_thr() = 0;
+	virtual float _get_bat_crit_thr() = 0;
+	virtual float _get_bat_emergen_thr() = 0;
+	virtual float _get_cnt_v_volt_raw() = 0;
+	virtual float _get_cnt_v_curr_raw() = 0;
+	virtual float _get_v_offs_cur() = 0;
+	virtual float _get_v_div_raw() = 0;
+	virtual float _get_a_per_v_raw() = 0;
+	virtual int _get_source() = 0;
+	virtual int _get_adc_channel() = 0;
+
+	virtual int _get_brick_index() = 0;
+
+	float _get_cnt_v_volt();
+	float _get_cnt_v_curr();
+	float _get_v_div();
+	float _get_a_per_v();
+
+private:
+	void filterVoltage(float voltage_v);
+	void filterThrottle(float throttle);
+	void filterCurrent(float current_a);
+	void sumDischarged(hrt_abstime timestamp, float current_a);
+	void estimateRemaining(float voltage_v, float current_a, float throttle, bool armed);
+	void determineWarning(bool connected);
+	void computeScale();
+
+	bool _battery_initialized = false;
+	float _voltage_filtered_v = -1.f;
+	float _throttle_filtered = -1.f;
+	float _current_filtered_a = -1.f;
+	float _discharged_mah = 0.f;
+	float _discharged_mah_loop = 0.f;
+	float _remaining_voltage = -1.f;		///< normalized battery charge level remaining based on voltage
+	float _remaining = -1.f;			///< normalized battery charge level, selected based on config param
+	float _scale = 1.f;
+	uint8_t _warning;
+	hrt_abstime _last_timestamp;
+
+	orb_advert_t _orbAdvert{nullptr};
+	int _orbInstance;
+	battery_status_s _battery_status;
+};

src/lib/battery/battery_params_1.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2019 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
@@ -32,11 +32,11 @@
  ****************************************************************************/
 
 /**
- * @file battery_params.c
+ * @file battery_params_1.c
+ * @author Timothy Scott <timothy@auterion.com>
  *
- * Parameters defined by the battery lib.
- *
- * @author Julian Oes <julian@oes.ch>
+ * Defines parameters for Battery 1. For backwards compatibility, the
+ * parameter names do not have a "1" in them.
  */
 
 #include <px4_platform_common/px4_config.h>
@@ -45,7 +45,7 @@
 /**
  * Empty cell voltage (5C load)
  *
- * Defines the voltage where a single cell of the battery is considered empty.
+ * Defines the voltage where a single cell of battery 1 is considered empty.
  * The voltage should be chosen before the steep dropoff to 2.8V. A typical
  * lithium battery can only be discharged down to 10% before it drops off
  * to a voltage level damaging the cells.
@@ -61,7 +61,7 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.5f);
 /**
  * Full cell voltage (5C load)
  *
- * Defines the voltage where a single cell of the battery is considered full
+ * Defines the voltage where a single cell of battery 1 is considered full
  * under a mild load. This will never be the nominal voltage of 4.2V
  *
  * @group Battery Calibration
@@ -72,61 +72,11 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.5f);
  */
 PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.05f);
 
-/**
- * Low threshold
- *
- * Sets the threshold when the battery will be reported as low.
- * This has to be higher than the critical threshold.
- *
- * @group Battery Calibration
- * @unit norm
- * @min 0.12
- * @max 0.4
- * @decimal 2
- * @increment 0.01
- * @reboot_required true
- */
-PARAM_DEFINE_FLOAT(BAT_LOW_THR, 0.15f);
-
-/**
- * Critical threshold
- *
- * Sets the threshold when the battery will be reported as critically low.
- * This has to be lower than the low threshold. This threshold commonly
- * will trigger RTL.
- *
- * @group Battery Calibration
- * @unit norm
- * @min 0.05
- * @max 0.1
- * @decimal 2
- * @increment 0.01
- * @reboot_required true
- */
-PARAM_DEFINE_FLOAT(BAT_CRIT_THR, 0.07f);
-
-/**
- * Emergency threshold
- *
- * Sets the threshold when the battery will be reported as dangerously low.
- * This has to be lower than the critical threshold. This threshold commonly
- * will trigger landing.
- *
- * @group Battery Calibration
- * @unit norm
- * @min 0.03
- * @max 0.07
- * @decimal 2
- * @increment 0.01
- * @reboot_required true
- */
-PARAM_DEFINE_FLOAT(BAT_EMERGEN_THR, 0.05f);
-
 /**
  * Voltage drop per cell on full throttle
  *
  * This implicitely defines the internal resistance
- * to maximum current ratio and assumes linearity.
+ * to maximum current ratio for battery 1 and assumes linearity.
  * A good value to use is the difference between the
  * 5C and 20-25C load. Not used if BAT_R_INTERNAL is
  * set.
@@ -142,7 +92,7 @@ PARAM_DEFINE_FLOAT(BAT_EMERGEN_THR, 0.05f);
 PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.3f);
 
 /**
- * Explicitly defines the per cell internal resistance
+ * Explicitly defines the per cell internal resistance for battery 1
  *
  * If non-negative, then this will be used in place of
  * BAT_V_LOAD_DROP for all calculations.
@@ -155,8 +105,9 @@ PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.3f);
  */
 PARAM_DEFINE_FLOAT(BAT_R_INTERNAL, -1.0f);
 
+
 /**
- * Number of cells.
+ * Number of cells for battery 1.
  *
  * Defines the number of cells the attached battery consists of.
  *
@@ -183,9 +134,9 @@ PARAM_DEFINE_FLOAT(BAT_R_INTERNAL, -1.0f);
 PARAM_DEFINE_INT32(BAT_N_CELLS, 0);
 
 /**
- * Battery capacity.
+ * Battery 1 capacity.
  *
- * Defines the capacity of the attached battery.
+ * Defines the capacity of battery 1.
  *
  * @group Battery Calibration
  * @unit mAh
@@ -196,3 +147,38 @@ PARAM_DEFINE_INT32(BAT_N_CELLS, 0);
  * @reboot_required true
  */
 PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
+
+/**
+ * Battery 1 voltage divider (V divider)
+ *
+ * This is the divider from battery 1 voltage to 3.3V ADC voltage.
+ * If using e.g. Mauch power modules the value from the datasheet
+ * can be applied straight here. A value of -1 means to use
+ * the board default.
+ *
+ * @group Battery Calibration
+ * @decimal 8
+ */
+PARAM_DEFINE_FLOAT(BAT_V_DIV, -1.0);
+
+/**
+ * Battery 1 current per volt (A/V)
+ *
+ * The voltage seen by the 3.3V ADC multiplied by this factor
+ * will determine the battery current. A value of -1 means to use
+ * the board default.
+ *
+ * @group Battery Calibration
+ * @decimal 8
+ */
+PARAM_DEFINE_FLOAT(BAT_A_PER_V, -1.0);
+
+/**
+ * Battery 1 ADC Channel
+ *
+ * This parameter specifies the ADC channel used to monitor voltage of main power battery.
+ * A value of -1 means to use the board default.
+ *
+ * @group Battery Calibration
+ */
+PARAM_DEFINE_INT32(BAT_ADC_CHANNEL, -1);

src/lib/battery/battery_params_2.c

@@ -0,0 +1,180 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2017 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_2.c
+ * @author Timothy Scott <timothy@auterion.com>
+ *
+ * Defines parameters for Battery 2.
+ */
+
+/**
+ * Empty cell voltage (5C load)
+ *
+ * Defines the voltage where a single cell of battery 2 is considered empty.
+ * The voltage should be chosen before the steep dropoff to 2.8V. A typical
+ * lithium battery can only be discharged down to 10% before it drops off
+ * to a voltage level damaging the cells.
+ *
+ * @group Battery Calibration
+ * @unit V
+ * @decimal 2
+ * @increment 0.01
+ * @reboot_required true
+ */
+PARAM_DEFINE_FLOAT(BAT2_V_EMPTY, 3.5f);
+
+/**
+ * Full cell voltage (5C load)
+ *
+ * Defines the voltage where a single cell of battery 2 is considered full
+ * under a mild load. This will never be the nominal voltage of 4.2V
+ *
+ * @group Battery Calibration
+ * @unit V
+ * @decimal 2
+ * @increment 0.01
+ * @reboot_required true
+ */
+PARAM_DEFINE_FLOAT(BAT2_V_CHARGED, 4.05f);
+
+/**
+ * Voltage drop per cell on full throttle
+ *
+ * This implicitely defines the internal resistance
+ * to maximum current ratio for battery 2 and assumes linearity.
+ * A good value to use is the difference between the
+ * 5C and 20-25C load. Not used if BAT2_R_INTERNAL is
+ * set.
+ *
+ * @group Battery Calibration
+ * @unit V
+ * @min 0.07
+ * @max 0.5
+ * @decimal 2
+ * @increment 0.01
+ * @reboot_required true
+ */
+PARAM_DEFINE_FLOAT(BAT2_V_LOAD_DROP, 0.3f);
+
+/**
+ * Explicitly defines the per cell internal resistance for battery 2
+ *
+ * If non-negative, then this will be used in place of
+ * BAT2_V_LOAD_DROP for all calculations.
+ *
+ * @group Battery Calibration
+ * @unit Ohms
+ * @min -1.0
+ * @max 0.2
+ * @reboot_required true
+ */
+PARAM_DEFINE_FLOAT(BAT2_R_INTERNAL, -1.0f);
+
+
+/**
+ * Number of cells for battery 2.
+ *
+ * Defines the number of cells the attached battery consists of.
+ *
+ * @group Battery Calibration
+ * @unit S
+ * @value 0 Unconfigured
+ * @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
+ * @reboot_required true
+ */
+PARAM_DEFINE_INT32(BAT2_N_CELLS, 0);
+
+/**
+ * Battery 2 capacity.
+ *
+ * Defines the capacity of battery 2.
+ *
+ * @group Battery Calibration
+ * @unit mAh
+ * @decimal 0
+ * @min -1.0
+ * @max 100000
+ * @increment 50
+ * @reboot_required true
+ */
+PARAM_DEFINE_FLOAT(BAT2_CAPACITY, -1.0f);
+
+/**
+ * Battery 2 voltage divider (V divider)
+ *
+ * This is the divider from battery 2 voltage to 3.3V ADC voltage.
+ * If using e.g. Mauch power modules the value from the datasheet
+ * can be applied straight here. A value of -1 means to use
+ * the board default.
+ *
+ * @group Battery Calibration
+ * @decimal 8
+ */
+PARAM_DEFINE_FLOAT(BAT2_V_DIV, -1.0);
+
+/**
+ * Battery 2 current per volt (A/V)
+ *
+ * The voltage seen by the 3.3V ADC multiplied by this factor
+ * will determine the battery current. A value of -1 means to use
+ * the board default.
+ *
+ * @group Battery Calibration
+ * @decimal 8
+ */
+PARAM_DEFINE_FLOAT(BAT2_A_PER_V, -1.0);
+
+/**
+ * Battery 2 ADC Channel
+ *
+ * This parameter specifies the ADC channel used to monitor voltage of main power battery.
+ * A value of -1 means to use the board default.
+ *
+ * @group Battery Calibration
+ */
+PARAM_DEFINE_INT32(BAT2_ADC_CHANNEL, -1);

src/lib/battery/battery_params_common.c

@@ -0,0 +1,144 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2017 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, shared between all batteries.
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include <px4_config.h>
+#include <parameters/param.h>
+
+
+/**
+ * Low threshold
+ *
+ * Sets the threshold when the battery will be reported as low.
+ * This has to be higher than the critical threshold.
+ *
+ * @group Battery Calibration
+ * @unit norm
+ * @min 0.12
+ * @max 0.4
+ * @decimal 2
+ * @increment 0.01
+ * @reboot_required true
+ */
+PARAM_DEFINE_FLOAT(BAT_LOW_THR, 0.15f);
+
+/**
+ * Critical threshold
+ *
+ * Sets the threshold when the battery will be reported as critically low.
+ * This has to be lower than the low threshold. This threshold commonly
+ * will trigger RTL.
+ *
+ * @group Battery Calibration
+ * @unit norm
+ * @min 0.05
+ * @max 0.1
+ * @decimal 2
+ * @increment 0.01
+ * @reboot_required true
+ */
+PARAM_DEFINE_FLOAT(BAT_CRIT_THR, 0.07f);
+
+/**
+ * Emergency threshold
+ *
+ * Sets the threshold when the battery will be reported as dangerously low.
+ * This has to be lower than the critical threshold. This threshold commonly
+ * will trigger landing.
+ *
+ * @group Battery Calibration
+ * @unit norm
+ * @min 0.03
+ * @max 0.07
+ * @decimal 2
+ * @increment 0.01
+ * @reboot_required true
+ */
+PARAM_DEFINE_FLOAT(BAT_EMERGEN_THR, 0.05f);
+
+/**
+ * Scaling from ADC counts to volt on the ADC input (battery voltage)
+ *
+ * This is not the battery voltage, but the intermediate ADC voltage.
+ * A value of -1 signifies that the board defaults are used, which is
+ * highly recommended.
+ *
+ * @group Battery Calibration
+ * @decimal 8
+ */
+PARAM_DEFINE_FLOAT(BAT_CNT_V_VOLT, -1.0f);
+
+/**
+ * Scaling from ADC counts to volt on the ADC input (battery current)
+ *
+ * This is not the battery current, but the intermediate ADC voltage.
+ * A value of -1 signifies that the board defaults are used, which is
+ * highly recommended.
+ *
+ * @group Battery Calibration
+ * @decimal 8
+ */
+PARAM_DEFINE_FLOAT(BAT_CNT_V_CURR, -1.0);
+
+/**
+ * Offset in volt as seen by the ADC input of the current sensor.
+ *
+ * This offset will be subtracted before calculating the battery
+ * current based on the voltage.
+ *
+ * @group Battery Calibration
+ * @decimal 8
+ */
+PARAM_DEFINE_FLOAT(BAT_V_OFFS_CURR, 0.0);
+
+/**
+ * Battery monitoring source.
+ *
+ * This parameter controls the source of battery data. The value 'Power Module'
+ * means that measurements are expected to come from a power module. If the value is set to
+ * 'External' then the system expects to receive mavlink battery status messages.
+ *
+ * @min 0
+ * @max 1
+ * @value 0 Power Module
+ * @value 1 External
+ * @group Battery Calibration
+ */
+PARAM_DEFINE_INT32(BAT_SOURCE, 0);

src/modules/sensors/CMakeLists.txt

@@ -43,6 +43,7 @@ px4_add_module(
 		sensors.cpp
 		parameters.cpp
 		temperature_compensation.cpp
+		power.cpp
 
 	DEPENDS
 		airspeed

src/modules/sensors/power.cpp

@@ -0,0 +1,91 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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 power.cpp
+ *
+ * @author Timothy Scott <timothy@auterion.com>
+ */
+#include "power.h"
+
+Power::Power() {}
+
+void Power::update(px4_adc_msg_t buf_adc[PX4_MAX_ADC_CHANNELS], int nchannels, float throttle, bool armed)
+{
+
+#if BOARD_NUMBER_BRICKS > 0
+	/* For legacy support we publish the battery_status for the Battery that is
+	 * associated with the Brick that is the selected source for VDD_5V_IN
+	 * Selection is done in HW ala a LTC4417 or similar, or may be hard coded
+	 * Like in the FMUv4
+	 */
+
+	/* Per Brick readings with default unread channels at 0 */
+	int32_t bat_current_cnt[BOARD_NUMBER_BRICKS] = {0};
+	int32_t bat_voltage_cnt[BOARD_NUMBER_BRICKS] = {0};
+
+	// The channel readings are not necessarily in a nice order, so we iterate through
+	// to find every relevant channel.
+	for (int i = 0; i < nchannels; i++) {
+		for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) {
+			/* look for specific channels and process the raw voltage to measurement data */
+			if (_analogBatteries[b]->vChannel == buf_adc[i].am_channel) {
+				/* Voltage in ADC counts */
+				bat_voltage_cnt[b] = buf_adc[i].am_data;
+
+			} else if (_analogBatteries[b]->iChannel == buf_adc[i].am_channel) {
+				/* Voltage at current sense resistor in ADC counts */
+				bat_current_cnt[b] = buf_adc[i].am_data;
+			}
+		}
+	}
+
+	/* Based on the valid_chan, used to indicate the selected the lowest index
+	 * (highest priority) supply that is the source for the VDD_5V_IN
+	 * When < 0 none selected
+	 */
+
+	int selected_source = -1;
+
+	for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) {
+		if (_analogBatteries[b]->is_valid() && selected_source < 0) {
+			selected_source = b;
+		}
+
+		_analogBatteries[b]->updateBatteryStatusRawADC(bat_voltage_cnt[b], bat_current_cnt[b], hrt_absolute_time(),
+				selected_source == b, b, throttle, armed);
+	}
+
+#endif /* BOARD_NUMBER_BRICKS > 0 */
+
+}

src/modules/sensors/power.h

@@ -0,0 +1,112 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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 power.h
+ *
+ * @author Timothy Scott <timothy@auterion.com>
+ */
+
+#pragma once
+
+#include <board_config.h>
+#include <battery/battery.h>
+
+#ifdef BOARD_NUMBER_DIGITAL_BRICKS
+#define TOTAL_BRICKS (BOARD_NUMBER_BRICKS + BOARD_NUMBER_DIGITAL_BRICKS)
+#else
+#define TOTAL_BRICKS BOARD_NUMBER_BRICKS
+#endif
+
+/**
+ * Measures voltage, current, etc. of all batteries connected to the vehicle, both
+ * analog and digital.
+ */
+class Power
+{
+public:
+	Power();
+
+	/**
+	 * Updates the measurements of each battery.
+	 *
+	 * If the parameter `BAT_SOURCE` == 0, this function will also publish an instance of the uORB topic
+	 * `battery_status` for each battery. For reasons of backwards compability, instance 0 will always be the
+	 * primary battery. However, this may change in the future! In the future, please use orb_priority() to find
+	 * the primary battery.
+	 * @param buf_adc Buffer of ADC readings
+	 * @param nchannels Number of valid ADC readings in `buf_adc`
+	 * @param throttle Normalized throttle (between 0 and 1, or maybe between -1 and 1 in the future)
+	 * @param armed True if the vehicle is armed
+	 */
+	void update(px4_adc_msg_t buf_adc[PX4_MAX_ADC_CHANNELS], int nchannels, float throttle, bool armed);
+
+private:
+
+	/*
+	 * All of these #if's are doing one thing: Building an array of `BatteryBase` objects, one
+	 * for each possible connected battery. A `BatteryBase` object does not mean that a battery IS connected,
+	 * it just means that one CAN be connected.
+	 *
+	 * For an example of what this looks like after preprocessing, assume that BOARD_NUMBER_BRICKS = 2:
+	 * ```
+	 * Battery1 _battery0;
+	 * Battery2 _battery1;
+	 *
+	 * BatteryBase *_analogBatteries[2] {
+	 *     &_battery0,
+	 *     &_battery1,
+	 * }
+	 * ```
+	 *
+	 * The #if BOARD_NUMBER_BRICKS > 0 wraps the entire declaration because otherwise, an empty array is declared
+	 * which then is unused. In some configurations, an unused variable throws a compile error.
+	 */
+
+	// TODO: Add digital batteries
+
+#if BOARD_NUMBER_BRICKS > 0
+	Battery1 _battery0;
+
+#if BOARD_NUMBER_BRICKS > 1
+	Battery2 _battery1;
+#endif
+
+	BatteryBase *_analogBatteries[BOARD_NUMBER_BRICKS] {
+		&_battery0,
+#if BOARD_NUMBER_BRICKS > 1
+		&_battery1,
+#endif
+	}; // End _analogBatteries
+#endif // End #if BOARD_NUMBER_BRICKS > 0
+};

src/modules/sensors/sensor_params_battery.c

@@ -1,57 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012-2019 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.
- *
- ****************************************************************************/
-
-/**
- * Battery voltage divider (V divider)
- *
- * This is the divider from battery voltage to 3.3V ADC voltage.
- * If using e.g. Mauch power modules the value from the datasheet
- * can be applied straight here. A value of -1 means to use
- * the board default.
- *
- * @group Battery Calibration
- * @decimal 8
- */
-PARAM_DEFINE_FLOAT(BAT_V_DIV, -1.0);
-
-/**
- * Battery current per volt (A/V)
- *
- * The voltage seen by the 3.3V ADC multiplied by this factor
- * will determine the battery current. A value of -1 means to use
- * the board default.
- *
- * @group Battery Calibration
- * @decimal 8
- */
-PARAM_DEFINE_FLOAT(BAT_A_PER_V, -1.0);

src/modules/sensors/sensors.cpp

@@ -142,7 +142,6 @@ private:
 
 	VotedSensorsUpdate _voted_sensors_update;
 
-
 	VehicleAcceleration	_vehicle_acceleration;
 	VehicleAngularVelocity	_vehicle_angular_velocity;
 

src/modules/simulator/simulator.h

@@ -239,7 +239,7 @@ private:
 	hrt_abstime _last_sitl_timestamp{0};
 
 	// Lib used to do the battery calculations.
-	Battery _battery {};
+	Battery1 _battery {};
 	battery_status_s _battery_status{};
 
 #ifndef __PX4_QURT

src/modules/simulator/simulator_mavlink.cpp

@@ -370,10 +370,7 @@ void Simulator::handle_message_hil_sensor(const mavlink_message_t *msg)
 		vbatt *= _battery.cell_count();
 
 		const float throttle = 0.0f; // simulate no throttle compensation to make the estimate predictable
-		_battery.updateBatteryStatus(now_us, vbatt, ibatt, true, true, 0, throttle, armed, &_battery_status);
-
-		// publish the battery voltage
-		_battery_pub.publish(_battery_status);
+		_battery.updateBatteryStatus(vbatt, ibatt, now_us, true, 0, throttle, armed);
 	}
 }