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

PX4IO: Acknowledge binding

Browse Source
This commit is contained in:
Lorenz Meier
2020-12-30 17:12:23 +01:00
parent 8445a08bff
commit 2d1551e5bf
1 changed files with 32 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/px4io/px4io.cpp
+32 -1
View File
@@ -88,6 +88,7 @@
#include <uORB/topics/safety.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/px4io_status.h>
#include <uORB/topics/parameter_update.h>
@@ -414,6 +415,14 @@ private:
*/
int dsm_bind_ioctl(int dsmMode);
/**
* Respond to a vehicle command with an ACK message
*
* @param cmd The command that was executed or denied (inbound)
* @param result The command result
*/
void answer_command(const vehicle_command_s &cmd, uint8_t result);
/**
* check and handle test_motor topic updates
*/
@@ -976,7 +985,15 @@ PX4IO::task_main()
break;
}
(void)dsm_bind_ioctl(bind_arg);
int dsm_ret = dsm_bind_ioctl(bind_arg);
/* publish ACK */
if (dsm_ret == OK) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
} else {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED);
}
}
}
@@ -1295,6 +1312,20 @@ PX4IO::io_set_control_state(unsigned group)
}
}
void
PX4IO::answer_command(const vehicle_command_s &cmd, uint8_t result)
{
/* publish ACK */
uORB::Publication<vehicle_command_ack_s> vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
vehicle_command_ack_s command_ack{};
command_ack.command = cmd.command;
command_ack.result = result;
command_ack.target_system = cmd.source_system;
command_ack.target_component = cmd.source_component;
command_ack.timestamp = hrt_absolute_time();
vehicle_command_ack_pub.publish(command_ack);
}
void
PX4IO::handle_motor_test()
{