UAVCANv0: add CUAV smart battery support

msg/battery_status.msg

@@ -25,9 +25,9 @@ uint16 manufacture_date		# manufacture date, part of serial number of the batter
 uint16 state_of_health		# state of health. FullChargeCapacity/DesignCapacity.
 uint16 max_error			# max error, expected margin of error in % in the state-of-charge calculation with a range of 1 to 100%
 uint8 id					# ID number of a battery. Should be unique and consistent for the lifetime of a vehicle. 1-indexed.
-uint16 interface_error		# SMBUS interface error counter
+uint16 interface_error		# interface error counter
 
-float32[10] voltage_cell_v		# Battery individual cell voltages
+float32[14] voltage_cell_v		# Battery individual cell voltages
 float32 max_cell_voltage_delta	# Max difference between individual cell voltages
 
 bool is_powering_off			# Power off event imminent indication, false if unknown
@@ -43,3 +43,11 @@ uint8 warning						# current battery warning
 
 
 uint8 MAX_INSTANCES = 4
+
+float32 average_power               # The average power of the current discharge
+float32 available_energy            # The predicted charge or energy remaining in the battery
+float32 remaining_capacity          # The compensated battery capacity remaining
+float32 design_capacity             # The design capacity of the battery
+uint16 average_time_to_full         # The predicted remaining time until the battery reaches full charge, in minutes
+uint16 over_discharge_count         # Number of battery overdischarge
+float32 nominal_voltage             # Nominal voltage of the battery pack

src/drivers/uavcan/CMakeLists.txt

