Created new AnalogBattery class

boards/bitcraze/crazyflie/syslink/syslink_main.cpp

@@ -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(vbat, -1, t, true, 0, 0, false);
+		_battery.updateBatteryStatus(t, vbat, -1, true, true, 0, 0, false, true);
 
 
 		// Update battery charge state

src/drivers/driver_framework_wrapper/df_bebop_bus_wrapper/df_bebop_bus_wrapper.cpp

@@ -220,7 +220,7 @@ int DfBebopBusWrapper::_publish(struct bebop_state_data &data)
 
 		// 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);
+		_battery.updateBatteryStatus(timestamp, data.battery_voltage_v, 0.0, true, true, 0, _last_throttle, _armed, true);
 
 		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

@@ -131,16 +131,17 @@ 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(data.battery_voltage_V, data.battery_current_A, t,
-				     true, 1,
+	_battery.updateBatteryStatus(t, data.battery_voltage_V, data.battery_current_A,
+				     connected, true, 1,
 				     ctrl.control[actuator_controls_s::INDEX_THROTTLE],
-				     vcontrol_mode.flag_armed);
+				     vcontrol_mode.flag_armed, true);
 	return 0;
 }
 

src/lib/battery/CMakeLists.txt

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

src/lib/battery/battery.cpp

@@ -0,0 +1,14 @@
+
+#include "battery.h"
+
+Battery1::Battery1()
+{
+	updateParams();
+
+	migrateParam(_param_old_bat_v_empty, _param_bat_v_empty, "V_EMPTY", 3.5f);
+	migrateParam(_param_old_bat_v_charged, _param_bat_v_charged, "V_CHARGED", 4.05f);
+	migrateParam(_param_old_bat_v_load_drop, _param_bat_v_load_drop, "V_LOAD_DROP", 0.3f);
+	migrateParam(_param_old_bat_r_internal, _param_bat_r_internal, "R_INTERNAL", -1.0f);
+	migrateParam(_param_old_bat_n_cells, _param_bat_n_cells, "N_CELLS", 0);
+	migrateParam(_param_old_bat_capacity, _param_bat_capacity, "CAPACITY", -1.0f);
+}

src/lib/battery/battery.h

@@ -33,12 +33,18 @@
 
 #pragma once
 
+#include "battery_base.h"
+
+#include <px4_log.h>
+#include <math.h>
+
 /**
  * @file battery.h
- * Implementations of BatteryBase
+ * Basic implementation of BatteryBase. Battery1 is calibrated by BAT1_* parameters. Battery2 is calibrated
+ * by BAT2_* parameters.
  *
  * 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.
+ * (Battery 1 uses `BAT1_*`, 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.
  *
@@ -47,54 +53,89 @@
  * 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.
+ * To add a third battery, follow these steps:
+ *  - Copy/Paste all of Battery2 to make Battery3
+ *  - Change all "BAT2_*" parameters to "BAT3_*" in Battery3
+ *  - Copy the file "battery_params_2.c" to "battery_params_3.c"
+ *  - Change all of the "BAT2_*" params in "battery_params_3.c" to "BAT3_*"
+ * This is not done now because there is not yet any demand for a third battery, and adding parameters uses up space.
  */
 
