Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-19 02:23:58 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

uavcan esc handling review suggestions

Browse Source
This commit is contained in:
Matthias Grob
2026-02-19 19:58:49 +01:00
committed by Nick
parent 790c2d3369
commit ca0dec5a33
5 changed files with 35 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/DeviceInformation.msg
+1 -1
View File
@@ -23,7 +23,7 @@ uint8 DEVICE_TYPE_DIFFERENTIAL_PRESSURE = 13 # Differential pressure
uint8 DEVICE_TYPE_BATTERY = 14 # Battery
uint8 DEVICE_TYPE_HYGROMETER = 15 # Hygrometer
char[80] name # Name of node
char[80] name # Name of device e.g. DroneCAN node name
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.
src/drivers/uavcan/actuators/esc.cpp
+25 -31
View File
@@ -129,8 +129,7 @@ void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<
_esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN;
_esc_status.esc_online_flags = check_escs_status();
_esc_status.esc_armed_flags = (1 << _rotor_count) - 1;
_esc_status.timestamp = hrt_absolute_time();
_esc_status.timestamp = esc_report.timestamp;
_esc_status_pub.publish(_esc_status);
}
@@ -172,29 +171,15 @@ uint8_t UavcanEscController::check_escs_status()
uint32_t UavcanEscController::get_failures(uint8_t 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_information{};
char esc_name[80] {};
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';
}
// Update node health from all available dronecan_node_status messages
dronecan_node_status_s node_status {};
uint8_t node_health{dronecan_node_status_s::HEALTH_OK};
uint16_t vendor_specific_status_code{0};
// Check DoneCAN node health of the ESC
dronecan_node_status_s node_status{};
uint8_t esc_node_id = _esc_status.esc[esc_index].esc_address;
uint8_t node_health = dronecan_node_status_s::HEALTH_OK;
uint16_t vendor_specific_status_code = 0;
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) {
if (node_status.node_id == esc_node_id) {
node_health = node_status.health;
vendor_specific_status_code = node_status.vendor_specific_status_code;
break;
@@ -202,12 +187,20 @@ uint32_t UavcanEscController::get_failures(uint8_t esc_index)
}
}
if (esc_report.esc_address <= uavcan::NodeID::Max && (node_health == dronecan_node_status_s::HEALTH_OK ||
node_health == dronecan_node_status_s::HEALTH_WARNING)) {
failures = 0;
const bool failure = (node_health == dronecan_node_status_s::HEALTH_ERROR)
|| (node_health == dronecan_node_status_s::HEALTH_CRITICAL);
} else if (strstr(esc_name, "iq_motion") != nullptr) {
// Parse iq_motion ESC errors
if (!failure) {
return 0;
}
// Parse VertiQ = iq_motion ESC error flags
device_information_s device_information{};
if (_device_information_sub.copy(&device_information)
&& device_information.device_type == device_information_s::DEVICE_TYPE_ESC
&& device_information.device_id == esc_index
&& strstr(device_information.name, "iq_motion") != nullptr) {
static const struct {
uint8_t bit;
uint8_t failure_type;
@@ -226,16 +219,17 @@ uint32_t UavcanEscController::get_failures(uint8_t esc_index)
{11, esc_report_s::FAILURE_OVER_VOLTAGE},
};
uint32_t failures = 0;
for (const auto &mapping : bit_to_failure_map) {
if (vendor_specific_status_code & (1 << mapping.bit)) {
failures |= (1 << mapping.failure_type);
}
}
} else {
// Generic parsing
failures |= (1 << esc_report_s::FAILURE_GENERIC);
return failures;
}
return failures;
// Generic parsing
return (1 << esc_report_s::FAILURE_GENERIC);
}
src/drivers/uavcan/actuators/esc.hpp
+4 -5
View File
@@ -74,6 +74,7 @@ public:
bool initialized() { return _initialized; };
void update_outputs(uint16_t outputs[MAX_ACTUATORS], uint8_t output_array_size);
/**
* Sets the number of rotors and enable timer
*/
@@ -100,7 +101,6 @@ private:
*/
uint8_t check_escs_status();
/**
* Gets failure flags for a specific ESC
*/
@@ -110,13 +110,12 @@ private:
void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)> StatusCbBinder;
typedef uavcan::MethodBinder<UavcanEscController *,
void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::StatusExtended>&)>
StatusExtendedCbBinder;
void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::StatusExtended>&)> StatusExtendedCbBinder;
typedef uavcan::MethodBinder<UavcanEscController *,
void (UavcanEscController::*)(const uavcan::TimerEvent &)> TimerCbBinder;
bool _initialized{};
bool _initialized = false;
esc_status_s _esc_status{};
@@ -133,7 +132,7 @@ private:
uavcan::INode &_node;
uavcan::Publisher<uavcan::equipment::esc::RawCommand> _uavcan_pub_raw_cmd;
uavcan::Subscriber<uavcan::equipment::esc::Status, StatusCbBinder> _uavcan_sub_status;
uavcan::Subscriber<uavcan::equipment::esc::StatusExtended, StatusExtendedCbBinder> _uavcan_sub_status_extended;
uavcan::Subscriber<uavcan::equipment::esc::StatusExtended, StatusExtendedCbBinder> _uavcan_sub_status_extended;
NodeInfoPublisher *_node_info_publisher{nullptr};
};
src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp
+1 -1
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 < 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) {
src/modules/commander/failure_detector/FailureDetector.cpp
+4
View File
@@ -183,6 +183,10 @@ 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);
}
_esc_failure_hysteresis.set_hysteresis_time_from(false, 300_ms);
_esc_failure_hysteresis.set_state_and_update(is_esc_failure, now);