@@ -99,6 +99,7 @@ set(DSDLC_INPUTS
 	"${DSDLC_DIR}/com"
 	"${DSDLC_DIR}/ardupilot"
 	"${LIBUAVCAN_DIR}/dsdl/uavcan"
+	"${DSDLC_DIR}/cuav"
 )
 set(DSDLC_OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/include/dsdlc_generated")
 
@@ -153,6 +154,7 @@ px4_add_module(
 		sensors/rangefinder.cpp
 		sensors/accel.cpp
 		sensors/gyro.cpp
+		sensors/cbat.cpp
 
 	DEPENDS
 		px4_uavcan_dsdlc

src/drivers/uavcan/dsdl/cuav/equipment/power/20300.CBAT.uavcan

@@ -0,0 +1,41 @@
+#
+# support for CUAV Smart Battery on UAVCAN
+#
+
+float32 temperature                 # The surface temperature of the battery
+float32 voltage                     # The total voltage of the battery
+float32[<=15] voltage_cell          # Battery individual cell voltages
+uint8 cell_count                    # Number of cells
+float32 current                     # The current flowing through the sense resistor
+float32 average_current             # The average current flowing through the sense resistor
+float32 average_power               # The average power of the current discharge
+float32 available_energy            # The predicted charge or energy remaining in the battery
+float32 remaining_capacity          # The compensated battery capacity remaining
+float32 full_charge_capacity        # The compensated capacity of the battery when fully charged
+float32 design_capacity             # The design capacity of the battery
+uint16 average_time_to_empty        # The predicted remaining battery life at the present rate of discharge, in minutes
+uint16 average_time_to_full         # The predicted remaining time until the battery reaches full charge, in minutes
+uint7 state_of_health               # Health of the battery
+uint7 state_of_charge               # Percent of the full charge [0, 100]
+uint7 max_error                     # max error, expected margin of error in % in the state-of-charge calculation with a range of 1 to 100%
+uint16 serial_number                # serial number of the battery pack
+uint16 manufacture_date             # manufacture date, part of serial number of the battery pack
+uint16 cycle_count                  # number of discharge cycles the battery has experienced
+uint16 over_discharge_count         # Number of battery overdischarge
+float32 passed_charge               # The amount of charge passing through the sense resistor
+float32 nominal_voltage             # Nominal voltage of the battery pack
+bool is_powering_off                # Power off event imminent indication, false if unknown
+uint16 interface_error              # interface error counter
+
+uint11 STATUS_FLAG_IN_USE       = 1     # The battery is currently used as a power supply
+uint11 STATUS_FLAG_CHARGING     = 2     # Charger is active
+uint11 STATUS_FLAG_CHARGED      = 4     # Charging complete, but the charger is still active
+uint11 STATUS_FLAG_TEMP_HOT     = 8     # Battery temperature is above normal
+uint11 STATUS_FLAG_TEMP_COLD    = 16    # Battery temperature is below normal
+uint11 STATUS_FLAG_OVERLOAD     = 32    # Safe operating area violation
+uint11 STATUS_FLAG_BAD_BATTERY  = 64    # This battery should not be used anymore (e.g. low SOH)
+uint11 STATUS_FLAG_NEED_SERVICE = 128   # This battery requires maintenance (e.g. balancing, full recharge)
+uint11 STATUS_FLAG_BMS_ERROR    = 256   # Battery management system/controller error, smart battery interface error
+uint11 STATUS_FLAG_RESERVED_A   = 512   # Keep zero
+uint11 STATUS_FLAG_RESERVED_B   = 1024  # Keep zero
+uint11 status_flags

src/drivers/uavcan/sensors/cbat.cpp

@@ -0,0 +1,139 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "cbat.hpp"
+
+#include <lib/ecl/geo/geo.h>
+#include <px4_defines.h>
+
+const char *const UavcanCBATBridge::NAME = "cbat";
+
+UavcanCBATBridge::UavcanCBATBridge(uavcan::INode &node) :
+	UavcanSensorBridgeBase("uavcan_cbat", ORB_ID(battery_status)),
+	ModuleParams(nullptr),
+	_sub_battery(node),
+	_warning(battery_status_s::BATTERY_WARNING_NONE)
+{
+}
+
+int UavcanCBATBridge::init()
+{
+	int res = _sub_battery.start(CBATCbBinder(this, &UavcanCBATBridge::battery_sub_cb));
+
+	if (res < 0) {
+		PX4_ERR("failed to start uavcan sub: %d", res);
+		return res;
+	}
+
+	return 0;
+}
+
+void
+UavcanCBATBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<cuav::equipment::power::CBAT> &msg)
+{
+	battery_status_s battery{};
+
+	battery.timestamp = hrt_absolute_time();
+	battery.voltage_v = msg.voltage;
+	battery.voltage_filtered_v = battery.voltage_v;
+	battery.current_a = msg.current;
+	battery.current_filtered_a = battery.current_a;
+	battery.average_current_a = msg.average_current;
+	battery.discharged_mah = msg.passed_charge * 1000;
+	battery.remaining = msg.state_of_charge / 100.0f;
+	// battery.scale = msg.; // Power scaling factor, >= 1, or -1 if unknown
+	battery.temperature = msg.temperature + CONSTANTS_ABSOLUTE_NULL_CELSIUS;
+	battery.cell_count = msg.cell_count;
+	battery.connected = true;
+	battery.source = msg.status_flags & cuav::equipment::power::CBAT::STATUS_FLAG_IN_USE; // BATTERY_SOURCE_EXTERNAL
+	// battery.priority = msg.;
+	battery.capacity = msg.full_charge_capacity * 1000;
+	battery.cycle_count = msg.cycle_count;
+	battery.run_time_to_empty = msg.average_time_to_empty;
+	battery.average_time_to_empty = msg.average_time_to_empty;
+	battery.serial_number = msg.serial_number;
+	battery.manufacture_date = msg.manufacture_date;
+	battery.state_of_health = msg.state_of_health;
+	battery.max_error = msg.max_error;
+	battery.id = msg.getSrcNodeID().get();
+	battery.interface_error = msg.interface_error;
+
+	for (uint8_t i = 0; i < msg.cell_count; i++) {
+		battery.voltage_cell_v[i] = msg.voltage_cell[i];
+	}
+
+	//Calculate max cell delta
+	float min_cell_voltage = msg.voltage_cell[0];
+	float max_cell_voltage = msg.voltage_cell[0];
+
+	for (uint8_t i = 1; i < msg.cell_count; i++) {
+		min_cell_voltage = math::min(min_cell_voltage, msg.voltage_cell[i]);
+		max_cell_voltage = math::max(max_cell_voltage, msg.voltage_cell[i]);
+	}
+
+	// Calculate the max difference between the min and max cells with complementary filter.
+	battery.max_cell_voltage_delta = (0.5f * (max_cell_voltage - min_cell_voltage)) +
+					 (0.5f * _max_cell_voltage_delta);
+	_max_cell_voltage_delta = battery.max_cell_voltage_delta;
+
+	battery.is_powering_off = msg.is_powering_off;
+
+	determineWarning(battery.remaining);
+	battery.warning = _warning;
+
+	// Expand the information
+	battery.average_power = msg.average_power;
+	battery.available_energy = msg.available_energy;
+	battery.remaining_capacity = msg.remaining_capacity;
+	battery.design_capacity = msg.design_capacity;
+	battery.average_time_to_full = msg.average_time_to_full;
+	battery.over_discharge_count = msg.over_discharge_count;
+	battery.nominal_voltage = msg.nominal_voltage;
+
+	publish(msg.getSrcNodeID().get(), &battery);
+}
+
+void
+UavcanCBATBridge::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;
+
+	} 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_low_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_LOW)) {
+		_warning = battery_status_s::BATTERY_WARNING_LOW;
+	}
+}

