From 44628694322dbcfea3b31c65ff08c968a1db1bf5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jos=C3=A9=20Roberto=20de=20Souza?= Date: Fri, 18 Aug 2017 16:52:46 -0700 Subject: [PATCH] Add support to new fields in command_ack --- msg/vehicle_command_ack.msg | 4 ++++ src/drivers/camera_trigger/camera_trigger.cpp | 7 ++++++- src/drivers/vmount/input_mavlink.cpp | 16 +++++++++------ src/drivers/vmount/input_mavlink.h | 6 ++++-- src/modules/commander/commander.cpp | 7 ++++++- src/modules/events/send_event.cpp | 5 +++++ src/modules/logger/logger.cpp | 20 +++++++++++-------- src/modules/logger/logger.h | 4 +++- src/modules/mavlink/mavlink_main.cpp | 4 ++++ src/modules/mavlink/mavlink_receiver.cpp | 20 ++++++++++++++++--- src/modules/uavcan/uavcan_servers.cpp | 7 ++++++- 11 files changed, 77 insertions(+), 23 deletions(-) diff --git a/msg/vehicle_command_ack.msg b/msg/vehicle_command_ack.msg index ff9d6d23ad..a6f3f3fa32 100644 --- a/msg/vehicle_command_ack.msg +++ b/msg/vehicle_command_ack.msg @@ -10,3 +10,7 @@ uint32 ORB_QUEUE_LENGTH = 3 uint16 command uint8 result uint8 from_external +uint8 result_param1 +int32 result_param2 +uint8 target_system +uint8 target_component diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp index f352fdc584..6beb43222a 100644 --- a/src/drivers/camera_trigger/camera_trigger.cpp +++ b/src/drivers/camera_trigger/camera_trigger.cpp @@ -724,8 +724,13 @@ CameraTrigger::cycle_trampoline(void *arg) if (updated && need_ack) { vehicle_command_ack_s command_ack = { .timestamp = 0, + .result_param2 = 0, .command = cmd.command, - .result = (uint8_t)cmd_result + .result = (uint8_t)cmd_result, + .from_external = 0, + .result_param1 = 0, + .target_system = cmd.source_system, + .target_component = cmd.source_component }; if (trig->_cmd_ack_pub == nullptr) { diff --git a/src/drivers/vmount/input_mavlink.cpp b/src/drivers/vmount/input_mavlink.cpp index f5211a0e3a..6704498ab7 100644 --- a/src/drivers/vmount/input_mavlink.cpp +++ b/src/drivers/vmount/input_mavlink.cpp @@ -41,7 +41,6 @@ #include "input_mavlink.h" #include #include -#include #include #include #include @@ -292,7 +291,7 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con break; } - _ack_vehicle_command(vehicle_command.command); + _ack_vehicle_command(&vehicle_command); } else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) { _stabilize[0] = (uint8_t) vehicle_command.param2 == 1; @@ -301,7 +300,7 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con _control_data.type = ControlData::Type::Neutral; //always switch to neutral position *control_data = &_control_data; - _ack_vehicle_command(vehicle_command.command); + _ack_vehicle_command(&vehicle_command); } } @@ -310,12 +309,17 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con return 0; } -void InputMavlinkCmdMount::_ack_vehicle_command(uint16_t command) +void InputMavlinkCmdMount::_ack_vehicle_command(vehicle_command_s *cmd) { vehicle_command_ack_s vehicle_command_ack = { .timestamp = hrt_absolute_time(), - .command = command, - .result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED + .result_param2 = 0, + .command = cmd->command, + .result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, + .from_external = 0, + .result_param1 = 0, + .target_system = cmd->source_system, + .target_component = cmd->source_component }; if (_vehicle_command_ack_pub == nullptr) { diff --git a/src/drivers/vmount/input_mavlink.h b/src/drivers/vmount/input_mavlink.h index d271fad588..0b72b63970 100644 --- a/src/drivers/vmount/input_mavlink.h +++ b/src/drivers/vmount/input_mavlink.h @@ -41,9 +41,11 @@ #include "input.h" #include "input_rc.h" -#include #include +#include +#include + namespace vmount { @@ -90,7 +92,7 @@ protected: virtual int initialize(); private: - void _ack_vehicle_command(uint16_t command); + void _ack_vehicle_command(vehicle_command_s *cmd); int _vehicle_command_sub = -1; orb_advert_t _vehicle_command_ack_pub = nullptr; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0b24522006..6bb5a8083f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -4135,8 +4135,13 @@ void answer_command(struct vehicle_command_s &cmd, unsigned result, /* publish ACK */ vehicle_command_ack_s command_ack = { .timestamp = 0, + .result_param2 = 0, .command = cmd.command, - .result = (uint8_t)result + .result = (uint8_t)result, + .from_external = 0, + .result_param1 = 0, + .target_system = cmd.source_system, + .target_component = cmd.source_component }; if (command_ack_pub != nullptr) { diff --git a/src/modules/events/send_event.cpp b/src/modules/events/send_event.cpp index 5c3c2b49fb..11b6568cd3 100644 --- a/src/modules/events/send_event.cpp +++ b/src/modules/events/send_event.cpp @@ -171,8 +171,13 @@ void SendEvent::answer_command(const vehicle_command_s &cmd, unsigned result) /* publish ACK */ struct vehicle_command_ack_s command_ack = { .timestamp = hrt_absolute_time(), + .result_param2 = 0, .command = cmd.command, .result = (uint8_t)result, + .from_external = 0, + .result_param1 = 0, + .target_system = cmd.source_system, + .target_component = cmd.source_component }; if (_command_ack_pub != nullptr) { diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 4604e373c1..d9b0edf406 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -49,7 +49,6 @@ #include #include #include -#include #include #include @@ -867,22 +866,22 @@ void Logger::run() if (command.command == vehicle_command_s::VEHICLE_CMD_LOGGING_START) { if ((int)(command.param1 + 0.5f) != 0) { - ack_vehicle_command(vehicle_command_ack_pub, command.command, + ack_vehicle_command(vehicle_command_ack_pub, &command, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED); } else if (can_start_mavlink_log()) { - ack_vehicle_command(vehicle_command_ack_pub, command.command, + ack_vehicle_command(vehicle_command_ack_pub, &command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); start_log_mavlink(); } else { - ack_vehicle_command(vehicle_command_ack_pub, command.command, + ack_vehicle_command(vehicle_command_ack_pub, &command, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); } } else if (command.command == vehicle_command_s::VEHICLE_CMD_LOGGING_STOP) { stop_log_mavlink(); - ack_vehicle_command(vehicle_command_ack_pub, command.command, + ack_vehicle_command(vehicle_command_ack_pub, &command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); } } @@ -1999,12 +1998,17 @@ int Logger::remove_directory(const char *dir) return ret; } -void Logger::ack_vehicle_command(orb_advert_t &vehicle_command_ack_pub, uint16_t command, uint32_t result) +void Logger::ack_vehicle_command(orb_advert_t &vehicle_command_ack_pub, vehicle_command_s *cmd, uint32_t result) { vehicle_command_ack_s vehicle_command_ack = { .timestamp = hrt_absolute_time(), - .command = command, - .result = (uint8_t)result + .result_param2 = 0, + .command = cmd->command, + .result = (uint8_t)result, + .from_external = 0, + .result_param1 = 0, + .target_system = cmd->source_system, + .target_component = cmd->source_component }; if (vehicle_command_ack_pub == nullptr) { diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h index 1fbadcc623..888df7590e 100644 --- a/src/modules/logger/logger.h +++ b/src/modules/logger/logger.h @@ -43,6 +43,8 @@ #include #include +#include + extern "C" __EXPORT int logger_main(int argc, char *argv[]); #define TRY_SUBSCRIBE_INTERVAL 1000*1000 // interval in microseconds at which we try to subscribe to a topic @@ -269,7 +271,7 @@ private: void add_thermal_calibration_topics(); void add_system_identification_topics(); - void ack_vehicle_command(orb_advert_t &vehicle_command_ack_pub, uint16_t command, uint32_t result); + void ack_vehicle_command(orb_advert_t &vehicle_command_ack_pub, vehicle_command_s *cmd, uint32_t result); /** * initialize the output for the process load, so that ~1 second later it will be written to the log diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 472f8952ee..bbf5c5284c 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2207,6 +2207,10 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_command_ack_t msg; msg.result = command_ack.result; msg.command = command_ack.command; + msg.progress = command_ack.result_param1; + msg.result_param2 = command_ack.result_param2; + msg.target_system = command_ack.target_system; + msg.target_component = command_ack.target_component; current_command_ack = command_ack.command; mavlink_msg_command_ack_send_struct(get_channel(), &msg); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 4ec488feb5..036aaf726c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -472,8 +472,13 @@ out: if (send_ack) { vehicle_command_ack_s command_ack = { .timestamp = 0, + .result_param2 = 0, .command = cmd_mavlink.command, - .result = (ret == PX4_OK ? vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_RESULT_FAILED) + .result = (ret == PX4_OK ? vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_RESULT_FAILED), + .from_external = 0, + .result_param1 = 0, + .target_system = msg->sysid, + .target_component = msg->compid }; if (_command_ack_pub == nullptr) { @@ -564,8 +569,13 @@ out: if (send_ack) { vehicle_command_ack_s command_ack = { .timestamp = 0, + .result_param2 = 0, .command = cmd_mavlink.command, - .result = (ret == PX4_OK ? vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_RESULT_FAILED) + .result = (ret == PX4_OK ? vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_RESULT_FAILED), + .from_external = 0, + .result_param1 = 0, + .target_system = msg->sysid, + .target_component = msg->compid }; if (_command_ack_pub == nullptr) { @@ -588,9 +598,13 @@ MavlinkReceiver::handle_message_command_ack(mavlink_message_t *msg) vehicle_command_ack_s command_ack = { .timestamp = hrt_absolute_time(), + .result_param2 = ack.result_param2, .command = ack.command, .result = ack.result, - .from_external = 1 + .from_external = 1, + .result_param1 = ack.progress, + .target_system = ack.target_system, + .target_component = ack.target_component }; if (_command_ack_pub == nullptr) { diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index e8f6742861..02951006fb 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -536,8 +536,13 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) // Acknowledge the received command struct vehicle_command_ack_s ack = { .timestamp = 0, + .result_param2 = 0, .command = cmd.command, - .result = cmd_ack_result + .result = cmd_ack_result, + .from_external = 0, + .result_param1 = 0, + .target_system = cmd.source_system, + .target_component = cmd.source_component }; if (_command_ack_pub == nullptr) {