fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 17:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: send ACK for CMD_SET_MESSAGE_INTERVAL

Browse Source 
There was no feedback if a CMD_SET_MESSAGE_INTERVAL went through or not.
This commit is contained in:
Julian Oes
2016-11-09 08:50:13 +01:00
committed by Beat Küng
parent 643ccd66b6
commit 1e617f362d
2 changed files with 16 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+15 -5
View File
@@ -82,6 +82,8 @@
#include <commander/px4_custom_mode.h> #include <commander/px4_custom_mode.h>
#include <geo/geo.h> #include <geo/geo.h>
#include <uORB/topics/vehicle_command_ack.h>
#include "mavlink_bridge_header.h" #include "mavlink_bridge_header.h"
#include "mavlink_receiver.h" #include "mavlink_receiver.h"
#include "mavlink_main.h" #include "mavlink_main.h"
@@ -128,6 +130,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_transponder_report_pub(nullptr), _transponder_report_pub(nullptr),
_control_state_pub(nullptr), _control_state_pub(nullptr),
_gps_inject_data_pub(nullptr), _gps_inject_data_pub(nullptr),
_command_ack_pub(nullptr),
_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
_hil_frames(0), _hil_frames(0),
_old_timestamp(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) { } 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); 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) { } else if (cmd_mavlink.command == MAV_CMD_GET_MESSAGE_INTERVAL) {
get_message_interval((int)cmd_mavlink.param1); 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); orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
} }
} }
/* control state */ /* control state */
control_state_s ctrl_state = {}; control_state_s ctrl_state = {};
matrix::Quaternion<float> q(hil_state.attitude_quaternion); matrix::Quaternion<float> q(hil_state.attitude_quaternion);
matrix::Dcm<float> R_to_body(q.inversed()); matrix::Dcm<float> R_to_body(q.inversed());
//Time //Time
ctrl_state.timestamp = hrt_absolute_time(); ctrl_state.timestamp = hrt_absolute_time();
//Roll Rates: //Roll Rates:
//ctrl_state: body angular rate (rad/s, x forward/y right/z down) //ctrl_state: body angular rate (rad/s, x forward/y right/z down)
//hil_state : body frame angular speed (rad/s) //hil_state : body frame angular speed (rad/s)
@@ -2120,7 +2130,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
float y; float y;
double lat = hil_state.lat * 1e-7; double lat = hil_state.lat * 1e-7;
double lon = hil_state.lon * 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.x_pos = x;
ctrl_state.y_pos = y; ctrl_state.y_pos = y;
ctrl_state.z_pos = hil_state.alt / 1000.0f; 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[1] = q(1);
ctrl_state.q[2] = q(2); ctrl_state.q[2] = q(2);
ctrl_state.q[3] = q(3); ctrl_state.q[3] = q(3);
// Velocity // Velocity
//ctrl_state: velocity in body frame (x forward/y right/z down) //ctrl_state: velocity in body frame (x forward/y right/z down)
//hil_state : Ground Speed in NED expressed as m/s * 100 //hil_state : Ground Speed in NED expressed as m/s * 100
src/modules/mavlink/mavlink_receiver.h
+1
View File
@@ -224,6 +224,7 @@ private:
orb_advert_t _control_state_pub; orb_advert_t _control_state_pub;
static const int _gps_inject_data_queue_size = 6; static const int _gps_inject_data_queue_size = 6;
orb_advert_t _gps_inject_data_pub; orb_advert_t _gps_inject_data_pub;
orb_advert_t _command_ack_pub;
int _control_mode_sub; int _control_mode_sub;
int _hil_frames; int _hil_frames;
uint64_t _old_timestamp; uint64_t _old_timestamp;