mavlink: minor refactoring and cleanup, rate limiter class implemented, new messages added

src/modules/mavlink/mavlink_main.cpp

@@ -79,6 +79,7 @@
 #include "mavlink_main.h"
 #include "mavlink_messages.h"
 #include "mavlink_receiver.h"
+#include "mavlink_rate_limiter.h"
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -321,7 +322,7 @@ Mavlink::get_uart_fd()
 mavlink_channel_t
 Mavlink::get_channel()
 {
-	return _chan;
+	return _channel;
 }
 
 /****************************************************************************
@@ -1321,7 +1322,7 @@ Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg)
 {
 	uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg);
 
-	mavlink_send_uart_bytes(_chan, missionlib_msg_buf, len);
+	mavlink_send_uart_bytes(_channel, missionlib_msg_buf, len);
 }
 
 
@@ -1356,7 +1357,7 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string)
 	}
 }
 
-MavlinkOrbSubscription *Mavlink::add_orb_subscription(const struct orb_metadata *topic, const size_t size)
+MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, const size_t size)
 {
 	/* check if already subscribed to this topic */
 	MavlinkOrbSubscription *sub;
@@ -1406,7 +1407,7 @@ Mavlink::task_main(int argc, char *argv[])
 
 	int ch;
 	_baudrate = 57600;
-	_chan = MAVLINK_COMM_0;
+	_channel = MAVLINK_COMM_0;
 
 	_mode = MODE_OFFBOARD;
 
@@ -1520,8 +1521,6 @@ Mavlink::task_main(int argc, char *argv[])
 
 	thread_running = true;
 
-	unsigned lowspeed_counter = 0;
-
 	MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update), sizeof(parameter_update_s));
 	MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status), sizeof(vehicle_status_s));
 
@@ -1530,20 +1529,42 @@ Mavlink::task_main(int argc, char *argv[])
 	warnx("started");
 	mavlink_log_info(_mavlink_fd, "[mavlink] started");
 
-	/* add default streams, intervals depend on baud rate */
+	/* add default streams depending on mode, intervals depend on baud rate */
 	float rate_mult = _baudrate / 57600.0f;
 	if (rate_mult > 4.0f) {
 		rate_mult = 4.0f;
 	}
 
 	add_stream("HEARTBEAT", 1.0f);
-	add_stream("SYS_STATUS", 1.0f * rate_mult);
-	add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult);
-	add_stream("HIGHRES_IMU", 1.0f * rate_mult);
-	add_stream("ATTITUDE", 10.0f * rate_mult);
-	add_stream("GPS_RAW_INT", 1.0f * rate_mult);
-	add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult);
-	add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult);
+
+	switch(_mode) {
+	case MODE_OFFBOARD:
+		add_stream("SYS_STATUS", 1.0f * rate_mult);
+		add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult);
+		add_stream("HIGHRES_IMU", 1.0f * rate_mult);
+		add_stream("ATTITUDE", 10.0f * rate_mult);
+		add_stream("GPS_RAW_INT", 1.0f * rate_mult);
+		add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult);
+		add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult);
+		add_stream("SERVO_OUTPUT_RAW_0", 1.0f * rate_mult);
+		break;
+
+	case MODE_HIL:
+		add_stream("SYS_STATUS", 1.0f * rate_mult);
+		add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult);
+		add_stream("HIGHRES_IMU", 1.0f * rate_mult);
+		add_stream("ATTITUDE", 10.0f * rate_mult);
+		add_stream("GPS_RAW_INT", 1.0f * rate_mult);
+		add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult);
+		add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult);
+		break;
+
+	default:
+		break;
+	}
+
+	MavlinkRateLimiter slow_rate_limiter(2000.0f / rate_mult);
+	MavlinkRateLimiter fast_rate_limiter(100.0f / rate_mult);
 
 	while (!_task_should_exit) {
 		/* main loop */
@@ -1571,13 +1592,6 @@ Mavlink::task_main(int argc, char *argv[])
 			stream->update(t);
 		}
 
-		/* 0.5 Hz */
-		if (lowspeed_counter % (2000000 / MAIN_LOOP_DELAY) == 0) {
-			mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission);
-		}
-
-		lowspeed_counter++;
-
 		bool updated;
 		orb_check(mission_result_sub, &updated);
 
@@ -1591,18 +1605,16 @@ Mavlink::task_main(int argc, char *argv[])
 			}
 
 			mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission);
+		} else {
+			if (slow_rate_limiter.check(t)) {
+				mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission);
+			}
 		}
 
