fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 11:37:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: send NACK if msg ID does not exist

Browse Source
This commit is contained in:
Julian Oes
2016-11-08 17:00:08 +01:00
committed by Beat Küng
parent 1e617f362d
commit d0b1983784
2 changed files with 22 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+19 -3
View File
@@ -375,11 +375,17 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
_mavlink->configure_stream_threadsafe("HOME_POSITION", 0.5f);
} 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);
int ret = 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;
if (ret == PX4_OK) {
command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
} else {
command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
}
int instance;
orb_publish_auto(ORB_ID(vehicle_command_ack), &_command_ack_pub, &command_ack, &instance, ORB_PRIO_DEFAULT);
@@ -1492,9 +1498,13 @@ MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg)
// REQUEST_DATA_STREAM is deprecated, please use SET_MESSAGE_INTERVAL instead
}
void
int
MavlinkReceiver::set_message_interval(int msgId, float interval, int data_rate)
{
if (msgId == 0) {
return PX4_ERROR;
}
if (data_rate > 0) {
_mavlink->set_data_rate(data_rate);
}
@@ -1514,6 +1524,9 @@ MavlinkReceiver::set_message_interval(int msgId, float interval, int data_rate)
// don't publish a default rate so for now let's pick a default rate of zero.
}
bool found_id = false;
// The interval between two messages is in microseconds.
// Set to -1 to disable and 0 to request default rate
if (msgId != 0) {
@@ -1522,10 +1535,13 @@ MavlinkReceiver::set_message_interval(int msgId, float interval, int data_rate)
if (msgId == item->get_id()) {
_mavlink->configure_stream_threadsafe(item->get_name(), rate);
found_id = true;
break;
}
}
}
return (found_id ? PX4_OK : PX4_ERROR);
}
void
src/modules/mavlink/mavlink_receiver.h
+3 -1
View File
@@ -157,8 +157,10 @@ private:
* @param msgId the message ID of to change the interval of
* @param interval the interval in us to send the message at
* @param data_rate the total link data rate in bytes per second
*
* @return PX4_OK on success, PX4_ERROR on fail
*/
void set_message_interval(int msgId, float interval, int data_rate = -1);
int set_message_interval(int msgId, float interval, int data_rate = -1);
void get_message_interval(int msgId);
/**