src/drivers/uavcan/sensors/cbat.hpp

@@ -0,0 +1,78 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @author Mengxiao Li <mengxiao@cuav.net>
+ */
+
+
+#pragma once
+
+#include "sensor_bridge.hpp"
+#include <uORB/topics/battery_status.h>
+#include <cuav/equipment/power/CBAT.hpp>
+#include <drivers/drv_hrt.h>
+#include <px4_platform_common/module_params.h>
+#include <mathlib/mathlib.h>
+
+class UavcanCBATBridge : public UavcanSensorBridgeBase, public ModuleParams
+{
+public:
+	static const char *const NAME;
+
+	UavcanCBATBridge(uavcan::INode &node);
+
+	const char *get_name() const override { return NAME; }
+
+	int init() override;
+
+private:
+
+	void battery_sub_cb(const uavcan::ReceivedDataStructure<cuav::equipment::power::CBAT> &msg);
+	void determineWarning(float remaining);
+
+	typedef uavcan::MethodBinder < UavcanCBATBridge *,
+		void (UavcanCBATBridge::*)
+		(const uavcan::ReceivedDataStructure<cuav::equipment::power::CBAT> &) >
+		CBATCbBinder;
+
+	uavcan::Subscriber<cuav::equipment::power::CBAT, CBATCbBinder> _sub_battery;
+
+	DEFINE_PARAMETERS(
+		(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
+	)
+
+	uint8_t _warning;
+	float _max_cell_voltage_delta = 0.f;
+};

src/drivers/uavcan/sensors/sensor_bridge.cpp

@@ -48,6 +48,7 @@
 #include "rangefinder.hpp"
 #include "accel.hpp"
 #include "gyro.hpp"
+#include "cbat.hpp"
 
 /*
  * IUavcanSensorBridge
@@ -58,7 +59,18 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
 	list.add(new UavcanMagnetometerBridge(node));
 	list.add(new UavcanGnssBridge(node));
 	list.add(new UavcanFlowBridge(node));
-	list.add(new UavcanBatteryBridge(node));
+
+	int32_t bat_monitor;
+	param_t _param_bat_monitor = param_find("UAVCAN_BAT_MON");
+	param_get(_param_bat_monitor, &bat_monitor);
+
+	if (bat_monitor == 0) {
+		list.add(new UavcanBatteryBridge(node));
+
+	} else if (bat_monitor == 1) {
+		list.add(new UavcanCBATBridge(node));
+	}
+
 	list.add(new UavcanAirspeedBridge(node));
 	list.add(new UavcanDifferentialPressureBridge(node));
 	list.add(new UavcanRangefinderBridge(node));

src/drivers/uavcan/uavcan_params.c

@@ -193,3 +193,20 @@ PARAM_DEFINE_INT32(UAVCAN_LGT_NAV, 3);
  * @group UAVCAN
  */
 PARAM_DEFINE_INT32(UAVCAN_LGT_LAND, 0);
+
+/**
+ * UAVCAN BATTERY_MONITOR battery monitor selection
+ *
+ * This parameter defines that the system will select the battery monitor under the following conditions
+ *
+ *  0 - default battery monitor
+ *  1 - CUAV battery monitor
+ *
+ * @min 0
+ * @max 1
+ * @value 0 default battery monitor
+ * @value 1 CUAV battery monitor
+ * @reboot_required true
+ * @group UAVCAN
+ */
+PARAM_DEFINE_INT32(UAVCAN_BAT_MON, 0);