From 8f7a5d0e0cfd171c0e4e4a64bce45981c288b55e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 31 Oct 2017 10:20:59 +0100 Subject: [PATCH] mavlink_receiver: merge handle_message_command_{long,int} into a common method --- src/modules/mavlink/mavlink_receiver.cpp | 154 ++++++++--------------- src/modules/mavlink/mavlink_receiver.h | 6 + 2 files changed, 55 insertions(+), 105 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 0c6e370ece..8a515244e5 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -450,88 +450,25 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) mavlink_command_long_t cmd_mavlink; mavlink_msg_command_long_decode(msg, &cmd_mavlink); - 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; - - if (!target_ok) { - ret = PX4_ERROR; - acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, ret); - return; - } - - if (cmd_mavlink.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) { - /* send autopilot version message */ - _mavlink->send_autopilot_capabilites(); - - } else if (cmd_mavlink.command == MAV_CMD_REQUEST_PROTOCOL_VERSION) { - /* send protocol version message */ - _mavlink->send_protocol_version(); - - } else if (cmd_mavlink.command == MAV_CMD_GET_HOME_POSITION) { - _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); - - } else if (cmd_mavlink.command == MAV_CMD_GET_MESSAGE_INTERVAL) { - get_message_interval((int)cmd_mavlink.param1); - - } else if (cmd_mavlink.command == MAV_CMD_REQUEST_FLIGHT_INFORMATION) { - send_flight_information(); - - } else { - - send_ack = false; - - if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { - PX4_WARN("ignoring CMD with same SYS/COMP (%d/%d) ID", mavlink_system.sysid, mavlink_system.compid); - return; - } - - if (cmd_mavlink.command == MAV_CMD_LOGGING_START) { - // we already instanciate the streaming object, because at this point we know on which - // mavlink channel streaming was requested. But in fact it's possible that the logger is - // not even running. The main mavlink thread takes care of this by waiting for an ack - // from the logger. - _mavlink->try_start_ulog_streaming(msg->sysid, msg->compid); - - } else if (cmd_mavlink.command == MAV_CMD_LOGGING_STOP) { - _mavlink->request_stop_ulog_streaming(); - } - - struct vehicle_command_s vcmd = { - .timestamp = hrt_absolute_time(), - .param5 = cmd_mavlink.param5, - .param6 = cmd_mavlink.param6, - /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ - .param1 = cmd_mavlink.param1, - .param2 = cmd_mavlink.param2, - .param3 = cmd_mavlink.param3, - .param4 = cmd_mavlink.param4, - .param7 = cmd_mavlink.param7, - // XXX do proper translation - .command = cmd_mavlink.command, - .target_system = cmd_mavlink.target_system, - .target_component = cmd_mavlink.target_component, - .source_system = msg->sysid, - .source_component = msg->compid, - .confirmation = cmd_mavlink.confirmation, - .from_external = true - }; - - if (_cmd_pub == nullptr) { - _cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); - } - } - - if (send_ack) { - acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, ret); - } + struct vehicle_command_s vcmd = { + .timestamp = hrt_absolute_time(), + .param5 = cmd_mavlink.param5, + .param6 = cmd_mavlink.param6, + /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ + .param1 = cmd_mavlink.param1, + .param2 = cmd_mavlink.param2, + .param3 = cmd_mavlink.param3, + .param4 = cmd_mavlink.param4, + .param7 = cmd_mavlink.param7, + .command = cmd_mavlink.command, + .target_system = cmd_mavlink.target_system, + .target_component = cmd_mavlink.target_component, + .source_system = msg->sysid, + .source_component = msg->compid, + .confirmation = cmd_mavlink.confirmation, + .from_external = true + }; + handle_message_command_both(msg, cmd_mavlink, vcmd); } void @@ -541,6 +478,34 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) mavlink_command_int_t cmd_mavlink; mavlink_msg_command_int_decode(msg, &cmd_mavlink); + struct vehicle_command_s vcmd = { + .timestamp = hrt_absolute_time(), + /* these are coordinates as 1e7 scaled integers to work around the 32 bit floating point limits */ + .param5 = ((double)cmd_mavlink.x) / 1e7, + .param6 = ((double)cmd_mavlink.y) / 1e7, + /* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */ + .param1 = cmd_mavlink.param1, + .param2 = cmd_mavlink.param2, + .param3 = cmd_mavlink.param3, + .param4 = cmd_mavlink.param4, + .param7 = cmd_mavlink.z, + .command = cmd_mavlink.command, + .target_system = cmd_mavlink.target_system, + .target_component = cmd_mavlink.target_component, + .source_system = msg->sysid, + .source_component = msg->compid, + .confirmation = false, + .from_external = true + }; + + handle_message_command_both(msg, cmd_mavlink, vcmd); +} + + +template +void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const T &cmd_mavlink, + const vehicle_command_s &vehicle_command) +{ bool target_ok = evaluate_target_ok(cmd_mavlink.command, cmd_mavlink.target_system, cmd_mavlink.target_component); bool send_ack = true; @@ -592,32 +557,11 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) _mavlink->request_stop_ulog_streaming(); } - struct vehicle_command_s vcmd = { - .timestamp = hrt_absolute_time(), - /* these are coordinates as 1e7 scaled integers to work around the 32 bit floating point limits */ - .param5 = ((double)cmd_mavlink.x) / 1e7, - .param6 = ((double)cmd_mavlink.y) / 1e7, - /* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */ - .param1 = cmd_mavlink.param1, - .param2 = cmd_mavlink.param2, - .param3 = cmd_mavlink.param3, - .param4 = cmd_mavlink.param4, - .param7 = cmd_mavlink.z, - // XXX do proper translation - .command = cmd_mavlink.command, - .target_system = cmd_mavlink.target_system, - .target_component = cmd_mavlink.target_component, - .source_system = msg->sysid, - .source_component = msg->compid, - .confirmation = false, - .from_external = true - }; - if (_cmd_pub == nullptr) { - _cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH); + _cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vehicle_command, vehicle_command_s::ORB_QUEUE_LENGTH); } else { - orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); + orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vehicle_command); } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index f58c6b05ef..c6526a4a3b 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -119,6 +119,12 @@ private: 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); + /** + * common method to handle both mavlink command types. T is one of mavlink_command_int_t or mavlink_command_long_t + */ + template + void handle_message_command_both(mavlink_message_t *msg, const T &cmd_mavlink, + const vehicle_command_s &vehicle_command); void handle_message_command_ack(mavlink_message_t *msg); void handle_message_optical_flow_rad(mavlink_message_t *msg); void handle_message_hil_optical_flow(mavlink_message_t *msg);