mavlink: GPS_GLOBAL_ORIGIN message added, set message rate depending on baudrate

2 files changed, 48 insertions, 26 deletions

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 5ac76e1cf..3102c937d 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1531,18 +1531,19 @@ Mavlink::task_main(int argc, char *argv[])
 	mavlink_log_info(_mavlink_fd, "[mavlink] started");
 
 	/* add default streams, intervals depend on baud rate */
-//	if (_baudrate >= 230400) {
-//	} else if (_baudrate >= 115200) {
-//	} else if (_baudrate >= 57600) {
-//	}
+	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);
-	add_stream("HIGHRES_IMU", 1.0f);
-	add_stream("ATTITUDE", 10.0f);
-	add_stream("GPS_RAW_INT", 1.0f);
-	add_stream("GLOBAL_POSITION_INT", 5.0f);
-	add_stream("LOCAL_POSITION_NED", 5.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);
 
 	while (!_task_should_exit) {
 		/* main loop */
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 27ae197a6..48443fbdc 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -430,6 +430,42 @@ protected:
 };
 
 
+class MavlinkStreamGPSGlobalOrigin : public MavlinkStream {
+public:
+	const char *get_name()
+	{
+		return "GPS_GLOBAL_ORIGIN";
+	}
+
+	MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamGPSGlobalOrigin();
+	}
+
+private:
+	MavlinkOrbSubscription *home_sub;
+
+	struct home_position_s *home;
+
+protected:
+
+	void subscribe(Mavlink *mavlink)
+	{
+		home_sub = mavlink->add_orb_subscription(ORB_ID(home_position), sizeof(struct home_position_s));
+		home = (struct home_position_s *)home_sub->get_data();
+	}
+
+	void send(const hrt_abstime t) {
+		home_sub->update(t);
+
+		mavlink_msg_gps_global_origin_send(_channel,
+				(int32_t)(home->lat * 1e7),
+				(int32_t)(home->lon * 1e7),
+				(int32_t)(home->alt) * 1000.0f);
+	}
+};
+
+
 MavlinkStream *streams_list[] = {
 		new MavlinkStreamHeartbeat(),
 		new MavlinkStreamSysStatus(),
@@ -438,6 +474,7 @@ MavlinkStream *streams_list[] = {
 		new MavlinkStreamGPSRawInt(),
 		new MavlinkStreamGlobalPositionInt(),
 		new MavlinkStreamLocalPositionNED(),
+		new MavlinkStreamGPSGlobalOrigin(),
 		nullptr
 };
 
@@ -517,14 +554,6 @@ MavlinkStream *streams_list[] = {
 //
 //
 //void
-//MavlinkOrbListener::l_rc_channels(const struct listener *l)
-//{
-//	/* copy rc channels into local buffer */
-//	orb_copy(ORB_ID(rc_channels), l->mavlink->get_subs()->rc_sub, &l->listener->rc);
-//	// XXX Add RC channels scaled message here
-//}
-//
-//void
 //MavlinkOrbListener::l_input_rc(const struct listener *l)
 //{
 //	/* copy rc _mavlink->get_chan()nels into local buffer */
@@ -801,14 +830,6 @@ MavlinkStream *streams_list[] = {
 //}
 //
 //void
-//MavlinkOrbListener::l_home(const struct listener *l)
-//{
-//	orb_copy(ORB_ID(home_position), l->mavlink->get_subs()->home_sub, &l->listener->home);
-//
-//	mavlink_msg_gps_global_origin_send(l->mavlink->get_chan(), (int32_t)(l->listener->home.lat*1e7d), (int32_t)(l->listener->home.lon*1e7d), (int32_t)(l->listener->home.alt)*1e3f);
-//}
-//
-//void
 //MavlinkOrbListener::l_airspeed(const struct listener *l)
 //{
 //	orb_copy(ORB_ID(airspeed), l->mavlink->get_subs()->airspeed_sub, &l->listener->airspeed);