Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-10 06:39:25 +08:00
Code Issues Actions 3 Packages Projects Releases Wiki Activity

add vendor specific error handling

Browse Source 
Vendor specific failure handling: IQ_Motion
This commit is contained in:
ttechnick
2026-01-26 09:37:24 +01:00
committed by Nick
parent 89c4980e55
commit 06942bbfcc
5 changed files with 113 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/EscReport.msg
+1
View File
@@ -4,6 +4,7 @@ int32 esc_rpm # Motor RPM, negative for reverse rotation [RPM] - if support
float32 esc_voltage # Voltage measured from current ESC [V] - if supported
float32 esc_current # Current measured from current ESC [A] - if supported
float32 esc_temperature # Temperature measured from current ESC [degC] - if supported
int16 motor_temperature # Temperature measured from current motor [degC] - if supported
uint8 esc_address # Address of current ESC (in most cases 1-8 / must be set by driver)
uint8 esc_cmdcount # Counter of number of commands
src/drivers/uavcan/actuators/esc.cpp
+98 -8
View File
@@ -117,15 +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 &ref = _esc_status.esc[msg.esc_index];
auto &esc_report = _esc_status.esc[msg.esc_index];
ref.timestamp = hrt_absolute_time();
ref.esc_address = msg.getSrcNodeID().get();
ref.esc_voltage = msg.voltage;
ref.esc_current = msg.current;
ref.esc_temperature = msg.temperature + atmosphere::kAbsoluteNullCelsius; // Kelvin to Celsius
ref.esc_rpm = msg.rpm;
ref.esc_errorcount = msg.error_count;
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_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_status.esc_count = _rotor_count;
_esc_status.counter += 1;
@@ -133,6 +134,8 @@ 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);
}
@@ -144,6 +147,20 @@ UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavca
}
}
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.motor_temperature = msg.motor_temperature_degC;
esc_report.esc_power = msg.input_pct;
}
}
uint8_t
UavcanEscController::check_escs_status()
{
@@ -160,3 +177,76 @@ UavcanEscController::check_escs_status()
return esc_status_flags;
}
uint32_t
UavcanEscController::get_failures(uint8_t esc_index)
{
auto &esc_report = _esc_status.esc[esc_index];
// Update device vendor/model information from device_information topic
device_information_s device_info;
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);
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};
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;
}
if (!have_node_status) {
return esc_report.failures;
}
if (esc_report.esc_address < kMaxUavcanNodeId && (node_health == dronecan_node_status_s::HEALTH_OK ||
node_health == dronecan_node_status_s::HEALTH_WARNING)) {
esc_report.failures = 0;
} else if (strstr(esc_name, "iq_motion") != nullptr) {
// Parse iq_motion ESC errors
static const struct {
uint8_t bit;
uint8_t failure_type;
} bit_to_failure_map[] = {
{0, esc_report_s::FAILURE_OVER_VOLTAGE},
{1, esc_report_s::FAILURE_OVER_VOLTAGE},
{2, esc_report_s::FAILURE_OVER_VOLTAGE},
{3, esc_report_s::FAILURE_OVER_CURRENT},
{4, esc_report_s::FAILURE_OVER_CURRENT},
{5, esc_report_s::FAILURE_OVER_ESC_TEMPERATURE},
{6, esc_report_s::FAILURE_MOTOR_OVER_TEMPERATURE},
{7, esc_report_s::FAILURE_GENERIC},
{8, esc_report_s::FAILURE_OVER_RPM},
{9, esc_report_s::FAILURE_WARN_ESC_TEMPERATURE},
{10, esc_report_s::FAILURE_MOTOR_WARN_TEMPERATURE},
{11, esc_report_s::FAILURE_OVER_VOLTAGE},
};
for (const auto &mapping : bit_to_failure_map) {
if (vendor_specific_status_code & (1 << mapping.bit)) {
esc_report.failures |= (1 << mapping.failure_type);
}
}
} else {
// Generic parsing
esc_report.failures |= (1 << esc_report_s::FAILURE_GENERIC);
}
return esc_report.failures;
}
src/drivers/uavcan/actuators/esc.hpp
+13 -3
View File
@@ -38,6 +38,7 @@
* uavcan.equipment.esc.RawCommand
* uavcan.equipment.esc.RPMCommand
* uavcan.equipment.esc.Status
* uavcan.equipment.esc.StatusExtended
*
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
@@ -58,12 +59,14 @@
#include <uORB/topics/device_information.h>
#include <drivers/drv_hrt.h>
#include <drivers/uavcan/node_info.hpp>
#include "../node_info.hpp"
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_assert(uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize >= MAX_ACTUATORS, "Too many actuators");
@@ -75,8 +78,7 @@ public:
bool initialized() { return _initialized; };
void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], uint8_t output_array_size);
void update_outputs(uint16_t outputs[MAX_ACTUATORS], uint8_t output_array_size);
/**
* Sets the number of rotors and enable timer
*/
@@ -103,6 +105,12 @@ private:
*/
uint8_t check_escs_status();
/**
* Gets failure flags for a specific ESC
*/
uint32_t get_failures(uint8_t esc_index);
typedef uavcan::MethodBinder<UavcanEscController *,
void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)> StatusCbBinder;
@@ -113,11 +121,13 @@ private:
typedef uavcan::MethodBinder<UavcanEscController *,
void (UavcanEscController::*)(const uavcan::TimerEvent &)> TimerCbBinder;
bool _initialized = false;
bool _initialized{};
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::Subscription _device_information_sub{ORB_ID(device_information)};
uint8_t _rotor_count{0};
src/drivers/uavcan/node_info.cpp
-1
View File
@@ -206,7 +206,6 @@ void NodeInfoPublisher::publishSingleDeviceInformation(const DeviceInformation &
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");
// Copy strings using memcpy and ensure null termination
static_assert(sizeof(msg.name) == sizeof(device_info.name), "Array size mismatch");
memcpy(msg.model_name, device_info.model_name, sizeof(msg.model_name));
src/drivers/uavcan/node_info.hpp
+1 -1
View File
@@ -148,10 +148,10 @@ private:
// Publishing methods
void publishDeviceInformationPeriodic();
void publishSingleDeviceInformation(const DeviceInformation &device_info);
void parseNodeName(const char *name, DeviceInformation &device_info);
// Helper functions
void populateDeviceInfoFields(DeviceInformation &device_info, const NodeInfo &info);
void parseNodeName(const char *name, DeviceInformation &device_info);
bool extendDeviceInformationsArray();
uavcan::NodeInfoRetriever &_node_info_retriever;