From 15f826d5d93a5507f31cba66ef4d982ff0e615de Mon Sep 17 00:00:00 2001
From: Sebastian Domoszlai <sebastian.domoszlai@protonmail.com>
Date: Tue, 10 Dec 2024 13:51:25 +0100
Subject: [PATCH] Copy callback function structure for parsing CBAT message

---
 src/drivers/uavcan/sensors/battery.cpp | 80 ++++++++++++++++++++++++++
 src/drivers/uavcan/sensors/battery.hpp |  7 +++
 2 files changed, 87 insertions(+)

diff --git a/src/drivers/uavcan/sensors/battery.cpp b/src/drivers/uavcan/sensors/battery.cpp
index 77698cff16d1..1235c1f7f7b2 100644
--- a/src/drivers/uavcan/sensors/battery.cpp
+++ b/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::BATTERY_WARNING_NONE),
 	_last_timestamp(0)
 {
@@ -79,6 +80,15 @@ int UavcanBatteryBridge::init()
 	}
 
 	return 0;
+
+	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;
 }
 
 void
@@ -189,6 +199,76 @@ UavcanBatteryBridge::battery_aux_sub_cb(const uavcan::ReceivedDataStructure<ardu
 	publish(msg.getSrcNodeID().get(), &_battery_status[instance]);
 }
 
+
+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() || _battery_status[instance].id == 0) {
+			break;
+		}
+	}
+
+	if (instance >= battery_status_s::MAX_INSTANCES) {
+		return;
+	}
+
+	if (_batt_update_mod[instance] == BatteryDataType::Filter) {
+
+		filterData(msg, instance);
+		return;
+	}
+
+	_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.current;
+
+	if (_batt_update_mod[instance] == BatteryDataType::Raw) {
+		sumDischarged(_battery_status[instance].timestamp, _battery_status[instance].current_a);
+		_battery_status[instance].discharged_mah = _discharged_mah;
+	}
+
+	_battery_status[instance].remaining = msg.state_of_charge_pct / 100.0f; // between 0 and 1
+	// _battery_status[instance].scale = msg.; // Power scaling factor, >= 1, or -1 if unknown
+	_battery_status[instance].temperature = msg.temperature + atmosphere::kAbsoluteNullCelsius; // Kelvin to Celsius
+	// _battery_status[instance].cell_count = msg.;
+	_battery_status[instance].connected = true;
+	_battery_status[instance].source = msg.status_flags & uavcan::equipment::power::BatteryInfo::STATUS_FLAG_IN_USE;
+	// _battery_status[instance].priority = msg.;
+	_battery_status[instance].capacity = msg.full_charge_capacity_wh;
+	_battery_status[instance].full_charge_capacity_wh = msg.full_charge_capacity_wh;
+	_battery_status[instance].remaining_capacity_wh = msg.remaining_capacity_wh;
+	// _battery_status[instance].cycle_count = msg.;
+	_battery_status[instance].time_remaining_s = NAN;
+	// _battery_status[instance].average_time_to_empty = msg.;
+	_battery_status[instance].serial_number = msg.model_instance_id;
+	_battery_status[instance].id = msg.getSrcNodeID().get();
+
+	if (_batt_update_mod[instance] == BatteryDataType::Raw) {
+		// Mavlink 2 needs individual cell voltages or cell[0] if cell voltages are not available.
+		_battery_status[instance].voltage_cell_v[0] = msg.voltage;
+
+		// Set cell count to 1 so the the battery code in mavlink_messages.cpp copies the values correctly (hack?)
+		_battery_status[instance].cell_count = 1;
+	}
+
+	// _battery_status[instance].max_cell_voltage_delta = msg.;
+
+	// _battery_status[instance].is_powering_off = msg.;
+
+	determineWarning(_battery_status[instance].remaining);
+	_battery_status[instance].warning = _warning;
+
+	if (_batt_update_mod[instance] == BatteryDataType::Raw) {
+		publish(msg.getSrcNodeID().get(), &_battery_status[instance]);
+	}
+	*/
+}
+
 void
 UavcanBatteryBridge::sumDischarged(hrt_abstime timestamp, float current_a)
 {
diff --git a/src/drivers/uavcan/sensors/battery.hpp b/src/drivers/uavcan/sensors/battery.hpp
index ceb6222da084..b8506d73f6d0 100644
--- a/src/drivers/uavcan/sensors/battery.hpp
+++ b/src/drivers/uavcan/sensors/battery.hpp
@@ -41,6 +41,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>
@@ -69,6 +70,7 @@ class UavcanBatteryBridge : public UavcanSensorBridgeBase, public ModuleParams
 
 	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);
@@ -81,9 +83,14 @@ class UavcanBatteryBridge : public UavcanSensorBridgeBase, public ModuleParams
 		void (UavcanBatteryBridge::*)
 		(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,