Update battery status enums; update UAVCAN_SUB_BAT parameter

boards/bitcraze/crazyflie/syslink/syslink_main.h

@@ -139,7 +139,7 @@ class Syslink
 
 	// nrf chip schedules battery updates with SYSLINK_SEND_PERIOD_MS
 	static constexpr uint32_t SYSLINK_BATTERY_STATUS_INTERVAL_US = 10_ms;
-	Battery _battery{1, nullptr, SYSLINK_BATTERY_STATUS_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE};
+	Battery _battery{1, nullptr, SYSLINK_BATTERY_STATUS_INTERVAL_US, battery_status_s::SOURCE_POWER_MODULE};
 
 	int32_t _rssi;
 	battery_state _bstate;

msg/versioned/BatteryStatus.msg

@@ -56,3 +56,29 @@ float32 volt_based_soc_estimate			# [0, 1] Normalized Volt-based state of charge
 float32 voltage_prediction			# [V] Predicted voltage
 float32 prediction_error                        # [V] Prediction error
 float32 estimation_covariance_norm		# Norm of the covariance matrix
+
+
+
+# to be removed
+uint8 cell_count				# Number of cells, 0 if unknown
+
+uint8 source					# Battery source
+uint8 POWER_MODULE = 0				# Source is a power module
+uint8 EXTERNAL = 1				# Source is external
+uint8 ESCS = 2					# Source is an ESC
+
+uint16 capacity					# Actual capacity of the battery
+uint16 cycle_count				# Number of discharge cycles the battery has experienced
+uint16 average_time_to_empty			# Predicted remaining battery capacity based on the average rate of discharge (min)
+char[32] serial_number				# Serial number of the battery pack
+uint16 manufacture_date				# Manufacture date, part of serial number of the battery pack. Formatted as: Day + Month×32 + (Year–1980)×512
+uint16 state_of_health				# State of health. FullChargeCapacity/DesignCapacity, 0-100%
+uint16 max_error				# Max error, expected margin of error in % in the state-of-charge calculation with a range of 1 to 100%
+
+float32 design_capacity_wh			# The original battery capacity
+float32 full_charge_capacity_wh     		# The compensated battery capacity
+float32 remaining_capacity_wh       		# The compensated battery capacity remaining
+uint16 over_discharge_count         		# Number of battery overdischarge
+float32 nominal_voltage             		# Nominal voltage of the battery pack
+
+float32 internal_resistance_estimate            # Internal resistance per cell estimate (Ohm)

platforms/common/work_queue/wqueue_test/wqueue_start_posix.cpp

@@ -40,8 +40,8 @@
 #include "wqueue_test.h"
 
 #include <px4_platform_common/app.h>
-#include <px4_platform_common/log.h>
-#include <px4_platform_common/tasks.h>
+#include <px4_platform_common/
+#include <px4_platform_common/tasks.h>log.h>
 #include <stdio.h>
 #include <string.h>
 #include <sched.h>

src/drivers/batt_smbus/batt_smbus.cpp

@@ -175,16 +175,16 @@ void BATT_SMBUS::RunImpl()
 			new_report.warning = battery_status_s::BATTERY_WARNING_CRITICAL;
 
 		} else if (new_report.remaining > _low_thr) {
-			new_report.warning = battery_status_s::BATTERY_WARNING_NONE;
+			new_report.warning = battery_status_s::WARNING_NONE;
 
 		} else if (new_report.remaining > _crit_thr) {
-			new_report.warning = battery_status_s::BATTERY_WARNING_LOW;
+			new_report.warning = battery_status_s::WARNING_LOW;
 
 		} else if (new_report.remaining > _emergency_thr) {
-			new_report.warning = battery_status_s::BATTERY_WARNING_CRITICAL;
+			new_report.warning = battery_status_s::WARNING_CRITICAL;
 
 		} else {
-			new_report.warning = battery_status_s::BATTERY_WARNING_EMERGENCY;
+			new_report.warning = battery_status_s::WARNING_EMERGENCY;
 		}
 
 		new_report.interface_error = perf_event_count(_interface->_interface_errors);

src/drivers/hygrometer/sht3x/sht3x.h

