fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 11:38:56 +08:00
Code Issues Packages Projects Releases Wiki Activity

uavcan: more efficient calculation of esc.RawCommand.cmd array size

Browse Source
This commit is contained in:
Matthias Grob
2025-11-03 08:18:55 -09:00
committed by Jacob Dahl
parent c44e0be18a
commit ae03630570
3 changed files with 19 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/uavcan/actuators/esc.cpp
+3 -26
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
* Copyright (C) 2014-2025 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
@@ -74,20 +74,13 @@ UavcanEscController::init()
_uavcan_pub_raw_cmd.getTransferSender().setIfaceMask(iface_mask);
}
char param_name[17];
for (unsigned i = 0; i < MAX_ACTUATORS; ++i) {
snprintf(param_name, sizeof(param_name), "UAVCAN_EC_FUNC%d", i + 1);
_param_handles[i] = param_find(param_name);
}
_initialized = true;
return res;
}
void
UavcanEscController::update_outputs(uint16_t outputs[MAX_ACTUATORS], unsigned total_outputs)
UavcanEscController::update_outputs(uint16_t outputs[MAX_ACTUATORS], uint8_t output_array_size)
{
// TODO: configurable rate limit
const auto timestamp = _node.getMonotonicTime();
@@ -100,26 +93,10 @@ UavcanEscController::update_outputs(uint16_t outputs[MAX_ACTUATORS], unsigned to
uavcan::equipment::esc::RawCommand msg = {};
// directly output values from mixer
for (unsigned i = 0; i < total_outputs; i++) {
for (unsigned i = 0; i < output_array_size; i++) {
msg.cmd.push_back(static_cast<int>(outputs[i]));
}
// but only output as many channels as are configured
uint8_t min_size = 0;
for (int i = 0; i < MAX_ACTUATORS; i++) {
int32_t val = 0;
if (param_get(_param_handles[i], &val) == 0) {
if (val > 0) {
min_size = i + 1;
}
}
}
msg.cmd.resize(min_size);
_uavcan_pub_raw_cmd.broadcast(msg);
}
src/drivers/uavcan/actuators/esc.hpp
+2 -5
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
* Copyright (C) 2014-2025 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
@@ -71,7 +71,7 @@ public:
bool initialized() { return _initialized; };
void update_outputs(uint16_t outputs[MAX_ACTUATORS], unsigned total_outputs);
void update_outputs(uint16_t outputs[MAX_ACTUATORS], uint8_t output_array_size);
/**
* Sets the number of rotors and enable timer
@@ -114,7 +114,4 @@ private:
uavcan::INode &_node;
uavcan::Publisher<uavcan::equipment::esc::RawCommand> _uavcan_pub_raw_cmd;
uavcan::Subscriber<uavcan::equipment::esc::Status, StatusCbBinder> _uavcan_sub_status;
param_t _param_handles[MAX_ACTUATORS] {PARAM_INVALID};
};
src/drivers/uavcan/uavcan_main.cpp
+14 -2
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2014-2022 PX4 Development Team. All rights reserved.
* Copyright (c) 2014-2025 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
@@ -1082,7 +1082,19 @@ bool UavcanMixingInterfaceESC::updateOutputs(uint16_t outputs[MAX_ACTUATORS], un
unsigned num_control_groups_updated)
{
if (_esc_controller.initialized()) {
_esc_controller.update_outputs(outputs, num_outputs);
// num_outputs is the maximum possible number of outputs (8)
// output_array_size adapts to the highest output index that is mapped (4 for a quad)
// this allows for sending less CAN frames depending on what output indices are mapped
uint8_t output_array_size = 0;
for (int i = MAX_ACTUATORS - 1; i >= 0; i--) {
if (mixingOutput().isFunctionSet(i)) {
output_array_size = i + 1;
break;
}
}
_esc_controller.update_outputs(outputs, output_array_size);
}
return true;