mavlink: external setpoint feed trough functionality

4 files changed, 40 insertions, 6 deletions

diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index e49288a74..bec706bad 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -68,6 +68,13 @@ PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING);
  * @group MAVLink
  */
 PARAM_DEFINE_INT32(MAV_USEHILGPS, 0);
+/**
+ * Forward external setpoint messages
+ * If set to 1 incomming external setpoint messages will be directly forwarded to the controllers if in offboard
+ * control mode
+ * @group MAVLink
+ */
+PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1);
 
 mavlink_system_t mavlink_system = {
 	100,
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index cd0581af4..7707c0bc8 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -220,6 +220,7 @@ Mavlink::Mavlink() :
 	_task_running(false),
 	_hil_enabled(false),
 	_use_hil_gps(false),
+	_forward_externalsp(false),
 	_is_usb_uart(false),
 	_wait_to_transmit(false),
 	_received_messages(false),
@@ -246,6 +247,7 @@ Mavlink::Mavlink() :
 	_param_component_id(0),
 	_param_system_type(0),
 	_param_use_hil_gps(0),
+	_param_forward_externalsp(0),
 
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
@@ -517,6 +519,7 @@ void Mavlink::mavlink_update_system(void)
 		_param_component_id = param_find("MAV_COMP_ID");
 		_param_system_type = param_find("MAV_TYPE");
 		_param_use_hil_gps = param_find("MAV_USEHILGPS");
+		_param_forward_externalsp = param_find("MAV_FWDEXTSP");
 		_param_initialized = true;
 	}
 
@@ -546,6 +549,11 @@ void Mavlink::mavlink_update_system(void)
 	param_get(_param_use_hil_gps, &use_hil_gps);
 
 	_use_hil_gps = (bool)use_hil_gps;
+
+	int32_t forward_externalsp;
+	param_get(_param_forward_externalsp, &forward_externalsp);
+
+	_forward_externalsp = (bool)forward_externalsp;
 }
 
 int Mavlink::get_system_id()
@@ -1501,14 +1509,14 @@ Mavlink::task_main(int argc, char *argv[])
                 if (read_count > sizeof(mavlink_message_t)) {
                     read_count = sizeof(mavlink_message_t);
                 }
-                
+
                 memcpy(write_ptr, read_ptr, read_count);
-                
+
                 // We hold the mutex until after we complete the second part of the buffer. If we don't
                 // we may end up breaking the empty slot overflow detection semantics when we mark the
                 // possibly partial read below.
                 pthread_mutex_lock(&_message_buffer_mutex);
-                
+
 				message_buffer_mark_read(read_count);
 
 				/* write second part of buffer if there is some */
@@ -1519,7 +1527,7 @@ Mavlink::task_main(int argc, char *argv[])
                     memcpy(write_ptr, read_ptr, read_count);
 					message_buffer_mark_read(available);
 				}
-                
+
                 pthread_mutex_unlock(&_message_buffer_mutex);
 
                 _mavlink_resend_uart(_channel, &msg);
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index acfc8b90e..8738a6f18 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -134,6 +134,8 @@ public:
 
 	bool			get_use_hil_gps() { return _use_hil_gps; }
 
+	bool			get_forward_externalsp() { return _forward_externalsp; }
+
 	bool			get_flow_control_enabled() { return _flow_control_enabled; }
 
 	bool			get_forwarding_on() { return _forwarding_on; }
@@ -205,7 +207,7 @@ public:
 	 * Send a status text with loglevel
 	 *
 	 * @param string the message to send (will be capped by mavlink max string length)
-	 * @param severity the log level, one of 
+	 * @param severity the log level, one of
 	 */
 	int			send_statustext(unsigned severity, const char *string);
 	MavlinkStream *		get_streams() const { return _streams; }
@@ -220,7 +222,7 @@ public:
 	bool			should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
 
 	bool			message_buffer_write(const void *ptr, int size);
-    
+
 	void			lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
 	void			unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); }
 
@@ -241,6 +243,7 @@ private:
 	/* states */
 	bool			_hil_enabled;		/**< Hardware In the Loop mode */
 	bool			_use_hil_gps;		/**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
+	bool			_forward_externalsp;	/**< Forward external setpoint messages to controllers directly if in offboard mode */
 	bool			_is_usb_uart;		/**< Port is USB */
 	bool        		_wait_to_transmit;  	/**< Wait to transmit until received messages. */
 	bool        		_received_messages;	/**< Whether we've received valid mavlink messages. */
@@ -301,6 +304,7 @@ private:
 	param_t			_param_component_id;
 	param_t			_param_system_type;
 	param_t			_param_use_hil_gps;
+	param_t			_param_forward_externalsp;
 
 	perf_counter_t		_loop_perf;			/**< loop performance counter */
 	perf_counter_t		_txerr_perf;			/**< TX error counter */
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index bd1296f62..9df843e58 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -451,6 +451,21 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes
 		} else {
 			orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp);
 		}
+
+		/* If we are in offboard control mode and offboard control loop through is enabled
+		 * also publish the setpoint topic which is read by the controller */
+		if (_mavlink->get_forward_externalsp()) {
+			bool updated;
+			orb_check(_control_mode_sub, &updated);
+			if (updated) {
+				orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
+			}
+
+			if (_control_mode.flag_control_offboard_enabled) {
+				//XXX: copy to and publish setpoint triplet here
+			}
+
+		}
 	}
 }