Update UAVCAN_SUB_BAT parameter

PX4 · Jan 29, 2025 · 387f241 · 387f241
1 parent 4989992
commit 387f241
Show file tree

Hide file tree

Showing 3 changed files with 41 additions and 12 deletions.
diff --git a/src/drivers/uavcan/Kconfig b/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

diff --git a/src/drivers/uavcan/sensors/battery.cpp b/src/drivers/uavcan/sensors/battery.cpp
@@ -57,11 +57,17 @@ int UavcanBatteryBridge::init()
 
 	for (uint8_t instance = 0; instance < battery_status_s::MAX_INSTANCES; instance++) {
 
-		if (uavcan_sub_bat == FILTER_DATA) {
-			_batt_update_mod[instance] = BatteryDataType::Filter;
-
-		} else {
+		if (uavcan_sub_bat == RAW_DATA) {
 			_batt_update_mod[instance] = BatteryDataType::Raw;
+
+		} else if (uavcan_sub_bat == RAW_AUX_DATA) {
+			_batt_update_mod[instance] = BatteryDataType::RawAux;
+
+		} else if (uavcan_sub_bat == RAW_AUX_CBAT_DATA) {
+			_batt_update_mod[instance] = BatteryDataType::RawAuxCBAT;
+
+		} else if (uavcan_sub_bat == FILTER_DATA) {
+			_batt_update_mod[instance] = BatteryDataType::Filter;
 		}
 	}
 
@@ -94,6 +100,8 @@ int UavcanBatteryBridge::init()
 void
 UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::power::BatteryInfo> &msg)
 {
+	PX4_INFO("Battery Sub Cb called");
+
 	uint8_t instance = 0;
 
 	for (instance = 0; instance < battery_status_s::MAX_INSTANCES; instance++) {
@@ -155,13 +163,17 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::
 
 	if (_batt_update_mod[instance] == BatteryDataType::Raw) {
 		publish(msg.getSrcNodeID().get(), &_battery_status[instance]);
+		PX4_INFO("Battery Sub Cb published");
+
 	}
 }
 
 void
 UavcanBatteryBridge::battery_aux_sub_cb(const uavcan::ReceivedDataStructure<ardupilot::equipment::power::BatteryInfoAux>
 					&msg)
 {
+	PX4_INFO("Battery Aux Sub Cb called");
+
 	uint8_t instance = 0;
 
 	for (instance = 0; instance < battery_status_s::MAX_INSTANCES; instance++) {
@@ -178,8 +190,6 @@ UavcanBatteryBridge::battery_aux_sub_cb(const uavcan::ReceivedDataStructure<ardu
 		return;
 	}
 
-	_batt_update_mod[instance] = BatteryDataType::RawAux;
-
 	_battery_status[instance].discharged_mah = (_battery_status[instance].full_charge_capacity_wh -
 			_battery_status[instance].remaining_capacity_wh) / msg.nominal_voltage *
 			1000;
@@ -196,30 +206,38 @@ UavcanBatteryBridge::battery_aux_sub_cb(const uavcan::ReceivedDataStructure<ardu
 		_battery_status[instance].voltage_cell_v[i] = msg.voltage_cell[i];
 	}
 
-	publish(msg.getSrcNodeID().get(), &_battery_status[instance]);
+	if (_batt_update_mod[instance] == BatteryDataType::RawAux) {
+		publish(msg.getSrcNodeID().get(), &_battery_status[instance]);
+		PX4_INFO("Battery Aux Sub Cb published");
+	}
+
 }
 
 
 void
 UavcanBatteryBridge::cbat_sub_cb(const uavcan::ReceivedDataStructure<cuav::equipment::power::CBAT> &msg)
 {
-
+	/*
 	if (msg.serial_number != 0){
 		PX4_ERR("CBAT message received");
 	}
 
 	else{
 		PX4_ERR("No CBAT message received");
 	}
+	*/
+
+	PX4_INFO("Battery CBAT Sub Cb called");
 
 	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) {
+		if (_battery_status[instance].id == msg.getSrcNodeID().get()) {
 			break;
 		}
 	}
 
+
 	if (instance >= battery_status_s::MAX_INSTANCES) {
 		return;
 	}
@@ -232,8 +250,16 @@ UavcanBatteryBridge::cbat_sub_cb(const uavcan::ReceivedDataStructure<cuav::equip
 	_battery_status[instance].warning = _warning;
 
 	if (_batt_update_mod[instance] == BatteryDataType::RawAuxCBAT) {
-		snprintf(_battery_status[instance].serial_number, 32, "%hu", msg.serial_number);
+		// char ser_num[32] = {0};
+		// snprintf(ser_num, 32, "%hu", msg.serial_number);
+		// strncpy(_battery_status[instance].serial_number, ser_num, sizeof(_battery_status[instance].serial_number) - 1);
+		strncpy(_battery_status[instance].serial_number, "123456", sizeof(_battery_status[instance].serial_number));
+
+		_battery_status[instance].serial_number[sizeof(_battery_status[instance].serial_number) - 1] = '\0';
+
 		publish(msg.getSrcNodeID().get(), &_battery_status[instance]);
+		PX4_INFO("CBAT Sub Cb published");
+
 	}
 }
 

diff --git a/src/drivers/uavcan/sensors/battery.hpp b/src/drivers/uavcan/sensors/battery.hpp
@@ -106,7 +106,10 @@ class UavcanBatteryBridge : public UavcanSensorBridgeBase, public ModuleParams
 	battery_status_s _battery_status[battery_status_s::MAX_INSTANCES] {};
 	BatteryDataType _batt_update_mod[battery_status_s::MAX_INSTANCES] {};
 
-	static constexpr int FILTER_DATA = 2;
+	static constexpr int RAW_DATA = 1;
+	static constexpr int RAW_AUX_DATA = 2;
+	static constexpr int RAW_AUX_CBAT_DATA = 3;
+	static constexpr int FILTER_DATA = 4;
 	static constexpr int BATTERY_INDEX_1 = 1;
 	static constexpr int BATTERY_INDEX_2 = 2;
 	static constexpr int BATTERY_INDEX_3 = 3;