orb: add gps_inject_data message & publish from mavlink

msg/gps_inject_data.msg

@@ -0,0 +1,2 @@
+uint8 len			# length of data
+uint8[110] data			# data to write to GPS device

src/modules/mavlink/mavlink_receiver.cpp

@@ -129,6 +129,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_time_offset_pub(nullptr),
 	_follow_target_pub(nullptr),
 	_transponder_report_pub(nullptr),
+	_gps_inject_data_pub(),
 	_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
 	_hil_frames(0),
 	_old_timestamp(0),
@@ -147,7 +148,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_mom_switch_pos{},
 	_mom_switch_state(0)
 {
-
+	_gps_inject_data_pub.fill(nullptr);
 }
 
 MavlinkReceiver::~MavlinkReceiver()
@@ -237,6 +238,11 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 	case MAVLINK_MSG_ID_ADSB_VEHICLE:
 		handle_message_adsb_vehicle(msg);
 		break;
+
+	case MAVLINK_MSG_ID_GPS_INJECT_DATA:
+		handle_message_gps_inject_data(msg);
+		break;
+
 	default:
 		break;
 	}
@@ -1686,6 +1692,32 @@ void MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg)
 	}
 }
 
+void MavlinkReceiver::handle_message_gps_inject_data(mavlink_message_t *msg)
+{
+	mavlink_gps_inject_data_t gps_inject_data_msg;
+	gps_inject_data_s gps_inject_data_topic;
+
+	mavlink_msg_gps_inject_data_decode(msg, &gps_inject_data_msg);
+
+	gps_inject_data_topic.len = gps_inject_data_msg.len;
+	memcpy(gps_inject_data_topic.data, gps_inject_data_msg.data,
+	       sizeof(uint8_t) * math::min((uint8_t)110, gps_inject_data_msg.len));
+
+	orb_advert_t &pub = _gps_inject_data_pub[_gps_inject_data_next_idx];
+
+	if (pub == nullptr) {
+		int idx = _gps_inject_data_next_idx;
+		pub = orb_advertise_multi(ORB_ID(gps_inject_data), &gps_inject_data_topic,
+				&idx, ORB_PRIO_DEFAULT);
+
+	} else {
+		orb_publish(ORB_ID(gps_inject_data), pub, &gps_inject_data_topic);
+	}
+
+	_gps_inject_data_next_idx = (_gps_inject_data_next_idx + 1) % _gps_inject_data_pub.size();
+
+}
+
 void
 MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 {

src/modules/mavlink/mavlink_receiver.h

@@ -76,6 +76,9 @@
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/follow_target.h>
 #include <uORB/topics/transponder_report.h>
+#include <uORB/topics/gps_inject_data.h>
+
+#include <array>
 
 #include "mavlink_ftp.h"
 
@@ -140,6 +143,7 @@ private:
 	void handle_message_distance_sensor(mavlink_message_t *msg);
 	void handle_message_follow_target(mavlink_message_t *msg);
 	void handle_message_adsb_vehicle(mavlink_message_t *msg);
+	void handle_message_gps_inject_data(mavlink_message_t *msg);
 
 	void *receive_thread(void *arg);
 
@@ -203,6 +207,8 @@ private:
 	orb_advert_t _time_offset_pub;
 	orb_advert_t _follow_target_pub;
 	orb_advert_t _transponder_report_pub;
+	std::array<orb_advert_t, 4> _gps_inject_data_pub;
+	int _gps_inject_data_next_idx = 0;
 	int _control_mode_sub;
 	int _hil_frames;
 	uint64_t _old_timestamp;

src/modules/uORB/objects_common.cpp

@@ -294,3 +294,6 @@ ORB_DEFINE(commander_state, struct commander_state_s);
 
 #include "topics/transponder_report.h"
 ORB_DEFINE(transponder_report, struct transponder_report_s);
+
+#include "topics/gps_inject_data.h"
+ORB_DEFINE(gps_inject_data, struct gps_inject_data_s);