-#include "battery_base.h"
-
+/**
+ * Battery1 represents a battery calibrated by BAT1_* parameters.
+ */
 class Battery1 : public BatteryBase
 {
 public:
-	Battery1() : BatteryBase()
+	Battery1();
+
+	/**
+	 * This function migrates the old deprecated parameters like BAT_N_CELLS to the new parameters like BAT1_N_CELLS.
+	 * It checks if the old parameter is non-defaulT AND the new parameter IS default, and if so, it:
+	 *  - Issues a warning using PX4_WARN(...)
+	 *  - Copies the value of the old parameter over to the new parameter
+	 *  - Resets the old parameter to its default
+	 *
+	 * The 'name' parameter should be only the part of the parameter name that comes after "BAT1_" or "BAT_". It is
+	 * used only for the warning message. For example, for parameter BAT1_N_CELLS, name should be "N_CELLS".
+	 * (See the implementation of this function for why I have taken this strange choice)
+	 *
+	 * In an ideal world, this function would be protected so that only child classes of Battery1 could access it.
+	 * However, the way ModuleParams works makes it very difficult to inherit from a ModuleParams class.
+	 * For example, the AnalogBattery class in the Sensors module does not inherit this class; it just contains
+	 * a Battery1 member variable.
+	 *
+	 * The templating is complicated because every parameter is technically a different type. However, in normal
+	 * use, the template can just be ignored. See the implementation of Battery1::Battery1() for example usage.
+	 *
+	 * @tparam P1 Type of the first parameter
+	 * @tparam P2 Type of the second parameter
+	 * @tparam T Data type for the default value
+	 * @param oldParam Reference to the old parameter, as a ParamFloat<...>, ParamInt<...>, or ParamBool<...>
+	 * @param newParam Reference to the new paramater, as a ParamFloat<...>, ParamInt<...>, or ParamBool<...>
+	 * @param name The name of the parameter, WITHOUT the "BAT_" or "BAT1_" prefix. This is used only for logging.
+	 * @param defaultValue Default value of the parameter, as specified in PARAM_DEFINE_*(...)
+	 */
+	template<class P1, class P2, typename T> static void
+	migrateParam(P1 &oldParam, P2 &newParam, const char *name, T defaultValue)
 	{
-		// Can't do this in the constructor because virtual functions
-		if (_get_adc_channel() >= 0) {
-			vChannel = _get_adc_channel();
-
-		} else {
-			vChannel = DEFAULT_V_CHANNEL[0];
+		float diffOld = fabs((float) oldParam.get() - defaultValue);
+		float diffNew = fabs((float) newParam.get() - defaultValue);
+
+		if (diffOld > 0.0001f && diffNew < 0.0001f) {
+			PX4_WARN("Parameter BAT_%s is deprecated. Copying value to BAT1_%s.", name, name);
+			newParam.set(oldParam.get());
+			oldParam.set(defaultValue);
+			newParam.commit();
+			oldParam.commit();
 		}
-
-		// TODO: Add parameter, like with V
-		iChannel = DEFAULT_I_CHANNEL[0];
 	}
 
 private:
 
 	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::BAT_V_EMPTY>) _param_bat_v_empty,
-		(ParamFloat<px4::params::BAT_V_CHARGED>) _param_bat_v_charged,
-		(ParamInt<px4::params::BAT_N_CELLS>) _param_bat_n_cells,
-		(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_V_EMPTY>) _param_old_bat_v_empty,
+		(ParamFloat<px4::params::BAT_V_CHARGED>) _param_old_bat_v_charged,
+		(ParamInt<px4::params::BAT_N_CELLS>) _param_old_bat_n_cells,
+		(ParamFloat<px4::params::BAT_CAPACITY>) _param_old_bat_capacity,
+		(ParamFloat<px4::params::BAT_V_LOAD_DROP>) _param_old_bat_v_load_drop,
+		(ParamFloat<px4::params::BAT_R_INTERNAL>) _param_old_bat_r_internal,
+
+		(ParamFloat<px4::params::BAT1_V_EMPTY>) _param_bat_v_empty,
+		(ParamFloat<px4::params::BAT1_V_CHARGED>) _param_bat_v_charged,
+		(ParamInt<px4::params::BAT1_N_CELLS>) _param_bat_n_cells,
+		(ParamFloat<px4::params::BAT1_CAPACITY>) _param_bat_capacity,
+		(ParamFloat<px4::params::BAT1_V_LOAD_DROP>) _param_bat_v_load_drop,
+		(ParamFloat<px4::params::BAT1_R_INTERNAL>) _param_bat_r_internal,
 
 		(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
 	)
 
@@ -107,34 +148,16 @@ class Battery1 : public BatteryBase
 	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; }
 };
 
+/**
+ * Battery2 represents a battery calibrated by BAT2_* parameters.
+ */
 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];
-	}
-
+	Battery2() {}
 private:
 
 	DEFINE_PARAMETERS(
@@ -144,16 +167,10 @@ class Battery2 : public BatteryBase
 		(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
 	)
 
@@ -166,13 +183,5 @@ class Battery2 : public BatteryBase
 	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

@@ -63,32 +63,13 @@ BatteryBase::reset()
 	// 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.capacity = _get_bat_capacity();
 }
 
 void
-BatteryBase::updateBatteryStatusRawADC(int32_t voltage_raw, int32_t current_raw, hrt_abstime timestamp,
-				       bool selected_source, int priority,
-				       float throttle_normalized,
-				       bool armed)
+BatteryBase::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a, bool connected,
+				 bool selected_source, int priority, float throttle_normalized, bool armed, bool should_publish)
 {
-
-	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);
@@ -119,11 +100,17 @@ BatteryBase::updateBatteryStatus(float voltage_v, float current_a, hrt_abstime t
 
 	_battery_status.timestamp = timestamp;
 
-	if (_get_source() == 0) {
-		orb_publish_auto(ORB_ID(battery_status), &_orbAdvert, &_battery_status, &_orbInstance, ORB_PRIO_DEFAULT);
+	if (should_publish) {
+		publish();
 	}
 
-	battery_status->temperature = NAN;
+	_battery_status.temperature = NAN;
+}
+
+void
+BatteryBase::publish()
+{
+	orb_publish_auto(ORB_ID(battery_status), &_orbAdvert, &_battery_status, &_orbInstance, ORB_PRIO_DEFAULT);
 }
 
 void
@@ -267,55 +254,3 @@ BatteryBase::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;
-	}
-}