From 459a71a6f19b96202c4b1292beb0fe409fac4283 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 31 Oct 2017 10:37:59 +0100 Subject: [PATCH] mavlink_receiver: directly pass the result to acknowledge() And use denied in case ulog streaming fails. --- src/modules/mavlink/mavlink_receiver.cpp | 17 +++++++++-------- src/modules/mavlink/mavlink_receiver.h | 2 +- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 797e59e264..65e0ec05cb 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -169,13 +169,13 @@ MavlinkReceiver::~MavlinkReceiver() orb_unsubscribe(_actuator_armed_sub); } -void MavlinkReceiver::acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, int ret) +void MavlinkReceiver::acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, uint8_t result) { vehicle_command_ack_s command_ack = { .timestamp = hrt_absolute_time(), .result_param2 = 0, .command = command, - .result = (ret == PX4_OK ? vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_RESULT_FAILED), + .result = result, .from_external = false, .result_param1 = 0, .target_system = sysid, @@ -509,11 +509,10 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const bool target_ok = evaluate_target_ok(cmd_mavlink.command, cmd_mavlink.target_system, cmd_mavlink.target_component); bool send_ack = true; - int ret = PX4_OK; + uint8_t result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED; if (!target_ok) { - ret = PX4_ERROR; - acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, ret); + acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, vehicle_command_ack_s::VEHICLE_RESULT_FAILED); return; } @@ -529,7 +528,9 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const _mavlink->configure_stream_threadsafe("HOME_POSITION", 0.5f); } else if (cmd_mavlink.command == MAV_CMD_SET_MESSAGE_INTERVAL) { - ret = set_message_interval((int)(cmd_mavlink.param1 + 0.5f), cmd_mavlink.param2, cmd_mavlink.param3); + if (set_message_interval((int)(cmd_mavlink.param1 + 0.5f), cmd_mavlink.param2, cmd_mavlink.param3)) { + result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED; + } } else if (cmd_mavlink.command == MAV_CMD_GET_MESSAGE_INTERVAL) { get_message_interval((int)cmd_mavlink.param1); @@ -552,7 +553,7 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const // on a radio link if (_mavlink->get_data_rate() < 5000) { send_ack = true; - ret = PX4_ERROR; + result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED; _mavlink->send_statustext_critical("Not enough bandwidth to enable log streaming"); } else { @@ -578,7 +579,7 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const } if (send_ack) { - acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, ret); + acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, result); } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index c6526a4a3b..73c99c1bdc 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -115,7 +115,7 @@ public: private: - void acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, int ret); + void acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, uint8_t result); void handle_message(mavlink_message_t *msg); void handle_message_command_long(mavlink_message_t *msg); void handle_message_command_int(mavlink_message_t *msg);