Browse Source

Fix code style using Tools/fix_code_style.sh

sbg
Chris Lovett 9 years ago committed by Lorenz Meier
parent
commit
847d9ec4f4
  1. 11
      src/modules/mavlink/mavlink_messages.h
  2. 127
      src/modules/mavlink/mavlink_receiver.cpp
  3. 8
      src/modules/mavlink/mavlink_receiver.h

11
src/modules/mavlink/mavlink_messages.h

@ -43,17 +43,18 @@ @@ -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() {};
};

127
src/modules/mavlink/mavlink_receiver.cpp

@ -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

8
src/modules/mavlink/mavlink_receiver.h

@ -145,10 +145,10 @@ private: @@ -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);

Loading…
Cancel
Save