From 1e617f362dbda39edf370fee6bdbfbde7c75c9c8 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 8 Nov 2016 16:09:33 +0100 Subject: [PATCH] mavlink: send ACK for CMD_SET_MESSAGE_INTERVAL There was no feedback if a CMD_SET_MESSAGE_INTERVAL went through or not. --- src/modules/mavlink/mavlink_receiver.cpp | 20 +++++++++++++++----- src/modules/mavlink/mavlink_receiver.h | 1 + 2 files changed, 16 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 513f29ff3b..b92c9d58a2 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -82,6 +82,8 @@ #include #include +#include + #include "mavlink_bridge_header.h" #include "mavlink_receiver.h" #include "mavlink_main.h" @@ -128,6 +130,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _transponder_report_pub(nullptr), _control_state_pub(nullptr), _gps_inject_data_pub(nullptr), + _command_ack_pub(nullptr), _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), _hil_frames(0), _old_timestamp(0), @@ -374,6 +377,13 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) } else if (cmd_mavlink.command == MAV_CMD_SET_MESSAGE_INTERVAL) { set_message_interval((int)(cmd_mavlink.param1 + 0.5f), cmd_mavlink.param2, cmd_mavlink.param3); + vehicle_command_ack_s command_ack; + command_ack.command = cmd_mavlink.command; + command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED; + + int instance; + orb_publish_auto(ORB_ID(vehicle_command_ack), &_command_ack_pub, &command_ack, &instance, ORB_PRIO_DEFAULT); + } else if (cmd_mavlink.command == MAV_CMD_GET_MESSAGE_INTERVAL) { get_message_interval((int)cmd_mavlink.param1); @@ -2097,15 +2107,15 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); } } - + /* control state */ control_state_s ctrl_state = {}; matrix::Quaternion q(hil_state.attitude_quaternion); matrix::Dcm R_to_body(q.inversed()); - + //Time ctrl_state.timestamp = hrt_absolute_time(); - + //Roll Rates: //ctrl_state: body angular rate (rad/s, x forward/y right/z down) //hil_state : body frame angular speed (rad/s) @@ -2120,7 +2130,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) float y; double lat = hil_state.lat * 1e-7; double lon = hil_state.lon * 1e-7; - map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y); + map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y); ctrl_state.x_pos = x; ctrl_state.y_pos = y; ctrl_state.z_pos = hil_state.alt / 1000.0f; @@ -2130,7 +2140,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) ctrl_state.q[1] = q(1); ctrl_state.q[2] = q(2); ctrl_state.q[3] = q(3); - + // Velocity //ctrl_state: velocity in body frame (x forward/y right/z down) //hil_state : Ground Speed in NED expressed as m/s * 100 diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 01da999d15..06efd59bed 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -224,6 +224,7 @@ private: orb_advert_t _control_state_pub; static const int _gps_inject_data_queue_size = 6; orb_advert_t _gps_inject_data_pub; + orb_advert_t _command_ack_pub; int _control_mode_sub; int _hil_frames; uint64_t _old_timestamp;