allow local position for takeoff (e.g. flow)

src/modules/navigator/navigator.h

@@ -52,6 +52,7 @@
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/mission_result.h>
@@ -137,6 +138,7 @@ public:
 	struct vehicle_land_detected_s	    *get_land_detected() { return &_land_detected; }
 	struct vehicle_control_mode_s	    *get_control_mode() { return &_control_mode; }
 	struct vehicle_global_position_s   *get_global_position() { return &_global_pos; }
+	struct vehicle_local_position_s   *get_local_position() { return &_local_pos; }
 	struct vehicle_gps_position_s	    *get_gps_position() { return &_gps_pos; }
 	struct sensor_combined_s	    *get_sensor_combined() { return &_sensor_combined; }
 	struct home_position_s		    *get_home_position() { return &_home_pos; }
@@ -238,6 +240,7 @@ private:
 	orb_advert_t	_mavlink_log_pub;		/**< the uORB advert to send messages over mavlink */
 
 	int		_global_pos_sub;		/**< global position subscription */
+	int		_local_pos_sub;		/**< local position subscription */
 	int		_gps_pos_sub;		/**< gps position subscription */
 	int		_sensor_combined_sub;		/**< sensor combined subscription */
 	int		_home_pos_sub;			/**< home position subscription */
@@ -261,6 +264,7 @@ private:
 	vehicle_land_detected_s				_land_detected;		/**< vehicle land_detected */
 	vehicle_control_mode_s				_control_mode;		/**< vehicle control mode */
 	vehicle_global_position_s			_global_pos;		/**< global vehicle position */
+	vehicle_local_position_s			_local_pos;		/**< local vehicle position */
 	vehicle_gps_position_s				_gps_pos;		/**< gps position */
 	sensor_combined_s				_sensor_combined;	/**< sensor values */
 	home_position_s					_home_pos;		/**< home position for RTL */
@@ -324,6 +328,11 @@ private:
 	 */
 	void		global_position_update();
 
+	/**
+	 * Retrieve local position
+	 */
+	void		local_position_update();
+
 	/**
 	 * Retrieve gps position
 	 */

src/modules/navigator/navigator_main.cpp

@@ -105,6 +105,7 @@ Navigator::Navigator() :
 	_navigator_task(-1),
 	_mavlink_log_pub(nullptr),
 	_global_pos_sub(-1),
+	_local_pos_sub(-1),
 	_gps_pos_sub(-1),
 	_sensor_combined_sub(-1),
 	_home_pos_sub(-1),
@@ -212,6 +213,12 @@ Navigator::global_position_update()
 	orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
 }
 
+void
+Navigator::local_position_update()
+{
+	orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
+}
+
 void
 Navigator::gps_position_update()
 {
@@ -301,6 +308,7 @@ Navigator::task_main()
 
 	/* do subscriptions */
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+	_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
 	_gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
 	_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
 	_fw_pos_ctrl_status_sub = orb_subscribe(ORB_ID(fw_pos_ctrl_status));
@@ -318,6 +326,7 @@ Navigator::task_main()
 	vehicle_land_detected_update();
 	vehicle_control_mode_update();
 	global_position_update();
+	local_position_update();
 	gps_position_update();
 	sensor_combined_update();
 	home_position_update(true);
@@ -382,6 +391,13 @@ Navigator::task_main()
 			}
 		}
 
+		/* local position updated */
+		orb_check(_local_pos_sub, &updated);
+
+		if (updated) {
+			local_position_update();
+		}
+
 		/* sensors combined updated */
 		orb_check(_sensor_combined_sub, &updated);
 
@@ -492,7 +508,13 @@ Navigator::task_main()
 				rep->current.loiter_radius = get_loiter_radius();
 				rep->current.loiter_direction = 1;
 				rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
-				rep->current.yaw = cmd.param4;
+				if (home_position_valid()) {
+					rep->current.yaw = cmd.param4;
+					rep->previous.valid = true;
+				} else {
+					rep->current.yaw = get_local_position()->yaw;
+					rep->previous.valid = false;
+				}
 
 				if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
 					rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7;
@@ -506,7 +528,6 @@ Navigator::task_main()
 
 				rep->current.alt = cmd.param7;
 
-				rep->previous.valid = true;
 				rep->current.valid = true;
 				rep->next.valid = false;