diff --git a/msg/battery_status.msg b/msg/battery_status.msg index 29925fe883..77a190a065 100644 --- a/msg/battery_status.msg +++ b/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 diff --git a/src/drivers/uavcan/CMakeLists.txt b/src/drivers/uavcan/CMakeLists.txt index 48d998715d..a03333b153 100644 --- a/src/drivers/uavcan/CMakeLists.txt +++ b/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 diff --git a/src/drivers/uavcan/dsdl/cuav/equipment/power/20300.CBAT.uavcan b/src/drivers/uavcan/dsdl/cuav/equipment/power/20300.CBAT.uavcan new file mode 100755 index 0000000000..2c9ff56d53 --- /dev/null +++ b/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 diff --git a/src/drivers/uavcan/sensors/cbat.cpp b/src/drivers/uavcan/sensors/cbat.cpp new file mode 100644 index 0000000000..f40f3481b6 --- /dev/null +++ b/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 +#include + +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 &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; + } +} diff --git a/src/drivers/uavcan/sensors/cbat.hpp b/src/drivers/uavcan/sensors/cbat.hpp new file mode 100644 index 0000000000..b70aa7b1c5 --- /dev/null +++ b/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 + */ + + +#pragma once + +#include "sensor_bridge.hpp" +#include +#include +#include +#include +#include + +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 &msg); + void determineWarning(float remaining); + + typedef uavcan::MethodBinder < UavcanCBATBridge *, + void (UavcanCBATBridge::*) + (const uavcan::ReceivedDataStructure &) > + CBATCbBinder; + + uavcan::Subscriber _sub_battery; + + DEFINE_PARAMETERS( + (ParamFloat) _param_bat_low_thr, + (ParamFloat) _param_bat_crit_thr, + (ParamFloat) _param_bat_emergen_thr + ) + + uint8_t _warning; + float _max_cell_voltage_delta = 0.f; +}; diff --git a/src/drivers/uavcan/sensors/sensor_bridge.cpp b/src/drivers/uavcan/sensors/sensor_bridge.cpp index 4f629aa01c..4429c8d723 100644 --- a/src/drivers/uavcan/sensors/sensor_bridge.cpp +++ b/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