fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

uavcan: cleanup & node_status fix

Browse Source 
uavcan: esc fault handling review suggestions

uavcan: fix node status & cleanup
This commit is contained in:
ttechnick 2026-02-09 10:25:37 +01:00 committed by Nick
parent 06942bbfcc
commit 6a18fd045f
7 changed files with 43 additions and 128 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
msg/DeviceInformation.msg
View File

@ -6,7 +6,6 @@
uint64 timestamp # time since system start (microseconds)
uint8 device_type # [@enum DEVICE_TYPE] Type of the device. Matches MAVLink DEVICE_TYPE enum
uint8 DEVICE_TYPE_GENERIC = 0 # Generic/unknown sensor
uint8 DEVICE_TYPE_AIRSPEED = 1 # Airspeed sensor
uint8 DEVICE_TYPE_ESC = 2 # ESC
@ -25,8 +24,6 @@ uint8 DEVICE_TYPE_BATTERY = 14 # Battery
uint8 DEVICE_TYPE_HYGROMETER = 15 # Hygrometer
char[80] name # Name of node
char[32] vendor_name # Name of the device vendor
char[32] model_name # Name of the device model
uint32 device_id # [-] [@invalid 0 if not available] Unique device ID for the sensor. Does not change between power cycles.
char[24] firmware_version # [-] [@invalid empty if not available] Firmware version.

72
src/drivers/uavcan/actuators/esc.cpp
View File

