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

src/modules/commander/commander.cpp

@@ -433,7 +433,7 @@ int commander_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "takeoff")) {
 
 		/* see if we got a home position */
-		if (status_flags.condition_home_position_valid) {
+		if (status_flags.condition_local_position_valid) {
 
 			if (TRANSITION_DENIED != arm_disarm(true, &mavlink_log_pub, "command line")) {
 

src/modules/commander/state_machine_helper.cpp

@@ -428,8 +428,6 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 
 	case commander_state_s::MAIN_STATE_AUTO_MISSION:
 	case commander_state_s::MAIN_STATE_AUTO_RTL:
-	case commander_state_s::MAIN_STATE_AUTO_TAKEOFF:
-	case commander_state_s::MAIN_STATE_AUTO_LAND:
 
 		/* need global position and home position */
 		if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
@@ -438,6 +436,16 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 
 		break;
 
+	case commander_state_s::MAIN_STATE_AUTO_TAKEOFF:
+	case commander_state_s::MAIN_STATE_AUTO_LAND:
+
+		/* need local position */
+		if (status_flags->condition_local_position_valid) {
+			ret = TRANSITION_CHANGED;
+		}
+
+		break;
+
 	case commander_state_s::MAIN_STATE_OFFBOARD:
 
 		/* need offboard signal
@@ -937,8 +945,7 @@ bool set_nav_state(struct vehicle_status_s *status,
 		if (status->engine_failure) {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
 
-		} else if (status_flags->gps_failure || (!status_flags->condition_global_position_valid ||
-				!status_flags->condition_home_position_valid)) {
+		} else if (status_flags->gps_failure || !status_flags->condition_local_position_valid) {
 			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_gps);
 
 			if (status_flags->condition_local_position_valid) {
@@ -964,8 +971,7 @@ bool set_nav_state(struct vehicle_status_s *status,
 		if (status->engine_failure) {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
 
-		} else if (status_flags->gps_failure || (!status_flags->condition_global_position_valid ||
-				!status_flags->condition_home_position_valid)) {
+		} else if (status_flags->gps_failure || !status_flags->condition_local_position_valid) {
 			enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_gps);
 
 			if (status_flags->condition_local_altitude_valid) {

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;