diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h index 685eaff475..3c302891a2 100644 --- a/src/modules/mavlink/mavlink_messages.h +++ b/src/modules/mavlink/mavlink_messages.h @@ -43,17 +43,18 @@ #include "mavlink_stream.h" -class StreamListItem { +class StreamListItem +{ public: - MavlinkStream* (*new_instance)(Mavlink *mavlink); - const char* (*get_name)(); + MavlinkStream *(*new_instance)(Mavlink *mavlink); + const char *(*get_name)(); uint8_t (*get_id)(); - StreamListItem(MavlinkStream* (*inst)(Mavlink *mavlink), const char* (*name)(), uint8_t (*id)()) : + StreamListItem(MavlinkStream * (*inst)(Mavlink *mavlink), const char *(*name)(), uint8_t (*id)()) : new_instance(inst), get_name(name), - get_id(id) {}; + get_id(id) {}; ~StreamListItem() {}; }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index f04d98f887..1841861298 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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) 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) 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) } 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) 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) 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; - } + req.req_stream_id != 0) { + // compute interval in microseconds. + float interval = 0; + int rate = req.req_message_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); + 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. - } + // configure_stream wants a rate (msgs/second), so convert here. + float rate = 0; - // 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; - } - } - } + 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; + unsigned interval = 0; - MavlinkStream *stream; - LL_FOREACH(_mavlink->get_streams(), stream) { - if (stream->get_id() == msgId) { - interval = stream->get_interval(); - break; - } - } + 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); + // send back this value... + mavlink_msg_message_interval_send(_mavlink->get_channel(), msgId, interval); } void diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 7894c3b817..45e904d295 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -145,10 +145,10 @@ private: void *receive_thread(void *arg); - /** - * Set the interval at which the given message stream is published. - * The rate is the number of messages per second. - */ + /** + * Set the interval at which the given message stream is published. + * The rate is the number of messages per second. + */ void set_message_interval(int msgId, float rate); void get_message_interval(int msgId);