@ -65,7 +65,7 @@ UavcanEscController::init()
return res;
}
//ESC Status Extended subscription
// ESC Status Extended subscription
res = _uavcan_sub_status_extended.start(StatusExtendedCbBinder(this, &UavcanEscController::esc_status_extended_sub_cb));
if (res < 0) {
@ -117,16 +117,16 @@ void
UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg)
{
if (msg.esc_index < esc_status_s::CONNECTED_ESC_MAX) {
auto &esc_report = _esc_status.esc[msg.esc_index];
esc_report.timestamp = hrt_absolute_time();
esc_report_s &esc_report = _esc_status.esc[msg.esc_index];
esc_report.timestamp = hrt_absolute_time();
esc_report.esc_address = msg.getSrcNodeID().get();
esc_report.esc_voltage = msg.voltage;
esc_report.esc_current = msg.current;
esc_report.esc_voltage = msg.voltage;
esc_report.esc_current = msg.current;
esc_report.esc_temperature = msg.temperature + atmosphere::kAbsoluteNullCelsius; // Kelvin to Celsius
// esc_report.motor_temperature is filled in the extended status callback
esc_report.esc_rpm = msg.rpm;
esc_report.esc_errorcount = msg.error_count;
esc_report.esc_rpm = msg.rpm;
esc_report.esc_errorcount = msg.error_count;
esc_report.failures = get_failures(msg.esc_index);
_esc_status.esc_count = _rotor_count;
_esc_status.counter += 1;
@ -134,7 +134,6 @@ UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavca
_esc_status.esc_online_flags = check_escs_status();
_esc_status.esc_armed_flags = (1 << _rotor_count) - 1;
_esc_status.timestamp = hrt_absolute_time();
esc_report.failures = get_failures(msg.esc_index);
_esc_status_pub.publish(_esc_status);
}
@ -147,15 +146,13 @@ UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavca
}
}
void
UavcanEscController::esc_status_extended_sub_cb(const
uavcan::ReceivedDataStructure<uavcan::equipment::esc::StatusExtended> &msg)
void UavcanEscController::esc_status_extended_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::StatusExtended> &msg)
{
uint8_t index = msg.esc_index;
if (index < esc_status_s::CONNECTED_ESC_MAX) {
auto &esc_report = _esc_status.esc[index];
// This will be published along with the regular ESC status
esc_report_s &esc_report = _esc_status.esc[index];
// published with the non-extended esc::Status
esc_report.motor_temperature = msg.motor_temperature_degC;
esc_report.esc_power = msg.input_pct;
}
@ -178,20 +175,20 @@ UavcanEscController::check_escs_status()
return esc_status_flags;
}
uint32_t
UavcanEscController::get_failures(uint8_t esc_index)
uint32_t UavcanEscController::get_failures(uint8_t esc_index)
{
auto &esc_report = _esc_status.esc[esc_index];
esc_report_s &esc_report = _esc_status.esc[esc_index];
uint32_t failures = 0;
// Update device vendor/model information from device_information topic
device_information_s device_info;
device_information_s device_information;
char esc_name[80] {};
if (_device_information_sub.copy(&device_info)
&& device_info.device_type == device_information_s::DEVICE_TYPE_ESC
&& device_info.device_id == esc_index) {
strncpy(esc_name, device_info.name, sizeof(esc_name) - 1);
if (_device_information_sub.copy(&device_information)
&& device_information.device_type == device_information_s::DEVICE_TYPE_ESC
&& device_information.device_id == esc_index) {
strncpy(esc_name, device_information.name, sizeof(esc_name) - 1);
esc_name[sizeof(esc_name) - 1] = '\0';
}
@ -199,23 +196,20 @@ UavcanEscController::get_failures(uint8_t esc_index)
dronecan_node_status_s node_status {};
uint8_t node_health{dronecan_node_status_s::HEALTH_OK};
uint16_t vendor_specific_status_code{0};
bool have_node_status{false};
if (_dronecan_node_status_sub.copy(&node_status)
&& esc_report.esc_address < kMaxUavcanNodeId
&& node_status.node_id == esc_report.esc_address) {
node_health = node_status.health;
vendor_specific_status_code = node_status.vendor_specific_status_code;
have_node_status = true;
for (auto &dronecan_node_status_sub : _dronecan_node_status_subs) {
if (dronecan_node_status_sub.copy(&node_status)) {
if (node_status.node_id == esc_report.esc_address) {
node_health = node_status.health;
vendor_specific_status_code = node_status.vendor_specific_status_code;
break;
}
}
}
if (!have_node_status) {
return esc_report.failures;
}
if (esc_report.esc_address < kMaxUavcanNodeId && (node_health == dronecan_node_status_s::HEALTH_OK ||
if (esc_report.esc_address < UAVCAN_NODE_ID_MAX && (node_health == dronecan_node_status_s::HEALTH_OK ||
node_health == dronecan_node_status_s::HEALTH_WARNING)) {
esc_report.failures = 0;
failures = 0;
} else if (strstr(esc_name, "iq_motion") != nullptr) {
// Parse iq_motion ESC errors
@ -239,14 +233,14 @@ UavcanEscController::get_failures(uint8_t esc_index)
for (const auto &mapping : bit_to_failure_map) {
if (vendor_specific_status_code & (1 << mapping.bit)) {
esc_report.failures |= (1 << mapping.failure_type);
failures |= (1 << mapping.failure_type);
}
}
} else {
// Generic parsing
esc_report.failures |= (1 << esc_report_s::FAILURE_GENERIC);
failures |= (1 << esc_report_s::FAILURE_GENERIC);
}
return esc_report.failures;
return failures;
}

5
src/drivers/uavcan/actuators/esc.hpp
View File

@ -52,6 +52,7 @@
#include <lib/perf/perf_counter.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/esc_status.h>
#include <uORB/topics/esc_report.h>
@ -66,7 +67,7 @@ class UavcanEscController
public:
static constexpr int MAX_ACTUATORS = esc_status_s::CONNECTED_ESC_MAX;
static constexpr unsigned MAX_RATE_HZ = 400;
static constexpr int16_t kMaxUavcanNodeId = 128; // UAVCAN supports up to 128 nodes (0-127)
static constexpr int16_t UAVCAN_NODE_ID_MAX = 128; // UAVCAN supports up to 128 nodes (0-127)
static_assert(uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize >= MAX_ACTUATORS, "Too many actuators");
@ -126,7 +127,7 @@ private:
esc_status_s _esc_status{};
uORB::PublicationMulti<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};
uORB::Subscription _dronecan_node_status_sub{ORB_ID(dronecan_node_status)};
uORB::SubscriptionMultiArray<dronecan_node_status_s, ORB_MULTI_MAX_INSTANCES> _dronecan_node_status_subs{ORB_ID::dronecan_node_status};
uORB::Subscription _device_information_sub{ORB_ID(device_information)};
uint8_t _rotor_count{0};

53
src/drivers/uavcan/node_info.cpp
View File

@ -201,18 +201,10 @@ void NodeInfoPublisher::publishSingleDeviceInformation(const DeviceInformation &
msg.device_id = device_info.device_id;
// Copy pre-populated fields directly from the struct
static_assert(sizeof(msg.model_name) == sizeof(device_info.model_name), "Array size mismatch");
static_assert(sizeof(msg.vendor_name) == sizeof(device_info.vendor_name), "Array size mismatch");
static_assert(sizeof(msg.name) == sizeof(device_info.name), "Array size mismatch");
static_assert(sizeof(msg.firmware_version) == sizeof(device_info.firmware_version), "Array size mismatch");
static_assert(sizeof(msg.hardware_version) == sizeof(device_info.hardware_version), "Array size mismatch");
static_assert(sizeof(msg.serial_number) == sizeof(device_info.serial_number), "Array size mismatch");
static_assert(sizeof(msg.name) == sizeof(device_info.name), "Array size mismatch");
memcpy(msg.model_name, device_info.model_name, sizeof(msg.model_name));
msg.model_name[sizeof(msg.model_name) - 1] = '\0';
memcpy(msg.vendor_name, device_info.vendor_name, sizeof(msg.vendor_name));
msg.vendor_name[sizeof(msg.vendor_name) - 1] = '\0';
memcpy(msg.name, device_info.name, sizeof(msg.name));
msg.name[sizeof(msg.name) - 1] = '\0';
@ -237,9 +229,6 @@ void NodeInfoPublisher::populateDeviceInfoFields(DeviceInformation &device_info,
{
device_info.has_node_info = true;
// Remain backward compatible - for now.
parseNodeName(info.name, device_info);
snprintf(device_info.name, sizeof(device_info.name), "%s", info.name);
snprintf(device_info.firmware_version, sizeof(device_info.firmware_version),
"%d.%d.%lu", info.sw_major, info.sw_minor, static_cast<unsigned long>(info.vcs_commit));
@ -253,46 +242,6 @@ void NodeInfoPublisher::populateDeviceInfoFields(DeviceInformation &device_info,
info.unique_id[12], info.unique_id[13], info.unique_id[14], info.unique_id[15]);
}
void NodeInfoPublisher::parseNodeName(const char *name, DeviceInformation &device_info)
{
if (!name || strlen(name) == 0) {
strlcpy(device_info.vendor_name, "", sizeof(device_info.vendor_name));
strlcpy(device_info.model_name, "", sizeof(device_info.model_name));
return;
}
// Find first dot and skip everything before it
const char *after_first_dot = strchr(name, '.');
if (after_first_dot == nullptr) {
// No dot - whole string is model, vendor is -1
strlcpy(device_info.vendor_name, "", sizeof(device_info.vendor_name));
strlcpy(device_info.model_name, name, sizeof(device_info.model_name));
return;
}
after_first_dot++;
// Find next dot in remaining string
const char *second_dot = strchr(after_first_dot, '.');
if (second_dot == nullptr) {
// Only one dot - everything after first dot is model, vendor is -1
strlcpy(device_info.vendor_name, "", sizeof(device_info.vendor_name));
strlcpy(device_info.model_name, after_first_dot, sizeof(device_info.model_name));
return;
}
// Copy vendor (between first and second dot)
size_t vendor_len = second_dot - after_first_dot;
size_t copy_len = (vendor_len < sizeof(device_info.vendor_name) - 1) ? vendor_len : sizeof(device_info.vendor_name) - 1;
strncpy(device_info.vendor_name, after_first_dot, copy_len);
device_info.vendor_name[copy_len] = '\0';
// Copy model (everything after second dot)
strlcpy(device_info.model_name, second_dot + 1, sizeof(device_info.model_name));
}
bool NodeInfoPublisher::extendDeviceInformationsArray()
{
const size_t new_size = _device_informations_size + 1;

34
src/drivers/uavcan/node_info.hpp
View File

@ -102,36 +102,10 @@ private:
DeviceCapability capability{DeviceCapability::NONE};
bool has_node_info{false};
char name[80];
char vendor_name[32];
char model_name[32];
char firmware_version[24];
char hardware_version[24];
char serial_number[33];
DeviceInformation() : node_id(UINT8_MAX), device_id(UINT32_MAX), capability(DeviceCapability::NONE),
has_node_info(false)
{
// Initialize string fields
name[0] = '\0';
vendor_name[0] = '\0';
model_name[0] = '\0';
firmware_version[0] = '\0';
hardware_version[0] = '\0';
serial_number[0] = '\0';
}
DeviceInformation(uint8_t nid, uint32_t did, DeviceCapability cap)
: node_id(nid), device_id(did), capability(cap), has_node_info(false)
{
// Initialize string fields
name[0] = '\0';
vendor_name[0] = '\0';
model_name[0] = '\0';
firmware_version[0] = '\0';
hardware_version[0] = '\0';
serial_number[0] = '\0';
}
char name[80] = "";
char firmware_version[24] = "";
char hardware_version[24] = "";
char serial_number[33] = "";
};
void handleNodeInfoRetrieved(uavcan::NodeID node_id,

2
src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp
View File

@ -132,7 +132,7 @@ void EscChecks::checkEscStatus(const Context &context, Report &reporter, const e
mavlink_log_critical(reporter.mavlink_log_pub(), "%soffline. %s\t", esc_fail_msg, context.isArmed() ? "Land now!" : "");
}
for (int index = 0; index < math::min(esc_status.esc_count, esc_status_s::CONNECTED_ESC_MAX); ++index) {
for (int index = 0; index < esc_status_s::CONNECTED_ESC_MAX; ++index) {
if (esc_status.esc[index].failures != 0) {

2
src/modules/commander/failure_detector/FailureDetector.cpp
View File

@ -184,7 +184,7 @@ void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status, c
bool is_esc_failure = !is_all_escs_armed;
for (int i = 0; i < limited_esc_count; i++) {
is_esc_failure = is_esc_failure || (esc_status.esc[i].failures > 0);
is_esc_failure = is_esc_failure;
}
_esc_failure_hysteresis.set_hysteresis_time_from(false, 300_ms);