|
|
|
@ -167,12 +167,14 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
@@ -167,12 +167,14 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
|
|
|
|
|
if (_mavlink->accepting_commands()) { |
|
|
|
|
handle_message_command_long(msg); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case MAVLINK_MSG_ID_COMMAND_INT: |
|
|
|
|
if (_mavlink->accepting_commands()) { |
|
|
|
|
handle_message_command_int(msg); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case MAVLINK_MSG_ID_OPTICAL_FLOW_RAD: |
|
|
|
@ -187,6 +189,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
@@ -187,6 +189,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
|
|
|
|
|
if (_mavlink->accepting_commands()) { |
|
|
|
|
handle_message_set_mode(msg); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case MAVLINK_MSG_ID_ATT_POS_MOCAP: |
|
|
|
@ -229,6 +232,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
@@ -229,6 +232,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
|
|
|
|
|
if (_mavlink->accepting_commands()) { |
|
|
|
|
handle_message_request_data_stream(msg); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case MAVLINK_MSG_ID_SYSTEM_TIME: |
|
|
|
@ -355,11 +359,11 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
@@ -355,11 +359,11 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
|
|
|
|
|
} 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) { |
|
|
|
|
set_message_interval((int)cmd_mavlink.param1, cmd_mavlink.param2); |
|
|
|
|
} else if (cmd_mavlink.command == MAV_CMD_SET_MESSAGE_INTERVAL) { |
|
|
|
|
set_message_interval((int)cmd_mavlink.param1, cmd_mavlink.param2); |
|
|
|
|
|
|
|
|
|
} else if (cmd_mavlink.command == MAV_CMD_GET_MESSAGE_INTERVAL) { |
|
|
|
|
get_message_interval((int)cmd_mavlink.param1); |
|
|
|
|
} else if (cmd_mavlink.command == MAV_CMD_GET_MESSAGE_INTERVAL) { |
|
|
|
|
get_message_interval((int)cmd_mavlink.param1); |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
|
|
|
|
@ -539,7 +543,7 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
@@ -539,7 +543,7 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
|
|
|
|
|
|
|
|
|
|
if (_flow_distance_sensor_pub == nullptr) { |
|
|
|
|
_flow_distance_sensor_pub = orb_advertise_multi(ORB_ID(distance_sensor), &d, |
|
|
|
|
&_orb_class_instance, ORB_PRIO_HIGH); |
|
|
|
|
&_orb_class_instance, ORB_PRIO_HIGH); |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
orb_publish(ORB_ID(distance_sensor), _flow_distance_sensor_pub, &d); |
|
|
|
@ -1374,75 +1378,76 @@ MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg)
@@ -1374,75 +1378,76 @@ MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg)
|
|
|
|
|
mavlink_request_data_stream_t req; |
|
|
|
|
mavlink_msg_request_data_stream_decode(msg, &req); |
|
|
|
|
|
|
|
|
|
PX4_WARN("REQUEST_DATA_STREAM is deprecated, please use SET_MESSAGE_INTERVAL instead"); |
|
|
|
|
PX4_WARN("REQUEST_DATA_STREAM is deprecated, please use SET_MESSAGE_INTERVAL instead"); |
|
|
|
|
|
|
|
|
|
if (req.target_system == mavlink_system.sysid && req.target_component == mavlink_system.compid && |
|
|
|
|
req.req_stream_id != 0) |
|
|
|
|
{ |
|
|
|
|
// compute interval in microseconds.
|
|
|
|
|
float interval = 0; |
|
|
|
|
int rate = req.req_message_rate; |
|
|
|
|
if (req.start_stop == 0 || rate == 0) { |
|
|
|
|
rate = -1; |
|
|
|
|
} |
|
|
|
|
else if (rate > 0) { |
|
|
|
|
interval = 1000000.0f / rate; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// todo: note it is kind of bogus to interpret the req_stream_id as a message id
|
|
|
|
|
// but this is how it was. MAVLINK defines separate MAV_DATA_STREAM ids which bundle
|
|
|
|
|
// messages into groups, but that is all deprecated now anyway, and this code was
|
|
|
|
|
// previously interpreting the req_stream_id as a message id, so we'll leave it that way.
|
|
|
|
|
set_message_interval(req.req_stream_id, interval); |
|
|
|
|
req.req_stream_id != 0) { |
|
|
|
|
// compute interval in microseconds.
|
|
|
|
|
float interval = 0; |
|
|
|
|
int rate = req.req_message_rate; |
|
|
|
|
|
|
|
|
|
if (req.start_stop == 0 || rate == 0) { |
|
|
|
|
rate = -1; |
|
|
|
|
|
|
|
|
|
} else if (rate > 0) { |
|
|
|
|
interval = 1000000.0f / rate; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// todo: note it is kind of bogus to interpret the req_stream_id as a message id
|
|
|
|
|
// but this is how it was. MAVLINK defines separate MAV_DATA_STREAM ids which bundle
|
|
|
|
|
// messages into groups, but that is all deprecated now anyway, and this code was
|
|
|
|
|
// previously interpreting the req_stream_id as a message id, so we'll leave it that way.
|
|
|
|
|
set_message_interval(req.req_stream_id, interval); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void |
|
|
|
|
MavlinkReceiver::set_message_interval(int msgId, float interval) |
|
|
|
|
{ |
|
|
|
|
// configure_stream wants a rate (msgs/second), so convert here.
|
|
|
|
|
float rate = 0; |
|
|
|
|
if (interval < 0) { |
|
|
|
|
// stop the stream.
|
|
|
|
|
rate = 0; |
|
|
|
|
} |
|
|
|
|
else if (interval > 0) { |
|
|
|
|
rate = 1000000.0f / interval; |
|
|
|
|
} |
|
|
|
|
else { |
|
|
|
|
// note: mavlink spec says rate == 0 is requesting a default rate but our streams
|
|
|
|
|
// don't publish a default rate so for now let's pick a default rate of zero.
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// The interval between two messages is in microseconds.
|
|
|
|
|
// Set to -1 to disable and 0 to request default rate
|
|
|
|
|
if (msgId != 0) |
|
|
|
|
{ |
|
|
|
|
for (unsigned int i = 0; streams_list[i] != nullptr; i++) { |
|
|
|
|
const StreamListItem* item = streams_list[i]; |
|
|
|
|
if (msgId == item->get_id()) { |
|
|
|
|
_mavlink->configure_stream_threadsafe(item->get_name(), rate); |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
// configure_stream wants a rate (msgs/second), so convert here.
|
|
|
|
|
float rate = 0; |
|
|
|
|
|
|
|
|
|
if (interval < 0) { |
|
|
|
|
// stop the stream.
|
|
|
|
|
rate = 0; |
|
|
|
|
|
|
|
|
|
} else if (interval > 0) { |
|
|
|
|
rate = 1000000.0f / interval; |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
// note: mavlink spec says rate == 0 is requesting a default rate but our streams
|
|
|
|
|
// don't publish a default rate so for now let's pick a default rate of zero.
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// The interval between two messages is in microseconds.
|
|
|
|
|
// Set to -1 to disable and 0 to request default rate
|
|
|
|
|
if (msgId != 0) { |
|
|
|
|
for (unsigned int i = 0; streams_list[i] != nullptr; i++) { |
|
|
|
|
const StreamListItem *item = streams_list[i]; |
|
|
|
|
|
|
|
|
|
if (msgId == item->get_id()) { |
|
|
|
|
_mavlink->configure_stream_threadsafe(item->get_name(), rate); |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void |
|
|
|
|
MavlinkReceiver::get_message_interval(int msgId) |
|
|
|
|
{ |
|
|
|
|
unsigned interval = 0; |
|
|
|
|
|
|
|
|
|
MavlinkStream *stream; |
|
|
|
|
LL_FOREACH(_mavlink->get_streams(), stream) { |
|
|
|
|
if (stream->get_id() == msgId) { |
|
|
|
|
interval = stream->get_interval(); |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// send back this value...
|
|
|
|
|
mavlink_msg_message_interval_send(_mavlink->get_channel(), msgId, interval); |
|
|
|
|
unsigned interval = 0; |
|
|
|
|
|
|
|
|
|
MavlinkStream *stream; |
|
|
|
|
LL_FOREACH(_mavlink->get_streams(), stream) { |
|
|
|
|
if (stream->get_id() == msgId) { |
|
|
|
|
interval = stream->get_interval(); |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// send back this value...
|
|
|
|
|
mavlink_msg_message_interval_send(_mavlink->get_channel(), msgId, interval); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void |
|
|
|
|