uavcan battery: add support for cuav::equipment::power::CBAT message

emitted by e.g. uavcan Tattu batteries.

Author: MaEtUgR

src/drivers/uavcan/Kconfig

@@ -47,7 +47,7 @@ if DRIVERS_UAVCAN
         default y
 
     config UAVCAN_SENSOR_BATTERY
-        bool "Subscribe to Battery:                     uavcan::equipment::power::BatteryInfo | ardupilot::equipment::power::BatteryInfoAux"
+        bool "Subscribe to Battery:                     uavcan::equipment::power::BatteryInfo | ardupilot::equipment::power::BatteryInfoAux | cuav::equipment::power::CBAT"
         default y
 
     config UAVCAN_SENSOR_DIFFERENTIAL_PRESSURE

src/drivers/uavcan/sensors/battery.cpp

@@ -44,6 +44,7 @@ UavcanBatteryBridge::UavcanBatteryBridge(uavcan::INode &node) :
 	ModuleParams(nullptr),
 	_sub_battery(node),
 	_sub_battery_aux(node),
+	_sub_cbat(node),
 	_warning(battery_status_s::WARNING_NONE),
 	_last_timestamp(0)
 {
@@ -78,6 +79,13 @@ int UavcanBatteryBridge::init()
 		return res;
 	}
 
+	res = _sub_cbat.start(CBATCbBinder(this, &UavcanBatteryBridge::cbat_sub_cb));
+
+	if (res < 0) {
+		PX4_ERR("failed to start uavcan sub: %d", res);
+		return res;
+	}
+
 	return 0;
 }
 
@@ -92,7 +100,8 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::
 		}
 	}
 
-	if (instance >= battery_status_s::MAX_INSTANCES) {
+	if (instance >= battery_status_s::MAX_INSTANCES
+	    || _batt_update_mod[instance] == BatteryDataType::CBAT) {
 		return;
 	}
 
@@ -165,11 +174,9 @@ UavcanBatteryBridge::battery_aux_sub_cb(const uavcan::ReceivedDataStructure<ardu
 		}
 	}
 
-	if (instance >= battery_status_s::MAX_INSTANCES) {
-		return;
-	}
-
-	if (_batt_update_mod[instance] == BatteryDataType::Filter) {
+	if (instance >= battery_status_s::MAX_INSTANCES
+	    || _batt_update_mod[instance] == BatteryDataType::Filter
+	    || _batt_update_mod[instance] == BatteryDataType::CBAT) {
 		return;
 	}
 
@@ -197,6 +204,90 @@ UavcanBatteryBridge::battery_aux_sub_cb(const uavcan::ReceivedDataStructure<ardu
 	}
 }
 