-		if (lowspeed_counter % (50000 / MAIN_LOOP_DELAY) == 0) {
-			/* send parameters at 20 Hz (if queued for sending) */
+		if (fast_rate_limiter.check(t)) {
 			mavlink_pm_queued_send();
 			mavlink_waypoint_eventloop(hrt_absolute_time());
 
-			if (_baudrate > 57600) {
-				mavlink_pm_queued_send();
-			}
-
-			 /* send one string at 20 Hz */
 			if (!mavlink_logbuffer_is_empty(&lb)) {
 				struct mavlink_logmessage msg;
 				int lb_ret = mavlink_logbuffer_read(&lb, &msg);

src/modules/mavlink/mavlink_main.h

@@ -201,7 +201,7 @@ public:
 	 */
 	int		set_hil_enabled(bool hil_enabled);
 
-	MavlinkOrbSubscription *add_orb_subscription(const struct orb_metadata *topic, size_t size);
+	MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, size_t size);
 
 	mavlink_channel_t get_channel();
 
@@ -231,10 +231,7 @@ private:
 	MAVLINK_MODE _mode;
 
 	uint8_t _mavlink_wpm_comp_id;
-	mavlink_channel_t _chan;
-
-//  XXX probably should be in a header... 
-// extern pthread_t receive_start(int uart);
+	mavlink_channel_t _channel;
 
 	struct mavlink_logbuffer lb;
 	unsigned int total_counter;
@@ -248,7 +245,7 @@ private:
 	bool _verbose;
 	int _uart;
 	int _baudrate;
-	bool gcs_link;
+
 	/**
 	 * If the queue index is not at 0, the queue sending
 	 * logic will send parameters from the current index

src/modules/mavlink/mavlink_orb_subscription.cpp

@@ -13,7 +13,7 @@
 
 #include "mavlink_orb_subscription.h"
 
-MavlinkOrbSubscription::MavlinkOrbSubscription(const struct orb_metadata *topic, size_t size) : _topic(topic), _last_check(0), next(nullptr)
+MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic, size_t size) : _topic(topic), _last_check(0), next(nullptr)
 {
 	_data = malloc(size);
 	memset(_data, 0, size);

src/modules/mavlink/mavlink_orb_subscription.h

@@ -16,7 +16,7 @@ class MavlinkOrbSubscription {
 public:
 	MavlinkOrbSubscription *next;
 
-	MavlinkOrbSubscription(const struct orb_metadata *topic, size_t size);
+	MavlinkOrbSubscription(const orb_id_t topic, size_t size);
 	~MavlinkOrbSubscription();
 
 	bool update(const hrt_abstime t);
@@ -24,7 +24,7 @@ public:
 	const struct orb_metadata *get_topic();
 
 private:
-	const struct orb_metadata *_topic;
+	const orb_id_t _topic;
 	int _fd;
 	void *_data;
 	hrt_abstime _last_check;

src/modules/mavlink/mavlink_rate_limiter.cpp

@@ -0,0 +1,38 @@
+/*
+ * mavlink_rate_limiter.cpp
+ *
+ *  Created on: 26.02.2014
+ *      Author: ton
+ */
+
+
+#include "mavlink_rate_limiter.h"
+
+MavlinkRateLimiter::MavlinkRateLimiter() : _last_sent(0), _interval(1000000)
+{
+}
+
+MavlinkRateLimiter::MavlinkRateLimiter(unsigned int interval) : _last_sent(0), _interval(interval * 1000)
+{
+}
+
+MavlinkRateLimiter::~MavlinkRateLimiter()
+{
+}
+
+void
+MavlinkRateLimiter::set_interval(unsigned int interval)
+{
+	_interval = interval * 1000;
+}
+
+bool
+MavlinkRateLimiter::check(hrt_abstime t)
+{
+	uint64_t dt = t - _last_sent;
+	if (dt > 0 && dt >= _interval) {
+		_last_sent = (t / _interval) * _interval;
+		return true;
+	}
+	return false;
+}

src/modules/mavlink/mavlink_rate_limiter.h

@@ -0,0 +1,28 @@
+/*
+ * mavlink_rate_limiter.h
+ *
+ *  Created on: 26.02.2014
+ *      Author: ton
+ */
+
+#ifndef MAVLINK_RATE_LIMITER_H_
+#define MAVLINK_RATE_LIMITER_H_
+
+#include <drivers/drv_hrt.h>
+
+
+class MavlinkRateLimiter {
+private:
+	hrt_abstime _last_sent;
+	hrt_abstime _interval;
+
+public:
+	MavlinkRateLimiter();
+	MavlinkRateLimiter(unsigned int interval);
+	~MavlinkRateLimiter();
+	void set_interval(unsigned int interval);
+	bool check(hrt_abstime t);
+};
+
+
+#endif /* MAVLINK_RATE_LIMITER_H_ */

src/modules/mavlink/mavlink_stream.cpp

@@ -10,7 +10,7 @@
 #include "mavlink_stream.h"
 #include "mavlink_main.h"
 
-MavlinkStream::MavlinkStream() : _interval(1000), _last_sent(0), _channel(MAVLINK_COMM_0), next(nullptr)
+MavlinkStream::MavlinkStream() : _interval(1000000), _last_sent(0), _channel(MAVLINK_COMM_0), next(nullptr)
 {
 }
 

src/modules/mavlink/module.mk

@@ -41,6 +41,7 @@ SRCS		 += mavlink_main.cpp \
 			mavlink_receiver.cpp \
 			mavlink_orb_subscription.cpp \
 			mavlink_messages.cpp \
-			mavlink_stream.cpp
+			mavlink_stream.cpp \
+			mavlink_rate_limiter.cpp
 
 INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink