diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index c1c3a265b5..60071d974e 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -88,6 +88,7 @@ #include #include #include +#include #include #include #include @@ -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_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() {