diff --git a/CMakeLists.txt b/CMakeLists.txt index f249164aa63a..ffdf2ff4bf3e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -399,6 +399,7 @@ if(BUILD_TESTING) set(FUZZTEST_FUZZING_MODE ON) endif() add_subdirectory(test) + add_subdirectory(src/drivers/uavcan/sensors/test EXCLUDE_FROM_ALL) fuzztest_setup_fuzzing_flags() endif() diff --git a/src/drivers/uavcan/sensors/battery.cpp b/src/drivers/uavcan/sensors/battery.cpp index 8dfff769c560..81401c334209 100644 --- a/src/drivers/uavcan/sensors/battery.cpp +++ b/src/drivers/uavcan/sensors/battery.cpp @@ -37,6 +37,19 @@ #include #include +// Pre-shifted bitmask helpers for battery_status_s fault flags. +#define FAULT_DEEP_DISCHARGE_FLAG (1 << battery_status_s::FAULT_DEEP_DISCHARGE) +#define FAULT_SPIKES_FLAG (1 << battery_status_s::FAULT_SPIKES) +#define FAULT_CELL_FAIL_FLAG (1 << battery_status_s::FAULT_CELL_FAIL) +#define FAULT_OVER_CURRENT_FLAG (1 << battery_status_s::FAULT_OVER_CURRENT) +#define FAULT_OVER_TEMPERATURE_FLAG (1 << battery_status_s::FAULT_OVER_TEMPERATURE) +#define FAULT_UNDER_TEMPERATURE_FLAG (1 << battery_status_s::FAULT_UNDER_TEMPERATURE) +#define FAULT_INCOMPATIBLE_VOLTAGE_FLAG (1 << battery_status_s::FAULT_INCOMPATIBLE_VOLTAGE) +#define FAULT_INCOMPATIBLE_FIRMWARE_FLAG (1 << battery_status_s::FAULT_INCOMPATIBLE_FIRMWARE) +#define FAULT_INCOMPATIBLE_MODEL_FLAG (1 << battery_status_s::FAULT_INCOMPATIBLE_MODEL) +#define FAULT_HARDWARE_FAILURE_FLAG (1 << battery_status_s::FAULT_HARDWARE_FAILURE) +#define FAULT_FAILED_TO_ARM_FLAG (1 << battery_status_s::FAULT_FAILED_TO_ARM) + const char *const UavcanBatteryBridge::NAME = "battery"; UavcanBatteryBridge::UavcanBatteryBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher) : @@ -45,6 +58,9 @@ UavcanBatteryBridge::UavcanBatteryBridge(uavcan::INode &node, NodeInfoPublisher _sub_battery(node), _sub_battery_aux(node), _sub_cbat(node), + _sub_battery_continuous(node), + _sub_battery_periodic(node), + _sub_battery_cells(node), _warning(battery_status_s::WARNING_NONE), _last_timestamp(0) { @@ -86,6 +102,27 @@ int UavcanBatteryBridge::init() return res; } + res = _sub_battery_continuous.start(BatteryContinuousCbBinder(this, &UavcanBatteryBridge::battery_continuous_sub_cb)); + + if (res < 0) { + PX4_ERR("failed to start uavcan sub: %d", res); + return res; + } + + res = _sub_battery_periodic.start(BatteryPeriodicCbBinder(this, &UavcanBatteryBridge::battery_periodic_sub_cb)); + + if (res < 0) { + PX4_ERR("failed to start uavcan sub: %d", res); + return res; + } + + res = _sub_battery_cells.start(BatteryCellsCbBinder(this, &UavcanBatteryBridge::battery_cells_sub_cb)); + + if (res < 0) { + PX4_ERR("failed to start uavcan sub: %d", res); + return res; + } + return 0; } @@ -106,7 +143,8 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure= battery_status_s::MAX_INSTANCES - || _batt_update_mod[instance] == BatteryDataType::CBAT) { + || _batt_update_mod[instance] == BatteryDataType::CBAT + || _batt_update_mod[instance] == BatteryDataType::Multi) { return; } @@ -175,7 +213,8 @@ UavcanBatteryBridge::battery_aux_sub_cb(const uavcan::ReceivedDataStructure= battery_status_s::MAX_INSTANCES || _batt_update_mod[instance] == BatteryDataType::Filter - || _batt_update_mod[instance] == BatteryDataType::CBAT) { + || _batt_update_mod[instance] == BatteryDataType::CBAT + || _batt_update_mod[instance] == BatteryDataType::Multi) { return; } @@ -221,7 +260,8 @@ void UavcanBatteryBridge::cbat_sub_cb(const uavcan::ReceivedDataStructure= battery_status_s::MAX_INSTANCES - || _batt_update_mod[instance] == BatteryDataType::Filter) { + || _batt_update_mod[instance] == BatteryDataType::Filter + || _batt_update_mod[instance] == BatteryDataType::Multi) { return; } @@ -270,19 +310,19 @@ void UavcanBatteryBridge::cbat_sub_cb(const uavcan::ReceivedDataStructure + &msg) +{ + using BatteryContinuous = ardupilot::equipment::power::BatteryContinuous; + + uint8_t instance = 0; + + for (instance = 0; instance < battery_status_s::MAX_INSTANCES; instance++) { + if (_node_ids[instance] == msg.getSrcNodeID().get() || _node_ids[instance] == 0) { + break; + } + } + + if (instance >= battery_status_s::MAX_INSTANCES + || _batt_update_mod[instance] == BatteryDataType::Filter) { + return; + } + + // Take ownership of this node_id and suppress legacy battery messages from it. + _batt_update_mod[instance] = BatteryDataType::Multi; + _node_ids[instance] = msg.getSrcNodeID().get(); + + _battery_status[instance].timestamp = hrt_absolute_time(); + _battery_status[instance].voltage_v = msg.voltage; + _battery_status[instance].current_a = msg.current; + + _battery_status[instance].temperature = msg.temperature_cells; + + _battery_status[instance].remaining = msg.state_of_charge / 100.f; + _battery_status[instance].discharged_mah = msg.capacity_consumed * 1000.f; + + _battery_status[instance].scale = -1.f; + _battery_status[instance].connected = true; + _battery_status[instance].source = battery_status_s::SOURCE_EXTERNAL; + _battery_status[instance].id = msg.getSrcNodeID().get(); + + // use Battery class for time_remaining calculation + _battery[instance]->updateDt(_battery_status[instance].timestamp); + _battery[instance]->setStateOfCharge(_battery_status[instance].remaining); + _battery_status[instance].time_remaining_s = + _battery[instance]->computeRemainingTime(fabsf(_battery_status[instance].current_a)); + _battery_status[instance].current_average_a = _battery[instance]->getCurrentAverage(); + + uint16_t faults = 0; + + if (msg.status_flags & BatteryContinuous::STATUS_FLAG_FAULT_OVER_CURRENT) { + faults |= FAULT_OVER_CURRENT_FLAG; + } + + if (msg.status_flags & BatteryContinuous::STATUS_FLAG_FAULT_OVER_TEMP) { + faults |= FAULT_OVER_TEMPERATURE_FLAG; + } + + if (msg.status_flags & BatteryContinuous::STATUS_FLAG_FAULT_UNDER_TEMP) { + faults |= FAULT_UNDER_TEMPERATURE_FLAG; + } + + if (msg.status_flags & BatteryContinuous::STATUS_FLAG_FAULT_INCOMPATIBLE_VOLTAGE) { + faults |= FAULT_INCOMPATIBLE_VOLTAGE_FLAG; + } + + if (msg.status_flags & BatteryContinuous::STATUS_FLAG_FAULT_INCOMPATIBLE_FIRMWARE) { + faults |= FAULT_INCOMPATIBLE_FIRMWARE_FLAG; + } + + if (msg.status_flags & BatteryContinuous::STATUS_FLAG_FAULT_INCOMPATIBLE_CELLS_CONFIGURATION) { + faults |= FAULT_INCOMPATIBLE_MODEL_FLAG; + } + + if (msg.status_flags & (BatteryContinuous::STATUS_FLAG_FAULT_SHORT_CIRCUIT + | BatteryContinuous::STATUS_FLAG_FAULT_PROTECTION_SYSTEM + | BatteryContinuous::STATUS_FLAG_FAULT_CELL_IMBALANCE + | BatteryContinuous::STATUS_FLAG_BAD_BATTERY)) { + faults |= FAULT_HARDWARE_FAILURE_FLAG; + } + + if (msg.status_flags & BatteryContinuous::STATUS_FLAG_FAULT_UNDER_VOLT) { + faults |= FAULT_DEEP_DISCHARGE_FLAG; + } + + _battery_status[instance].faults = faults; + + if (faults != 0) { + _battery_status[instance].warning = battery_status_s::STATE_UNHEALTHY; + + } else if (msg.status_flags & BatteryContinuous::STATUS_FLAG_CHARGING) { + _battery_status[instance].warning = battery_status_s::STATE_CHARGING; + + } else { + _battery_status[instance].warning = _battery[instance]->determineWarning(_battery_status[instance].remaining); + } + + publish(msg.getSrcNodeID().get(), &_battery_status[instance]); + + if (_node_info_publisher != nullptr) { + _node_info_publisher->registerDeviceCapability(msg.getSrcNodeID().get(), + msg.getSrcNodeID().get(), NodeInfoPublisher::DeviceCapability::BATTERY); + } +} + +void +UavcanBatteryBridge::battery_periodic_sub_cb(const uavcan::ReceivedDataStructure + &msg) +{ + uint8_t instance = 0; + + for (instance = 0; instance < battery_status_s::MAX_INSTANCES; instance++) { + if (_node_ids[instance] == msg.getSrcNodeID().get() || _node_ids[instance] == 0) { + break; + } + } + + if (instance >= battery_status_s::MAX_INSTANCES + || _batt_update_mod[instance] == BatteryDataType::Filter) { + return; + } + + _batt_update_mod[instance] = BatteryDataType::Multi; + _node_ids[instance] = msg.getSrcNodeID().get(); + + _battery_status[instance].cell_count = msg.cells_in_series; + _battery_status[instance].nominal_voltage = (float)msg.nominal_voltage > FLT_EPSILON ? (float)msg.nominal_voltage : NAN; + + float capacity_mah = 0.f; + + // if the Battery has an full_charge_estimate, use this, otherwise use the design_capacity as a fallback + if (PX4_ISFINITE(msg.full_charge_capacity)) { + capacity_mah = msg.full_charge_capacity * 1000.f; + + } else if (msg.design_capacity > 0.f) { + capacity_mah = msg.design_capacity * 1000.f; + } + + _battery_status[instance].capacity = (uint16_t)capacity_mah; + + // if nominal voltage or capacity is not provided, both of them are zero. resulting in a full_charge_capacity_wh of zero + _battery_status[instance].full_charge_capacity_wh = capacity_mah * msg.nominal_voltage / 1000.f; + + if (capacity_mah > FLT_EPSILON) { // if neither design_capacity nor full_charge_estimate is provided, use param + _battery[instance]->setCapacityMah(_battery_status[instance].capacity); + } + + if (msg.cycle_count != UINT16_MAX) { + _battery_status[instance].cycle_count = msg.cycle_count; + } + + if (msg.state_of_health != UINT8_MAX) { + _battery_status[instance].state_of_health = msg.state_of_health; + } + + // Parse manufacture_date "DDMMYYYY" ASCII into Day + Month*32 + (Year-1980)*512 + if (msg.manufacture_date.size() >= 8) { + char buf[9] = {}; + + for (size_t i = 0; i < 8 && i < msg.manufacture_date.size(); i++) { + buf[i] = (char)msg.manufacture_date[i]; + } + + int day = 0, month = 0, year = 0; + + if (sscanf(buf, "%2d%2d%4d", &day, &month, &year) == 3 + && day >= 1 && day <= 31 && month >= 1 && month <= 12 && year >= 1980) { + _battery_status[instance].manufacture_date = (uint16_t)(day + month * 32 + (year - 1980) * 512); + } + } + + // Publish battery_info on every Periodic message. + _battery_info[instance].timestamp = hrt_absolute_time(); + _battery_info[instance].id = msg.getSrcNodeID().get(); + memset(_battery_info[instance].serial_number, 0, sizeof(_battery_info[instance].serial_number)); + memcpy(_battery_info[instance].serial_number, msg.serial_number.begin(), + math::min((size_t)msg.serial_number.size(), sizeof(_battery_info[instance].serial_number) - 1)); + + _battery_info_pub[instance].publish(_battery_info[instance]); +} + +void +UavcanBatteryBridge::battery_cells_sub_cb(const uavcan::ReceivedDataStructure + &msg) +{ + uint8_t instance = 0; + + for (instance = 0; instance < battery_status_s::MAX_INSTANCES; instance++) { + if (_node_ids[instance] == msg.getSrcNodeID().get() || _node_ids[instance] == 0) { + break; + } + } + + if (instance >= battery_status_s::MAX_INSTANCES + || _batt_update_mod[instance] == BatteryDataType::Filter) { + return; + } + + _batt_update_mod[instance] = BatteryDataType::Multi; + _node_ids[instance] = msg.getSrcNodeID().get(); + + // uORB holds 14 cells; BatteryCells supports more + constexpr size_t max_cells = sizeof(_battery_status[0].voltage_cell_v) / sizeof(_battery_status[0].voltage_cell_v[0]); + + // BatteryCells are chunked and provide the start offset in msg.index. + for (size_t i = 0; i < msg.voltages.size(); i++) { + const size_t cell_idx = msg.index + i; + + if (cell_idx >= max_cells) { + break; + } + + _battery_status[instance].voltage_cell_v[cell_idx] = msg.voltages[i]; + } + + _battery_status[instance].max_cell_voltage_delta = + Battery::computeMaxCellVoltageDelta(_battery_status[instance].voltage_cell_v, max_cells); +} + void UavcanBatteryBridge::filterData(const uavcan::ReceivedDataStructure &msg, uint8_t instance) diff --git a/src/drivers/uavcan/sensors/battery.hpp b/src/drivers/uavcan/sensors/battery.hpp index 84dc07522717..db0b2600889a 100644 --- a/src/drivers/uavcan/sensors/battery.hpp +++ b/src/drivers/uavcan/sensors/battery.hpp @@ -42,6 +42,9 @@ #include #include #include +#include +#include +#include #include #include #include @@ -68,11 +71,15 @@ class UavcanBatteryBridge : public UavcanSensorBridgeBase, public ModuleParams RawAux, // data combination from BatteryInfo and BatteryInfoAux messages Filter, // filter data from BatteryInfo message with Battery library CBAT, // CBAT messages + Multi, // multi-message smart battery (e.g. ardupilot BatteryContinuous/Periodic/Cells trio) }; void battery_sub_cb(const uavcan::ReceivedDataStructure &msg); void battery_aux_sub_cb(const uavcan::ReceivedDataStructure &msg); void cbat_sub_cb(const uavcan::ReceivedDataStructure &msg); + void battery_continuous_sub_cb(const uavcan::ReceivedDataStructure &msg); + void battery_periodic_sub_cb(const uavcan::ReceivedDataStructure &msg); + void battery_cells_sub_cb(const uavcan::ReceivedDataStructure &msg); void filterData(const uavcan::ReceivedDataStructure &msg, uint8_t instance); typedef uavcan::MethodBinder < UavcanBatteryBridge *, @@ -89,9 +96,25 @@ class UavcanBatteryBridge : public UavcanSensorBridgeBase, public ModuleParams (const uavcan::ReceivedDataStructure &) > CBATCbBinder; + typedef uavcan::MethodBinder < UavcanBatteryBridge *, + void (UavcanBatteryBridge::*) + (const uavcan::ReceivedDataStructure &) > + BatteryContinuousCbBinder; + typedef uavcan::MethodBinder < UavcanBatteryBridge *, + void (UavcanBatteryBridge::*) + (const uavcan::ReceivedDataStructure &) > + BatteryPeriodicCbBinder; + typedef uavcan::MethodBinder < UavcanBatteryBridge *, + void (UavcanBatteryBridge::*) + (const uavcan::ReceivedDataStructure &) > + BatteryCellsCbBinder; + uavcan::Subscriber _sub_battery; uavcan::Subscriber _sub_battery_aux; uavcan::Subscriber _sub_cbat; + uavcan::Subscriber _sub_battery_continuous; + uavcan::Subscriber _sub_battery_periodic; + uavcan::Subscriber _sub_battery_cells; DEFINE_PARAMETERS( (ParamFloat) _param_bat_low_thr, diff --git a/src/drivers/uavcan/sensors/test/BatteryBridgeMultiInstanceTest.cpp b/src/drivers/uavcan/sensors/test/BatteryBridgeMultiInstanceTest.cpp new file mode 100644 index 000000000000..b71d6aa28196 --- /dev/null +++ b/src/drivers/uavcan/sensors/test/BatteryBridgeMultiInstanceTest.cpp @@ -0,0 +1,127 @@ +/**************************************************************************** + * + * Copyright (c) 2026 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. + * + ****************************************************************************/ + +// Own gtest binary so multi-instance uORB advertises don't leak into the +// single-bridge BatteryBridgeTest suite. + +#include "UavcanBridgeTestFixture.hpp" + +#include + +#include + +#include +#include +#include + +namespace +{ + +constexpr unsigned kSpinMs = 50; + +bool find_battery_status_for_node(uint8_t wanted_node_id, battery_status_s &out) +{ + hrt_abstime newest = 0; + bool found = false; + + for (uint8_t inst = 0; inst < ORB_MULTI_MAX_INSTANCES; inst++) { + uORB::Subscription sub{ORB_ID(battery_status), inst}; + battery_status_s data{}; + + if (!sub.copy(&data)) { + continue; + } + + if (data.id == wanted_node_id && data.timestamp >= newest) { + newest = data.timestamp; + out = data; + found = true; + } + } + + return found; +} + +class BatteryBridgeMultiInstanceTest : public UavcanBridgeTestFixtureT<3> +{ +protected: + void SetUp() override + { + UavcanBridgeTestFixtureT<3>::SetUp(); + _bridge = std::make_unique(subscriber_node(), nullptr); + ASSERT_EQ(_bridge->init(), 0); + } + + void TearDown() override + { + _bridge.reset(); + UavcanBridgeTestFixtureT<3>::TearDown(); + } + + std::unique_ptr _bridge; +}; + +TEST_F(BatteryBridgeMultiInstanceTest, TwoNodesFillSeparateInstances) +{ + ardupilot::equipment::power::BatteryContinuous msg_a; + msg_a.voltage = 22.0f; + msg_a.current = 3.0f; + msg_a.state_of_charge = 60.0f; + msg_a.slot_id = 1; + + ardupilot::equipment::power::BatteryContinuous msg_b; + msg_b.voltage = 24.5f; + msg_b.current = 5.5f; + msg_b.state_of_charge = 85.0f; + msg_b.slot_id = 2; + + ASSERT_GE(broadcast(msg_a, /*publisher_idx*/ 0), 0); + ASSERT_GE(broadcast(msg_b, /*publisher_idx*/ 1), 0); + ASSERT_GE(spin(kSpinMs), 0); + + battery_status_s data_a{}; + battery_status_s data_b{}; + + constexpr uint8_t kNodeA = uavcan_bridge_test::kFirstPublisherNodeId; + constexpr uint8_t kNodeB = uavcan_bridge_test::kFirstPublisherNodeId + 1; + + ASSERT_TRUE(find_battery_status_for_node(kNodeA, data_a)) + << "battery_status instance for node " << (int)kNodeA << " not populated"; + ASSERT_TRUE(find_battery_status_for_node(kNodeB, data_b)) + << "battery_status instance for node " << (int)kNodeB << " not populated"; + + EXPECT_FLOAT_EQ(data_a.voltage_v, 22.0f); + EXPECT_FLOAT_EQ(data_b.voltage_v, 24.5f); +} + +} // namespace diff --git a/src/drivers/uavcan/sensors/test/BatteryBridgeTest.cpp b/src/drivers/uavcan/sensors/test/BatteryBridgeTest.cpp new file mode 100644 index 000000000000..2654ca76f517 --- /dev/null +++ b/src/drivers/uavcan/sensors/test/BatteryBridgeTest.cpp @@ -0,0 +1,443 @@ +/**************************************************************************** + * + * Copyright (c) 2026 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 "UavcanBridgeTestFixture.hpp" + +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace +{ + +constexpr unsigned kSpinMs = 50; + +// Single publisher node ID across the suite — bridge reuses one +// _battery_status slot, so uORB only advertises one instance per topic. +constexpr uint8_t kPublisherNodeId = uavcan_bridge_test::kFirstPublisherNodeId; + +bool find_battery_status_for_node(uint8_t wanted_node_id, battery_status_s &out) +{ + hrt_abstime newest = 0; + bool found = false; + + for (uint8_t inst = 0; inst < ORB_MULTI_MAX_INSTANCES; inst++) { + uORB::Subscription sub{ORB_ID(battery_status), inst}; + battery_status_s data{}; + + if (!sub.copy(&data)) { + continue; + } + + if (data.id == wanted_node_id && data.timestamp >= newest) { + newest = data.timestamp; + out = data; + found = true; + } + } + + return found; +} + +bool find_battery_info_for_node(uint8_t wanted_node_id, battery_info_s &out) +{ + hrt_abstime newest = 0; + bool found = false; + + for (uint8_t inst = 0; inst < ORB_MULTI_MAX_INSTANCES; inst++) { + uORB::Subscription sub{ORB_ID(battery_info), inst}; + battery_info_s data{}; + + if (!sub.copy(&data)) { + continue; + } + + if (data.id == wanted_node_id && data.timestamp >= newest) { + newest = data.timestamp; + out = data; + found = true; + } + } + + return found; +} + +ardupilot::equipment::power::BatteryPeriodic make_default_periodic() +{ + ardupilot::equipment::power::BatteryPeriodic per; + per.cells_in_series = 6; + per.nominal_voltage = 22.2f; + per.full_charge_capacity = 6.6f; + per.design_capacity = 6.6f; + per.cycle_count = 42; + per.state_of_health = 98; + return per; +} + +ardupilot::equipment::power::BatteryContinuous make_default_continuous() +{ + ardupilot::equipment::power::BatteryContinuous cont; + cont.voltage = 22.2f; + cont.current = 4.0f; + cont.state_of_charge = 80.0f; + cont.slot_id = 1; + cont.status_flags = 0; + return cont; +} + +// Bridge + network shared across the suite — uORB has no unadvertise path, +// so a fresh bridge per test would exhaust ORB_MULTI_MAX_INSTANCES. +// reset_to_baseline() in SetUp() decouples per-test state. +class BatteryBridgeTest : public ::testing::Test +{ +protected: + static constexpr unsigned kNumNodes = 2; + + static void SetUpTestSuite() + { + _shared_network = std::make_unique>(uavcan_bridge_test::kSubscriberNodeId); + _shared_bridge = std::make_unique((*_shared_network)[0], nullptr); + ASSERT_EQ(_shared_bridge->init(), 0); + } + + static void TearDownTestSuite() + { + _shared_bridge.reset(); + _shared_network.reset(); + } + + void SetUp() override + { + // Substitute for a test-only state-reset hook into the bridge. + reset_to_baseline(); + } + + TestNode &publisher_node() { return (*_shared_network)[1]; } + + int spin(unsigned ms) + { + return _shared_network->spinAll(uavcan::MonotonicDuration::fromMSec(ms)); + } + + template + int broadcast(const Msg &msg) + { + uavcan::Publisher pub(publisher_node()); + const int init_res = pub.init(); + + if (init_res < 0) { + return init_res; + } + + return pub.broadcast(msg); + } + + UavcanBatteryBridge *bridge() { return _shared_bridge.get(); } + + static std::unique_ptr> _shared_network; + static std::unique_ptr _shared_bridge; + +private: + void reset_to_baseline() + { + // Periodic with 0 cycle_count / 0 state_of_health stamps known values + // past the bridge's UINT16_MAX / UINT8_MAX skip conditions. + ardupilot::equipment::power::BatteryPeriodic per; + per.cells_in_series = 0; + per.nominal_voltage = 22.2f; + per.full_charge_capacity = NAN; + per.design_capacity = 0.0f; + per.cycle_count = 0; + per.state_of_health = 0; + ASSERT_GE(broadcast(per), 0); + ASSERT_GE(spin(kSpinMs), 0); + + // 14 zero-volt entries wipe voltage_cell_v left by prior tests. + ardupilot::equipment::power::BatteryCells cells; + cells.index = 0; + constexpr size_t kMaxCells = 14; + + for (size_t i = 0; i < kMaxCells; ++i) { + cells.voltages.push_back(0.0f); + } + + ASSERT_GE(broadcast(cells), 0); + ASSERT_GE(spin(kSpinMs), 0); + + // Continuous flushes merged state to battery_status uORB. + ASSERT_GE(broadcast(make_default_continuous()), 0); + ASSERT_GE(spin(kSpinMs), 0); + } +}; + +std::unique_ptr> BatteryBridgeTest::_shared_network; +std::unique_ptr BatteryBridgeTest::_shared_bridge; + +TEST_F(BatteryBridgeTest, BridgeReportsName) +{ + EXPECT_STREQ(bridge()->get_name(), "battery"); +} + +TEST_F(BatteryBridgeTest, ContinuousFieldsMapToBatteryStatus) +{ + ardupilot::equipment::power::BatteryContinuous msg; + msg.voltage = 23.1f; + msg.current = 8.2f; + msg.state_of_charge = 72.0f; + msg.capacity_consumed = 1.8f; + msg.slot_id = 7; + msg.status_flags = 0; + + ASSERT_GE(broadcast(msg), 0); + ASSERT_GE(spin(kSpinMs), 0); + + battery_status_s data{}; + ASSERT_TRUE(find_battery_status_for_node(kPublisherNodeId, data)) << "no battery_status published"; + + EXPECT_FLOAT_EQ(data.voltage_v, 23.1f); + EXPECT_FLOAT_EQ(data.current_a, 8.2f); + EXPECT_NEAR(data.remaining, 0.72f, 1e-3f); + EXPECT_TRUE(data.connected); + EXPECT_EQ(data.source, battery_status_s::SOURCE_EXTERNAL); +} + +TEST_F(BatteryBridgeTest, PeriodicSetsCellCountAndNominalVoltage) +{ + // Continuous first so the node enters Multi mode and owns its slot. + ASSERT_GE(broadcast(make_default_continuous()), 0); + ASSERT_GE(spin(kSpinMs), 0); + + ardupilot::equipment::power::BatteryPeriodic per; + per.cells_in_series = 6; + per.nominal_voltage = 22.2f; + per.full_charge_capacity = 6.6f; // Ah -> 6600 mAh + per.design_capacity = 6.6f; + per.cycle_count = 42; + per.state_of_health = 98; + + ASSERT_GE(broadcast(per), 0); + ASSERT_GE(spin(kSpinMs), 0); + + // Periodic updates the bridge cache but only publishes battery_info; + // follow-up Continuous flushes to battery_status uORB. + ASSERT_GE(broadcast(make_default_continuous()), 0); + ASSERT_GE(spin(kSpinMs), 0); + + battery_status_s data{}; + ASSERT_TRUE(find_battery_status_for_node(kPublisherNodeId, data)) << "no battery_status published"; + + EXPECT_EQ(data.cell_count, 6); + // float16 wire format — loose tolerance for quantisation + EXPECT_NEAR(data.nominal_voltage, 22.2f, 0.05f); + EXPECT_EQ(data.capacity, 6600); + EXPECT_EQ(data.cycle_count, 42); + EXPECT_EQ(data.state_of_health, 98); +} + +TEST_F(BatteryBridgeTest, CellsPopulateVoltagesAndDelta) +{ + ASSERT_GE(broadcast(make_default_continuous()), 0); + ASSERT_GE(spin(kSpinMs), 0); + + ardupilot::equipment::power::BatteryCells cells; + cells.index = 0; + cells.voltages.push_back(3.85f); + cells.voltages.push_back(3.84f); + cells.voltages.push_back(3.86f); + cells.voltages.push_back(3.85f); + cells.voltages.push_back(3.83f); + cells.voltages.push_back(3.85f); + + ASSERT_GE(broadcast(cells), 0); + ASSERT_GE(spin(kSpinMs), 0); + + // Cells doesn't publish on its own; follow-up Continuous flushes to uORB. + ASSERT_GE(broadcast(make_default_continuous()), 0); + ASSERT_GE(spin(kSpinMs), 0); + + battery_status_s data{}; + ASSERT_TRUE(find_battery_status_for_node(kPublisherNodeId, data)) << "no battery_status published"; + + EXPECT_NEAR(data.voltage_cell_v[0], 3.85f, 1e-2f); + EXPECT_NEAR(data.voltage_cell_v[2], 3.86f, 1e-2f); + EXPECT_NEAR(data.voltage_cell_v[4], 3.83f, 1e-2f); + // float16 quantisation tolerance + EXPECT_NEAR(data.max_cell_voltage_delta, 0.03f, 1e-2f); +} + +TEST_F(BatteryBridgeTest, PeriodicSerialNumberPublishedToBatteryInfo) +{ + auto per = make_default_periodic(); + const char *serial = "ABC123"; + + for (const char *p = serial; *p; ++p) { + per.serial_number.push_back((uint8_t)*p); + } + + ASSERT_GE(broadcast(per), 0); + ASSERT_GE(spin(kSpinMs), 0); + + battery_info_s info{}; + ASSERT_TRUE(find_battery_info_for_node(kPublisherNodeId, info)) << "no battery_info published"; + EXPECT_STREQ(info.serial_number, serial); +} + +TEST_F(BatteryBridgeTest, BatteryInfoIdMatchesBatteryStatusId) +{ + auto per = make_default_periodic(); + per.serial_number.push_back('S'); + ASSERT_GE(broadcast(per), 0); + ASSERT_GE(spin(kSpinMs), 0); + + ASSERT_GE(broadcast(make_default_continuous()), 0); + ASSERT_GE(spin(kSpinMs), 0); + + battery_status_s status{}; + battery_info_s info{}; + ASSERT_TRUE(find_battery_status_for_node(kPublisherNodeId, status)); + ASSERT_TRUE(find_battery_info_for_node(kPublisherNodeId, info)); + EXPECT_EQ(status.id, info.id); +} + +TEST_F(BatteryBridgeTest, PeriodicWithZeroNominalVoltageYieldsNan) +{ + auto per = make_default_periodic(); + per.nominal_voltage = 0.0f; + + ASSERT_GE(broadcast(per), 0); + ASSERT_GE(spin(kSpinMs), 0); + ASSERT_GE(broadcast(make_default_continuous()), 0); + ASSERT_GE(spin(kSpinMs), 0); + + battery_status_s data{}; + ASSERT_TRUE(find_battery_status_for_node(kPublisherNodeId, data)); + EXPECT_TRUE(std::isnan(data.nominal_voltage)) << "expected NaN, got " << data.nominal_voltage; +} + +TEST_F(BatteryBridgeTest, PeriodicWithoutCapacityKeepsZeroCapacity) +{ + auto per = make_default_periodic(); + per.full_charge_capacity = NAN; + per.design_capacity = 0.0f; + + ASSERT_GE(broadcast(per), 0); + ASSERT_GE(spin(kSpinMs), 0); + ASSERT_GE(broadcast(make_default_continuous()), 0); + ASSERT_GE(spin(kSpinMs), 0); + + battery_status_s data{}; + ASSERT_TRUE(find_battery_status_for_node(kPublisherNodeId, data)); + EXPECT_EQ(data.capacity, 0); +} + +TEST_F(BatteryBridgeTest, PeriodicFallsBackToDesignCapacityWhenFullChargeNan) +{ + auto per = make_default_periodic(); + per.full_charge_capacity = NAN; + per.design_capacity = 5.0f; // 5 Ah -> 5000 mAh + + ASSERT_GE(broadcast(per), 0); + ASSERT_GE(spin(kSpinMs), 0); + ASSERT_GE(broadcast(make_default_continuous()), 0); + ASSERT_GE(spin(kSpinMs), 0); + + battery_status_s data{}; + ASSERT_TRUE(find_battery_status_for_node(kPublisherNodeId, data)); + EXPECT_EQ(data.capacity, 5000); +} + +TEST_F(BatteryBridgeTest, PeriodicSkipsInvalidCycleCountAndStateOfHealth) +{ + auto good = make_default_periodic(); + good.cycle_count = 100; + good.state_of_health = 50; + ASSERT_GE(broadcast(good), 0); + ASSERT_GE(spin(kSpinMs), 0); + ASSERT_GE(broadcast(make_default_continuous()), 0); + ASSERT_GE(spin(kSpinMs), 0); + + auto sentinel = make_default_periodic(); + sentinel.cycle_count = UINT16_MAX; + sentinel.state_of_health = UINT8_MAX; + ASSERT_GE(broadcast(sentinel), 0); + ASSERT_GE(spin(kSpinMs), 0); + ASSERT_GE(broadcast(make_default_continuous()), 0); + ASSERT_GE(spin(kSpinMs), 0); + + battery_status_s data{}; + ASSERT_TRUE(find_battery_status_for_node(kPublisherNodeId, data)); + EXPECT_EQ(data.cycle_count, 100); + EXPECT_EQ(data.state_of_health, 50); +} + +TEST_F(BatteryBridgeTest, ContinuousChargingFlagSetsChargingWarning) +{ + auto cont = make_default_continuous(); + cont.status_flags = ardupilot::equipment::power::BatteryContinuous::STATUS_FLAG_CHARGING; + + ASSERT_GE(broadcast(cont), 0); + ASSERT_GE(spin(kSpinMs), 0); + + battery_status_s data{}; + ASSERT_TRUE(find_battery_status_for_node(kPublisherNodeId, data)); + EXPECT_EQ(data.warning, battery_status_s::STATE_CHARGING); + EXPECT_EQ(data.faults, 0u); +} + +TEST_F(BatteryBridgeTest, ContinuousFaultFlagsMapToFaultBits) +{ + using BC = ardupilot::equipment::power::BatteryContinuous; + auto cont = make_default_continuous(); + cont.status_flags = BC::STATUS_FLAG_FAULT_OVER_CURRENT | BC::STATUS_FLAG_FAULT_OVER_TEMP; + + ASSERT_GE(broadcast(cont), 0); + ASSERT_GE(spin(kSpinMs), 0); + + battery_status_s data{}; + ASSERT_TRUE(find_battery_status_for_node(kPublisherNodeId, data)); + EXPECT_NE(data.faults & (1u << battery_status_s::FAULT_OVER_CURRENT), 0u); + EXPECT_NE(data.faults & (1u << battery_status_s::FAULT_OVER_TEMPERATURE), 0u); + EXPECT_EQ(data.warning, battery_status_s::STATE_UNHEALTHY); +} + +} // namespace diff --git a/src/drivers/uavcan/sensors/test/CMakeLists.txt b/src/drivers/uavcan/sensors/test/CMakeLists.txt new file mode 100644 index 000000000000..9f055210b866 --- /dev/null +++ b/src/drivers/uavcan/sensors/test/CMakeLists.txt @@ -0,0 +1,154 @@ +############################################################################ +# +# Copyright (c) 2026 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. +# +############################################################################ +# +# Standalone test build for UAVCAN sensor bridges. +# +# The production uavcan driver only builds when CONFIG_DRIVERS_UAVCAN is set, +# which is gated on PLATFORM_NUTTX. SITL test target (px4_sitl_test) is Linux, +# so the driver dir is otherwise skipped. This file re-creates the minimum +# pieces (libuavcan core + DSDL gen + bridge sources) needed to compile and +# link host-side gtest binaries that exercise the UAVCAN -> uORB bridge logic +# through libuavcan's in-tree loopback transport. +# + +if(NOT BUILD_TESTING) + return() +endif() + +set(UAVCAN_DRIVER_DIR ${PX4_SOURCE_DIR}/src/drivers/uavcan) +set(LIBDRONECAN_DIR ${UAVCAN_DRIVER_DIR}/libdronecan) +set(DSDLC_DIR ${LIBDRONECAN_DIR}/dsdl) + +px4_add_git_submodule(TARGET git_uavcan_test_dsdl PATH ${DSDLC_DIR}) +px4_add_git_submodule(TARGET git_uavcan_test_pydronecan PATH ${LIBDRONECAN_DIR}/libuavcan/dsdl_compiler/pydronecan) + +set(DSDLC_INPUTS + "${DSDLC_DIR}/ardupilot" + "${DSDLC_DIR}/com" + "${DSDLC_DIR}/cuav" + "${DSDLC_DIR}/dronecan" + "${DSDLC_DIR}/uavcan" +) +set(DSDLC_OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/include/dsdlc_generated) +set(DSDLC_STAMP ${CMAKE_CURRENT_BINARY_DIR}/uavcan_test_dsdlc.stamp) +set(DSDLC_INPUT_FILES) +foreach(_in ${DSDLC_INPUTS}) + file(GLOB_RECURSE _files CONFIGURE_DEPENDS "${_in}/*.uavcan") + list(APPEND DSDLC_INPUT_FILES ${_files}) +endforeach() + +add_custom_command(OUTPUT ${DSDLC_STAMP} + COMMAND ${PYTHON_EXECUTABLE} ${LIBDRONECAN_DIR}/libuavcan/dsdl_compiler/libuavcan_dsdlc + --outdir ${DSDLC_OUTPUT} ${DSDLC_INPUTS} + COMMAND ${CMAKE_COMMAND} -E touch ${DSDLC_STAMP} + DEPENDS ${DSDLC_INPUT_FILES} + COMMENT "UAVCAN bridge test: DSDL compiler" +) +add_custom_target(uavcan_test_dsdlc DEPENDS ${DSDLC_STAMP}) +add_dependencies(uavcan_test_dsdlc git_uavcan_test_dsdl git_uavcan_test_pydronecan) + +# libuavcan core (test-only static lib). +file(GLOB_RECURSE LIBUAVCAN_SRCS CONFIGURE_DEPENDS "${LIBDRONECAN_DIR}/libuavcan/src/*.cpp") +add_library(uavcan_test_core STATIC ${LIBUAVCAN_SRCS}) +add_dependencies(uavcan_test_core uavcan_test_dsdlc) +target_include_directories(uavcan_test_core PUBLIC + ${LIBDRONECAN_DIR}/libuavcan/include + ${DSDLC_OUTPUT} +) +target_compile_definitions(uavcan_test_core PUBLIC + UAVCAN_CPP_VERSION=UAVCAN_CPP03 + UAVCAN_MEM_POOL_BLOCK_SIZE=64 + UAVCAN_PLATFORM=generic + UAVCAN_NUM_IFACES=1 + UAVCAN_NO_ASSERTIONS + UAVCAN_IMPLEMENT_PLACEMENT_NEW=0 + UAVCAN_TOSTRING=1 +) +target_compile_options(uavcan_test_core PRIVATE + -Wno-cast-align + -Wno-deprecated-copy + -Wno-address-of-packed-member + -Wno-zero-as-null-pointer-constant +) + +# Warning-suppression flags repeat on the gtest binary because libuavcan +# headers (included transitively from ) trip +# -Wcast-align / -Wzero-as-null-pointer-constant at the point of inclusion, +# not only inside libuavcan's own .cpp files. +function(add_uavcan_bridge_gtest) + set(options) + set(oneValueArgs SRC) + set(multiValueArgs EXTRA_SRCS) + cmake_parse_arguments(ABG "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN}) + + px4_add_functional_gtest( + SRC ${ABG_SRC} + EXTRA_SRCS + ${UAVCAN_DRIVER_DIR}/sensors/sensor_bridge.cpp + ${UAVCAN_DRIVER_DIR}/sensors/battery.cpp + # node_info is pulled in because UavcanBatteryBridge's serial-number + # path consults the NodeInfoPublisher abstraction at link time. + ${UAVCAN_DRIVER_DIR}/node_info.cpp + ${ABG_EXTRA_SRCS} + INCLUDES + ${UAVCAN_DRIVER_DIR} + ${CMAKE_CURRENT_SOURCE_DIR} + ${PX4_SOURCE_DIR}/src + ${PX4_SOURCE_DIR}/src/include + ${PX4_SOURCE_DIR}/src/lib + ${PX4_SOURCE_DIR}/src/lib/matrix + ${PX4_SOURCE_DIR}/platforms/common + ${PX4_SOURCE_DIR}/platforms/common/include + ${PX4_SOURCE_DIR}/platforms/posix/include + ${PX4_SOURCE_DIR}/platforms/posix/src/px4/generic/generic/include + ${PX4_BINARY_DIR} + ${PX4_BINARY_DIR}/uORB + ${PX4_BINARY_DIR}/src/lib + LINKLIBS + uavcan_test_core + battery + drivers__device + COMPILE_FLAGS + -D__PX4_POSIX + -D__PX4_LINUX + -include visibility.h + -DCONFIG_UAVCAN_SENSOR_BATTERY=1 + -Wno-cast-align + -Wno-deprecated-copy + -Wno-address-of-packed-member + -Wno-zero-as-null-pointer-constant + ) +endfunction() + +add_uavcan_bridge_gtest(SRC BatteryBridgeTest.cpp) +add_uavcan_bridge_gtest(SRC BatteryBridgeMultiInstanceTest.cpp) diff --git a/src/drivers/uavcan/sensors/test/UavcanBridgeTestFixture.hpp b/src/drivers/uavcan/sensors/test/UavcanBridgeTestFixture.hpp new file mode 100644 index 000000000000..d74a4d3352c2 --- /dev/null +++ b/src/drivers/uavcan/sensors/test/UavcanBridgeTestFixture.hpp @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * Copyright (c) 2026 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. + * + ****************************************************************************/ + +/** + * @file UavcanBridgeTestFixture.hpp + * + * N libuavcan nodes on a loopback bus (PairableCanDriver): index 0 hosts the + * bridge under test, indices >=1 publish synthetic messages. Template stays + * because TestNetwork is compile-time parameterised. + */ + +#pragma once + +#include +#include + +#include +#include + +#include + +namespace uavcan_bridge_test +{ + +// Subscriber=1, publishers contiguous from 2. +static constexpr uint8_t kSubscriberNodeId = 1; +static constexpr uint8_t kFirstPublisherNodeId = 2; + +} // namespace uavcan_bridge_test + +template +class UavcanBridgeTestFixtureT : public ::testing::Test +{ +protected: + static_assert(NumNodes >= 2, "Need at least one subscriber + one publisher"); + + void SetUp() override + { + _network = std::make_unique>(uavcan_bridge_test::kSubscriberNodeId); + } + + void TearDown() override + { + _network.reset(); + } + + TestNode &subscriber_node() { return (*_network)[0]; } + TestNode &publisher_node(unsigned idx = 0) { return (*_network)[1 + idx]; } + + int spin(unsigned ms) + { + return _network->spinAll(uavcan::MonotonicDuration::fromMSec(ms)); + } + + template + int broadcast(const Msg &msg, unsigned publisher_idx = 0) + { + uavcan::Publisher pub(publisher_node(publisher_idx)); + const int init_res = pub.init(); + + if (init_res < 0) { + return init_res; + } + + return pub.broadcast(msg); + } + + std::unique_ptr> _network; +}; + +using UavcanBridgeTestFixture = UavcanBridgeTestFixtureT<>; diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index 766c147e61c1..61ba91220627 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -370,6 +370,34 @@ void Battery::computeScale() } } +// Returns the voltage spread across cells: max(cell_v) - min(cell_v). +// Only cells with a positive voltage are considered; cells reporting 0 V are +// treated as absent. +// Returns 0 if fewer than two valid cells are present. +float Battery::computeMaxCellVoltageDelta(const float *cells, size_t n) +{ + if (cells == nullptr) { return 0.0; } + + float v_min = FLT_MAX; + float v_max = 0.f; + + for (size_t i = 0; i < n; i++) { + const float v = cells[i]; + + if (v > 0.f) { + if (v < v_min) { + v_min = v; + } + + if (v > v_max) { + v_max = v; + } + } + } + + return (v_max > 0.f && v_min <= v_max) ? (v_max - v_min) : 0.f; +} + float Battery::computeRemainingTime(float current_a) { float time_remaining_s = NAN; diff --git a/src/lib/battery/battery.h b/src/lib/battery/battery.h index bcc9baa6d9e1..8fbb240445e8 100644 --- a/src/lib/battery/battery.h +++ b/src/lib/battery/battery.h @@ -42,6 +42,7 @@ #pragma once +#include #include #include @@ -133,6 +134,12 @@ class Battery : public ModuleParams uint8_t determineWarning(float state_of_charge); void updateDt(const hrt_abstime ×tamp); + /** + * Compute max-min delta over a cell-voltage array. Cells with value <= 0 are ignored. + * @return Delta in volts, or 0 if no valid cells. + */ + static float computeMaxCellVoltageDelta(const float *cells, size_t n); + protected: static constexpr float LITHIUM_BATTERY_RECOGNITION_VOLTAGE = 2.1f;