+void UavcanBatteryBridge::cbat_sub_cb(const uavcan::ReceivedDataStructure<cuav::equipment::power::CBAT> &msg)
+{
+	uint8_t instance = 0;
+
+	for (instance = 0; instance < battery_status_s::MAX_INSTANCES; instance++) {
+		if (_battery_status[instance].id == msg.getSrcNodeID().get()) {
+			break;
+		}
+	}
+
+	if (instance >= battery_status_s::MAX_INSTANCES
+	    || _batt_update_mod[instance] == BatteryDataType::Filter) {
+		return;
+	}
+
+	// If CBAT message with superset of data was received, skip BatteryInfo messages
+	_batt_update_mod[instance] = BatteryDataType::CBAT;
+
+	_battery_status[instance].timestamp = hrt_absolute_time();
+	_battery_status[instance].voltage_v = msg.voltage;
+	_battery_status[instance].current_a = msg.current;
+	_battery_status[instance].current_average_a = msg.average_current;
+	_battery_status[instance].discharged_mah =
+		(msg.full_charge_capacity - msg.remaining_capacity) / msg.nominal_voltage * 1000.f;
+	_battery_status[instance].remaining = msg.state_of_charge / 100.f;
+	_battery_status[instance].scale = -1;
+	_battery_status[instance].temperature = msg.temperature + atmosphere::kAbsoluteNullCelsius; // Kelvin to Celsius
+	_battery_status[instance].full_charge_capacity_wh =
+		msg.nominal_voltage * msg.full_charge_capacity / 1000.f; // mAh -> Wh
+	_battery_status[instance].remaining_capacity_wh = msg.nominal_voltage * msg.remaining_capacity / 1000.f; // mAh -> Wh
+	_battery_status[instance].nominal_voltage = msg.nominal_voltage;
+	_battery_status[instance].capacity = msg.design_capacity; // mAh
+	_battery_status[instance].cycle_count = msg.cycle_count;
+	_battery_status[instance].average_time_to_empty = msg.average_time_to_empty;
+	_battery_status[instance].manufacture_date = msg.manufacture_date;
+	_battery_status[instance].state_of_health = msg.state_of_health;
+	_battery_status[instance].max_error = msg.max_error;
+	_battery_status[instance].over_discharge_count = msg.over_discharge_count;
+	_battery_status[instance].connected = true;
+	_battery_status[instance].cell_count = msg.cell_count;
+	_battery_status[instance].source = battery_status_s::SOURCE_EXTERNAL;
+	_battery_status[instance].id = msg.getSrcNodeID().get();
+	_battery_status[instance].is_powering_off = msg.is_powering_off;
+
+	const float remaining_ah = msg.remaining_capacity / 1000.f;
+	const float current_a = math::isZero(msg.average_current) ? msg.current : msg.average_current;
+	_battery_status[instance].time_remaining_s = math::isZero(current_a) ? NAN : (remaining_ah / -current_a * 3600.f);
+
+	for (uint8_t i = 0; i < _battery_status[instance].cell_count; i++) {
+		_battery_status[instance].voltage_cell_v[i] = msg.voltage_cell[i];
+	}
+
+	determineWarning(_battery_status[instance].remaining);
+	_battery_status[instance].warning = _warning;
+
+	uint16_t faults = 0;
+
+	if (msg.status_flags & cuav::equipment::power::CBAT::STATUS_FLAG_OVERLOAD) {
+		faults |= (1 << battery_status_s::FAULT_OVER_CURRENT);
+	}
+
+	if (msg.status_flags & cuav::equipment::power::CBAT::STATUS_FLAG_BAD_BATTERY) {
+		faults |= (1 << battery_status_s::FAULT_HARDWARE_FAILURE);
+	}
+
+	if (msg.status_flags & cuav::equipment::power::CBAT::STATUS_FLAG_TEMP_HOT) {
+		faults |= (1 << battery_status_s::FAULT_OVER_TEMPERATURE);
+	}
+
+	if (msg.status_flags & cuav::equipment::power::CBAT::STATUS_FLAG_TEMP_COLD) {
+		faults |= (1 << battery_status_s::FAULT_UNDER_TEMPERATURE);
+	}
+
+	_battery_status[instance].faults = faults;
+
+	publish(msg.getSrcNodeID().get(), &_battery_status[instance]);
+
+	_battery_info[instance].timestamp = _battery_status[instance].timestamp;
+	_battery_info[instance].id = _battery_status[instance].id;
+	snprintf(_battery_info[instance].serial_number, sizeof(_battery_info[instance].serial_number), "%" PRIu16,
+		 msg.serial_number);
+	_battery_info_pub[instance].publish(_battery_info[instance]);
+}
+
 void
 UavcanBatteryBridge::sumDischarged(hrt_abstime timestamp, float current_a)
 {

src/drivers/uavcan/sensors/battery.hpp

@@ -42,6 +42,7 @@
 #include <uORB/topics/battery_status.h>
 #include <uavcan/equipment/power/BatteryInfo.hpp>
 #include <ardupilot/equipment/power/BatteryInfoAux.hpp>
+#include <cuav/equipment/power/CBAT.hpp>
 #include <battery/battery.h>
 #include <drivers/drv_hrt.h>
 #include <px4_platform_common/module_params.h>
@@ -66,10 +67,12 @@ class UavcanBatteryBridge : public UavcanSensorBridgeBase, public ModuleParams
 		Raw, // data from BatteryInfo message only
 		RawAux, // data combination from BatteryInfo and BatteryInfoAux messages
 		Filter, // filter data from BatteryInfo message with Battery library
+		CBAT, // CBAT messages
 	};
 
 	void battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::power::BatteryInfo> &msg);
 	void battery_aux_sub_cb(const uavcan::ReceivedDataStructure<ardupilot::equipment::power::BatteryInfoAux> &msg);
+	void cbat_sub_cb(const uavcan::ReceivedDataStructure<cuav::equipment::power::CBAT> &msg);
 	void sumDischarged(hrt_abstime timestamp, float current_a);
 	void determineWarning(float remaining);
 	void filterData(const uavcan::ReceivedDataStructure<uavcan::equipment::power::BatteryInfo> &msg, uint8_t instance);
@@ -83,8 +86,14 @@ class UavcanBatteryBridge : public UavcanSensorBridgeBase, public ModuleParams
 		(const uavcan::ReceivedDataStructure<ardupilot::equipment::power::BatteryInfoAux> &) >
 		BatteryInfoAuxCbBinder;
 
+	typedef uavcan::MethodBinder < UavcanBatteryBridge *,
+		void (UavcanBatteryBridge::*)
+		(const uavcan::ReceivedDataStructure<cuav::equipment::power::CBAT> &) >
+		CBATCbBinder;
+
 	uavcan::Subscriber<uavcan::equipment::power::BatteryInfo, BatteryInfoCbBinder> _sub_battery;
 	uavcan::Subscriber<ardupilot::equipment::power::BatteryInfoAux, BatteryInfoAuxCbBinder> _sub_battery_aux;
+	uavcan::Subscriber<cuav::equipment::power::CBAT, CBATCbBinder> _sub_cbat;
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::BAT_LOW_THR>) _param_bat_low_thr,

src/drivers/uavcan/uavcan_params.c

@@ -288,10 +288,11 @@ PARAM_DEFINE_INT32(UAVCAN_SUB_BARO, 0);
  * Enable UAVCAN battery subscription.
  *  uavcan::equipment::power::BatteryInfo
  *  ardupilot::equipment::power::BatteryInfoAux
+ *  cuav::equipment::power::CBAT
  *
  *  0 - Disable
  *  1 - Use raw data. Recommended for Smart battery
- *  2 - Filter the data with internal battery library
+ *  2 - Filter the data with internal battery library (unsupported with CBAT)
  *
  * @min 0
  * @max 2