diff --git a/src/drivers/uavcan/CMakeLists.txt b/src/drivers/uavcan/CMakeLists.txt index 07ba27b3a3..8dd796f12e 100644 --- a/src/drivers/uavcan/CMakeLists.txt +++ b/src/drivers/uavcan/CMakeLists.txt @@ -164,7 +164,6 @@ px4_add_module( sensors/rangefinder.cpp sensors/accel.cpp sensors/gyro.cpp - sensors/cbat.cpp sensors/ice_status.cpp sensors/hygrometer.cpp diff --git a/src/drivers/uavcan/sensors/battery.cpp b/src/drivers/uavcan/sensors/battery.cpp index eedf4b7c6b..d92ef509d8 100644 --- a/src/drivers/uavcan/sensors/battery.cpp +++ b/src/drivers/uavcan/sensors/battery.cpp @@ -42,6 +42,7 @@ UavcanBatteryBridge::UavcanBatteryBridge(uavcan::INode &node) : UavcanSensorBridgeBase("uavcan_battery", ORB_ID(battery_status)), ModuleParams(nullptr), _sub_battery(node), + _sub_battery_aux(node), _warning(battery_status_s::BATTERY_WARNING_NONE), _last_timestamp(0) { @@ -56,52 +57,114 @@ int UavcanBatteryBridge::init() return res; } + res = _sub_battery_aux.start(BatteryInfoAuxCbBinder(this, &UavcanBatteryBridge::battery_aux_sub_cb)); + + if (res < 0) { + PX4_ERR("failed to start uavcan sub: %d", res); + return res; + } + return 0; } void UavcanBatteryBridge::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 = msg.voltage; - battery.current_a = msg.current; - battery.current_filtered_a = msg.current; - // battery.current_average_a = msg.; - - sumDischarged(battery.timestamp, battery.current_a); - battery.discharged_mah = _discharged_mah; - - battery.remaining = msg.state_of_charge_pct / 100.0f; // between 0 and 1 - // battery.scale = msg.; // Power scaling factor, >= 1, or -1 if unknown - battery.temperature = msg.temperature + CONSTANTS_ABSOLUTE_NULL_CELSIUS; // Kelvin to Celcius - // battery.cell_count = msg.; - battery.connected = true; - battery.source = msg.status_flags & uavcan::equipment::power::BatteryInfo::STATUS_FLAG_IN_USE; - // battery.priority = msg.; - battery.capacity = msg.full_charge_capacity_wh; - // battery.cycle_count = msg.; - // battery.time_remaining_s = msg.; - // battery.average_time_to_empty = msg.; - battery.serial_number = msg.model_instance_id; - battery.id = msg.getSrcNodeID().get(); - - // Mavlink 2 needs individual cell voltages or cell[0] if cell voltages are not available. - battery.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.cell_count = 1; - - // battery.max_cell_voltage_delta = msg.; - - // battery.is_powering_off = msg.; - - determineWarning(battery.remaining); - battery.warning = _warning; - - publish(msg.getSrcNodeID().get(), &battery); + 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; + } + + battery_status[instance].timestamp = hrt_absolute_time(); + battery_status[instance].voltage_v = msg.voltage; + battery_status[instance].voltage_filtered_v = msg.voltage; + battery_status[instance].current_a = msg.current; + battery_status[instance].current_filtered_a = msg.current; + // battery_status[instance].current_average_a = msg.; + + if (battery_aux_support[instance] == false) { + 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 + CONSTANTS_ABSOLUTE_NULL_CELSIUS; // Kelvin to Celcius + // 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.; + 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 = msg.; + // 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 (battery_aux_support[instance] == false) { + // 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 (battery_aux_support[instance] == false) { + publish(msg.getSrcNodeID().get(), &battery_status[instance]); + } +} + +void +UavcanBatteryBridge::battery_aux_sub_cb(const uavcan::ReceivedDataStructure + &msg) +{ + uint8_t instance = 0; + + for (instance = 0; instance < battery_status_s::MAX_INSTANCES; instance++) { + if (battery_status[instance].id == msg.getSrcNodeID().get()) { + break; + } + } + + if (instance >= battery_status_s::MAX_INSTANCES) { + return; + } + + battery_aux_support[instance] = true; + + battery_status[instance].discharged_mah = (battery_status[instance].full_charge_capacity_wh - + battery_status[instance].remaining_capacity_wh) / msg.nominal_voltage * + 1000; + battery_status[instance].cell_count = math::min((uint8_t)msg.voltage_cell.size(), (uint8_t)14); + battery_status[instance].cycle_count = msg.cycle_count; + battery_status[instance].over_discharge_count = msg.over_discharge_count; + battery_status[instance].nominal_voltage = msg.nominal_voltage; + battery_status[instance].time_remaining_s = math::isZero(battery_status[instance].current_a) ? 0 : + (battery_status[instance].remaining_capacity_wh / + battery_status[instance].nominal_voltage / battery_status[instance].current_a * 3600); + battery_status[instance].is_powering_off = msg.is_powering_off; + + for (uint8_t i = 0; i < battery_status[instance].cell_count; i++) { + battery_status[instance].voltage_cell_v[i] = msg.voltage_cell[i]; + } + + publish(msg.getSrcNodeID().get(), &battery_status[instance]); } void diff --git a/src/drivers/uavcan/sensors/battery.hpp b/src/drivers/uavcan/sensors/battery.hpp index 22608154a3..8e26c5153a 100644 --- a/src/drivers/uavcan/sensors/battery.hpp +++ b/src/drivers/uavcan/sensors/battery.hpp @@ -40,6 +40,7 @@ #include "sensor_bridge.hpp" #include #include +#include #include #include @@ -57,6 +58,7 @@ public: private: void battery_sub_cb(const uavcan::ReceivedDataStructure &msg); + void battery_aux_sub_cb(const uavcan::ReceivedDataStructure &msg); void sumDischarged(hrt_abstime timestamp, float current_a); void determineWarning(float remaining); @@ -64,8 +66,13 @@ private: void (UavcanBatteryBridge::*) (const uavcan::ReceivedDataStructure &) > BatteryInfoCbBinder; + typedef uavcan::MethodBinder < UavcanBatteryBridge *, + void (UavcanBatteryBridge::*) + (const uavcan::ReceivedDataStructure &) > + BatteryInfoAuxCbBinder; uavcan::Subscriber _sub_battery; + uavcan::Subscriber _sub_battery_aux; DEFINE_PARAMETERS( (ParamFloat) _param_bat_low_thr, @@ -77,4 +84,6 @@ private: float _discharged_mah_loop = 0.f; uint8_t _warning; hrt_abstime _last_timestamp; + battery_status_s battery_status[battery_status_s::MAX_INSTANCES] {}; + bool battery_aux_support[battery_status_s::MAX_INSTANCES] {}; }; diff --git a/src/drivers/uavcan/sensors/cbat.cpp b/src/drivers/uavcan/sensors/cbat.cpp deleted file mode 100644 index d28827eeca..0000000000 --- a/src/drivers/uavcan/sensors/cbat.cpp +++ /dev/null @@ -1,139 +0,0 @@ -/**************************************************************************** - * - * 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.current_average_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.time_remaining_s = msg.average_time_to_empty * 60; - 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 deleted file mode 100644 index b70aa7b1c5..0000000000 --- a/src/drivers/uavcan/sensors/cbat.hpp +++ /dev/null @@ -1,78 +0,0 @@ -/**************************************************************************** - * - * 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 89bcfccafd..c23b921cdd 100644 --- a/src/drivers/uavcan/sensors/sensor_bridge.cpp +++ b/src/drivers/uavcan/sensors/sensor_bridge.cpp @@ -42,7 +42,6 @@ #include "airspeed.hpp" #include "baro.hpp" #include "battery.hpp" -#include "cbat.hpp" #include "differential_pressure.hpp" #include "flow.hpp" #include "gnss.hpp" @@ -77,11 +76,8 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List