@@ -78,7 +78,7 @@ const char *sht_state_names[] = {"General error", "Readout error", "Initializati
 
 
 struct sht_info {
-	uint32_t serial_number;
+	char[32] serial_number;
 };
 
 

src/drivers/power_monitor/ina226/ina226.cpp

@@ -49,7 +49,7 @@ INA226::INA226(const I2CSPIDriverConfig &config, int battery_index) :
 	_comms_errors(perf_alloc(PC_COUNT, "ina226_com_err")),
 	_collection_errors(perf_alloc(PC_COUNT, "ina226_collection_err")),
 	_measure_errors(perf_alloc(PC_COUNT, "ina226_measurement_err")),
-	_battery(battery_index, this, INA226_SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE)
+	_battery(battery_index, this, INA226_SAMPLE_INTERVAL_US, battery_status_s::POWER_MODULE)
 {
 	float fvalue = MAX_CURRENT;
 	_max_current = fvalue;

src/drivers/power_monitor/ina228/ina228.cpp

@@ -49,7 +49,7 @@ INA228::INA228(const I2CSPIDriverConfig &config, int battery_index) :
 	_comms_errors(perf_alloc(PC_COUNT, "ina228_com_err")),
 	_collection_errors(perf_alloc(PC_COUNT, "ina228_collection_err")),
 	_measure_errors(perf_alloc(PC_COUNT, "ina228_measurement_err")),
-	_battery(battery_index, this, INA228_SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE)
+	_battery(battery_index, this, INA228_SAMPLE_INTERVAL_US, battery_status_s::POWER_MODULE)
 {
 	float fvalue = MAX_CURRENT;
 	_max_current = fvalue;

src/drivers/power_monitor/ina238/ina238.cpp

@@ -45,7 +45,7 @@ INA238::INA238(const I2CSPIDriverConfig &config, int battery_index) :
 	_sample_perf(perf_alloc(PC_ELAPSED, "ina238_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "ina238_com_err")),
 	_collection_errors(perf_alloc(PC_COUNT, "ina238_collection_err")),
-	_battery(battery_index, this, INA238_SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE)
+	_battery(battery_index, this, INA238_SAMPLE_INTERVAL_US, battery_status_s::POWER_MODULE)
 {
 	float fvalue = DEFAULT_MAX_CURRENT;
 	_max_current = fvalue;

src/drivers/smart_battery/batmon/batmon.cpp

@@ -202,16 +202,16 @@ void Batmon::RunImpl()
 		//	new_report.warning = battery_status_s::BATTERY_WARNING_CRITICAL;
 
 		if (new_report.remaining > _low_thr) {
-			new_report.warning = battery_status_s::BATTERY_WARNING_NONE;
+			new_report.warning = battery_status_s::WARNING_NONE;
 
 		} else if (new_report.remaining > _crit_thr) {
-			new_report.warning = battery_status_s::BATTERY_WARNING_LOW;
+			new_report.warning = battery_status_s::WARNING_LOW;
 
 		} else if (new_report.remaining > _emergency_thr) {
-			new_report.warning = battery_status_s::BATTERY_WARNING_CRITICAL;
+			new_report.warning = battery_status_s::WARNING_CRITICAL;
 
 		} else {
-			new_report.warning = battery_status_s::BATTERY_WARNING_EMERGENCY;
+			new_report.warning = battery_status_s::WARNING_EMERGENCY;
 		}
 
 		new_report.interface_error = perf_event_count(_interface->_interface_errors);

src/drivers/uavcan/sensors/battery.cpp

@@ -45,7 +45,7 @@ UavcanBatteryBridge::UavcanBatteryBridge(uavcan::INode &node) :
 	_sub_battery(node),
 	_sub_battery_aux(node),
 	_sub_cbat(node),
-	_warning(battery_status_s::BATTERY_WARNING_NONE),
+	_warning(battery_status_s::WARNING_NONE),
 	_last_timestamp(0)
 {
 }
@@ -135,7 +135,7 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::
 	// _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].serial_number = msg.model_instance_id;
 	_battery_status[instance].id = msg.getSrcNodeID().get();
 
 	if (_batt_update_mod[instance] == BatteryDataType::Raw) {
@@ -203,7 +203,15 @@ UavcanBatteryBridge::battery_aux_sub_cb(const uavcan::ReceivedDataStructure<ardu
 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");
+	}
+
 	uint8_t instance = 0;
 
 	for (instance = 0; instance < battery_status_s::MAX_INSTANCES; instance++) {
@@ -217,56 +225,16 @@ UavcanBatteryBridge::cbat_sub_cb(const uavcan::ReceivedDataStructure<cuav::equip
 	}
 
 	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) {
+	if (_batt_update_mod[instance] == BatteryDataType::RawAuxCBAT) {
+		snprintf(_battery_status[instance].serial_number, 32, "%hu", msg.serial_number);
 		publish(msg.getSrcNodeID().get(), &_battery_status[instance]);
 	}
-	*/
 }
 
 void
@@ -295,14 +263,14 @@ void
 UavcanBatteryBridge::determineWarning(float remaining)
 {
 	// propagate warning state only if the state is higher, otherwise remain in current warning state
-	if (remaining < _param_bat_emergen_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_EMERGENCY)) {
-		_warning = battery_status_s::BATTERY_WARNING_EMERGENCY;
+	if (remaining < _param_bat_emergen_thr.get() || (_warning == battery_status_s::WARNING_EMERGENCY)) {
+		_warning = battery_status_s::WARNING_EMERGENCY;
 
-	} else if (remaining < _param_bat_crit_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) {
-		_warning = battery_status_s::BATTERY_WARNING_CRITICAL;
+	} else if (remaining < _param_bat_crit_thr.get() || (_warning == battery_status_s::WARNING_CRITICAL)) {
+		_warning = battery_status_s::WARNING_CRITICAL;
 
-	} else if (remaining < _param_bat_low_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_LOW)) {
-		_warning = battery_status_s::BATTERY_WARNING_LOW;
+	} else if (remaining < _param_bat_low_thr.get() || (_warning == battery_status_s::WARNING_LOW)) {
+		_warning = battery_status_s::WARNING_LOW;
 	}
 }
 
@@ -318,7 +286,7 @@ UavcanBatteryBridge::filterData(const uavcan::ReceivedDataStructure<uavcan::equi
 	/* Override data that is expected to arrive from UAVCAN msg*/
 	_battery_status[instance] = _battery[instance]->getBatteryStatus();
 	_battery_status[instance].temperature = msg.temperature + atmosphere::kAbsoluteNullCelsius; // Kelvin to Celsius
-	_battery_status[instance].serial_number = msg.model_instance_id;
+	//_battery_status[instance].serial_number = msg.model_instance_id;
 	_battery_status[instance].id = msg.getSrcNodeID().get(); // overwrite zeroed index from _battery
 
 	publish(msg.getSrcNodeID().get(), &_battery_status[instance]);

src/drivers/uavcan/sensors/battery.hpp

@@ -65,6 +65,7 @@ class UavcanBatteryBridge : public UavcanSensorBridgeBase, public ModuleParams
 	enum class BatteryDataType {
 		Raw, // data from BatteryInfo message only
 		RawAux, // data combination from BatteryInfo and BatteryInfoAux messages
+		RawAuxCBAT, // data combination from BatteryInfo, BatteryInfoAux, and CBAT messages
 		Filter, // filter data from BatteryInfo message with Battery library
 	};
 
@@ -114,10 +115,10 @@ class UavcanBatteryBridge : public UavcanSensorBridgeBase, public ModuleParams
 
 	static_assert(battery_status_s::MAX_INSTANCES <= BATTERY_INDEX_4, "Battery array too big");
 
-	Battery battery1 = {BATTERY_INDEX_1, this, SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_EXTERNAL};
-	Battery battery2 = {BATTERY_INDEX_2, this, SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_EXTERNAL};
-	Battery battery3 = {BATTERY_INDEX_3, this, SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_EXTERNAL};
-	Battery battery4 = {BATTERY_INDEX_4, this, SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_EXTERNAL};
+	Battery battery1 = {BATTERY_INDEX_1, this, SAMPLE_INTERVAL_US, battery_status_s::EXTERNAL};
+	Battery battery2 = {BATTERY_INDEX_2, this, SAMPLE_INTERVAL_US, battery_status_s::EXTERNAL};
+	Battery battery3 = {BATTERY_INDEX_3, this, SAMPLE_INTERVAL_US, battery_status_s::EXTERNAL};
+	Battery battery4 = {BATTERY_INDEX_4, this, SAMPLE_INTERVAL_US, battery_status_s::EXTERNAL};
 
 	Battery *_battery[battery_status_s::MAX_INSTANCES] = { &battery1, &battery2, &battery3, &battery4 };
 };

src/drivers/uavcan/uavcan_params.c

@@ -285,19 +285,24 @@ PARAM_DEFINE_INT32(UAVCAN_SUB_BARO, 0);
 /**
  * subscription battery
  *
- * Enable UAVCAN battery subscription.
+ * Enable UAVCAN battery subscription. Messages considered:
  *  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
+ *  1 - Use raw data. Recommended for smart battery
+ *  2 - Use raw and auxiliary data
+ *  3 - Use raw, auxiliary, and CBAT-specific data
+ *  4 - Filter the data with internal battery library
  *
  * @min 0
  * @max 2
  * @value 0 Disable
  * @value 1 Raw data
- * @value 2 Filter data
+ * @value 2 Raw and auxiliary data
+ * @value 3 Raw, auxiliary, and CBAT-specific data
+ * @value 4 Filter data
  * @reboot_required true
  * @group UAVCAN
  */

src/lib/battery/battery.cpp

@@ -307,16 +307,16 @@ void Battery::estimateStateOfCharge()
 uint8_t Battery::determineWarning(float state_of_charge)
 {
 	if (state_of_charge < _params.emergen_thr) {
-		return battery_status_s::BATTERY_WARNING_EMERGENCY;
+		return battery_status_s::WARNING_EMERGENCY;
 
 	} else if (state_of_charge < _params.crit_thr) {
-		return battery_status_s::BATTERY_WARNING_CRITICAL;
+		return battery_status_s::WARNING_CRITICAL;
 
 	} else if (state_of_charge < _params.low_thr) {
-		return battery_status_s::BATTERY_WARNING_LOW;
+		return battery_status_s::WARNING_LOW;
 
 	} else {
-		return battery_status_s::BATTERY_WARNING_NONE;
+		return battery_status_s::WARNING_NONE;
 	}
 }
 
@@ -327,7 +327,7 @@ uint16_t Battery::determineFaults()
 	if ((_params.n_cells > 0)
 	    && (_voltage_v > (_params.n_cells * _params.v_charged * 1.05f))) {
 		// Reported as a "spike" since "over-voltage" does not exist in MAV_BATTERY_FAULT
-		faults |= (1 << battery_status_s::BATTERY_FAULT_SPIKES);
+		faults |= (1 << battery_status_s::SPIKES);
 	}
 
 	return faults;

src/lib/battery/battery.h

@@ -175,7 +175,7 @@ class Battery : public ModuleParams
 	float _state_of_charge_volt_based{-1.f}; // [0,1]
 	float _state_of_charge{-1.f}; // [0,1]
 	float _scale{1.f};
-	uint8_t _warning{battery_status_s::BATTERY_WARNING_NONE};
+	uint8_t _warning{battery_status_s::WARNING_NONE};
 	hrt_abstime _last_timestamp{0};
 	bool _armed{false};
 	bool _vehicle_status_is_fw{false};

src/modules/battery_status/battery_status.cpp

@@ -137,9 +137,9 @@ class BatteryStatus : public ModuleBase<BatteryStatus>, public ModuleParams, pub
 BatteryStatus::BatteryStatus() :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
-	_battery1(1, this, SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0),
+	_battery1(1, this, SAMPLE_INTERVAL_US, battery_status_s::POWER_MODULE, 0),
 #if BOARD_NUMBER_BRICKS > 1
-	_battery2(2, this, SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE, 1),
+	_battery2(2, this, SAMPLE_INTERVAL_US, battery_status_s::POWER_MODULE, 1),
 #endif
 	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME))
 {

src/modules/commander/Commander.cpp

@@ -2198,13 +2198,13 @@ void Commander::updateTunes()
 
 	} else if (!_vehicle_status.usb_connected &&
 		   (_vehicle_status.hil_state != vehicle_status_s::HIL_STATE_ON) &&
-		   (_battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) {
+		   (_battery_warning == battery_status_s::WARNING_CRITICAL)) {
 
 		/* play tune on battery critical */
 		set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_FAST);
 
 	} else if ((_vehicle_status.hil_state != vehicle_status_s::HIL_STATE_ON) &&
-		   (_battery_warning == battery_status_s::BATTERY_WARNING_LOW)) {
+		   (_battery_warning == battery_status_s::WARNING_LOW)) {
 		/* play tune on battery warning */
 		set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_SLOW);
 
@@ -2498,10 +2498,10 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
 			if (_vehicle_status.failsafe) {
 				led_color = led_control_s::COLOR_PURPLE;
 
-			} else if (battery_warning == battery_status_s::BATTERY_WARNING_LOW) {
+			} else if (battery_warning == battery_status_s::WARNING_LOW) {
 				led_color = led_control_s::COLOR_AMBER;
 
-			} else if (battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL) {
+			} else if (battery_warning == battery_status_s::WARNING_CRITICAL) {
 				led_color = led_control_s::COLOR_RED;
 
 			} else {
@@ -2893,7 +2893,7 @@ void Commander::battery_status_check()
 	// Handle shutdown request from emergency battery action
 	if (_battery_warning != _failsafe_flags.battery_warning) {
 
-		if (_failsafe_flags.battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) {
+		if (_failsafe_flags.battery_warning == battery_status_s::WARNING_EMERGENCY) {
 #if defined(BOARD_HAS_POWER_CONTROL)
 
 			if (!isArmed() && (px4_shutdown_request(60_s) == 0)) {