Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 03:49:12 +08:00
Code Issues Actions 6 Packages Projects Releases Wiki Activity

uavcan: uavcan_servers only acknowledge supported commands

Browse Source
This commit is contained in:
Daniel Agar
2021-03-11 14:15:29 -05:00
parent c13266ad26
commit eef304110d
1 changed files with 13 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/uavcan/uavcan_servers.cpp
+6 -2
View File
@@ -475,12 +475,14 @@ UavcanServers::run(pthread_addr_t)
// Check for ESC enumeration commands // Check for ESC enumeration commands
if (vcmd_sub.updated() && !_cmd_in_progress) { if (vcmd_sub.updated() && !_cmd_in_progress) {
bool acknowledge = false;
vehicle_command_s cmd{}; vehicle_command_s cmd{};
vcmd_sub.copy(&cmd); vcmd_sub.copy(&cmd);
uint8_t cmd_ack_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED; uint8_t cmd_ack_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN) { if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN) {
acknowledge = true;
int command_id = static_cast<int>(cmd.param1 + 0.5f); int command_id = static_cast<int>(cmd.param1 + 0.5f);
int node_id = static_cast<int>(cmd.param2 + 0.5f); int node_id = static_cast<int>(cmd.param2 + 0.5f);
int call_res; int call_res;
@@ -520,6 +522,7 @@ UavcanServers::run(pthread_addr_t)
} }
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE) { } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE) {
acknowledge = true;
int command_id = static_cast<int>(cmd.param1 + 0.5f); int command_id = static_cast<int>(cmd.param1 + 0.5f);
PX4_INFO("UAVCAN command bridge: received storage command ID %d", command_id); PX4_INFO("UAVCAN command bridge: received storage command ID %d", command_id);
@@ -552,16 +555,17 @@ UavcanServers::run(pthread_addr_t)
} }
} }
if (acknowledge) {
// Acknowledge the received command // Acknowledge the received command
vehicle_command_ack_s ack{}; vehicle_command_ack_s ack{};
ack.timestamp = hrt_absolute_time();
ack.command = cmd.command; ack.command = cmd.command;
ack.result = cmd_ack_result; ack.result = cmd_ack_result;
ack.target_system = cmd.source_system; ack.target_system = cmd.source_system;
ack.target_component = cmd.source_component; ack.target_component = cmd.source_component;
ack.timestamp = hrt_absolute_time();
_command_ack_pub.publish(ack); _command_ack_pub.publish(ack);
} }
}
// Shut down once armed // Shut down once armed
// TODO (elsewhere): start up again once disarmed? // TODO (elsewhere): start up again once disarmed?