Navigator: differentiate between takeoff and reposition commands, perform calculations for repositions only when armed.

src/modules/navigator/loiter.cpp

@@ -58,7 +58,8 @@
 
 Loiter::Loiter(Navigator *navigator, const char *name) :
 	MissionBlock(navigator, name),
-	_param_min_alt(this, "MIS_LTRMIN_ALT", false)
+	_param_min_alt(this, "MIS_LTRMIN_ALT", false),
+	_loiter_pos_set(false)
 {
 	// load initial params
 	updateParams();
@@ -71,6 +72,7 @@ Loiter::~Loiter()
 void
 Loiter::on_inactive()
 {
+	_loiter_pos_set = false;
 }
 
 void
@@ -79,6 +81,34 @@ Loiter::on_activation()
 	if (_navigator->get_reposition_triplet()->current.valid) {
 		reposition();
 	} else {
+		set_loiter_position();
+	}
+}
+
+void
+Loiter::on_active()
+{
+	if (_navigator->get_reposition_triplet()->current.valid) {
+		reposition();
+	}
+
+	// reset the loiter position if we get disarmed
+	if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
+		_loiter_pos_set = false;
+	}
+}
+
+void
+Loiter::set_loiter_position()
+{
+	// not setting loiter position until armed
+	if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED ||
+		_loiter_pos_set) {
+		return;
+	} else {
+		_loiter_pos_set = true;
+	}
+
 	// set current mission item to loiter
 	set_loiter_item(&_mission_item, _param_min_alt.get());
 
@@ -92,20 +122,15 @@ Loiter::on_activation()
 	_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);
 
 	_navigator->set_position_setpoint_triplet_updated();
-	}
-}
-
-void
-Loiter::on_active()
-{
-	if (_navigator->get_reposition_triplet()->current.valid) {
-		reposition();
-	}
 }
 
 void
 Loiter::reposition()
 {
+	// we can't reposition if we are not armed yet
+	if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
+		return;
+	}
 
 	struct position_setpoint_triplet_s *rep = _navigator->get_reposition_triplet();
 
@@ -115,7 +140,10 @@ Loiter::reposition()
 		// convert mission item to current setpoint
 		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 		pos_sp_triplet->current.velocity_valid = false;
-		memcpy(&pos_sp_triplet->previous, &rep->previous, sizeof(rep->previous));
+		pos_sp_triplet->previous.yaw = _navigator->get_global_position()->yaw;
+		pos_sp_triplet->previous.lat = _navigator->get_global_position()->lat;
+		pos_sp_triplet->previous.lon = _navigator->get_global_position()->lon;
+		pos_sp_triplet->previous.alt = _navigator->get_global_position()->alt;
 		memcpy(&pos_sp_triplet->current, &rep->current, sizeof(rep->current));
 		pos_sp_triplet->next.valid = false;
 

src/modules/navigator/loiter.h

@@ -67,7 +67,13 @@ private:
 	 */
 	void reposition();
 
+	/**
+	 * Set the position to hold based on the current local position
+	 */
+	void set_loiter_position();
+
 	control::BlockParamFloat _param_min_alt;
+	bool _loiter_pos_set;
 };
 
 #endif

src/modules/navigator/navigator.h

@@ -143,6 +143,7 @@ public:
 	bool				    home_position_valid() { return (_home_pos.timestamp > 0); }
 	struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; }
 	struct position_setpoint_triplet_s* get_reposition_triplet() { return &_reposition_triplet; }
+	struct position_setpoint_triplet_s* get_takeoff_triplet() { return &_takeoff_triplet; }
 	struct mission_result_s*	    get_mission_result() { return &_mission_result; }
 	struct geofence_result_s*		    get_geofence_result() { return &_geofence_result; }
 	struct vehicle_attitude_setpoint_s* get_att_sp() { return &_att_sp; }
@@ -231,6 +232,7 @@ private:
 	navigation_capabilities_s			_nav_caps;		/**< navigation capabilities */
 	position_setpoint_triplet_s			_pos_sp_triplet;	/**< triplet of position setpoints */
 	position_setpoint_triplet_s			_reposition_triplet;	/**< triplet for non-mission direct position command */
+	position_setpoint_triplet_s			_takeoff_triplet;	/**< triplet for non-mission direct takeoff command */
 
 	mission_result_s				_mission_result;
 	geofence_result_s				_geofence_result;

src/modules/navigator/navigator_main.cpp

@@ -129,6 +129,7 @@ Navigator::Navigator() :
 	_nav_caps{},
 	_pos_sp_triplet{},
 	_reposition_triplet{},
+	_takeoff_triplet{},
 	_mission_result{},
 	_att_sp{},
 	_mission_item_valid(false),
@@ -415,7 +416,7 @@ Navigator::task_main()
 				struct position_setpoint_triplet_s *rep = get_reposition_triplet();
 
 				// store current position as previous position and goal as next
-				rep->previous.yaw = NAN;
+				rep->previous.yaw = get_global_position()->yaw;
 				rep->previous.lat = get_global_position()->lat;
 				rep->previous.lon = get_global_position()->lon;
 				rep->previous.alt = get_global_position()->alt;
@@ -448,9 +449,28 @@ Navigator::task_main()
 				rep->previous.valid = true;
 				rep->current.valid = true;
 				rep->next.valid = false;
-			}
+			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) {
+				struct position_setpoint_triplet_s *rep = get_takeoff_triplet();
+
+				// store current position as previous position and goal as next
+				rep->previous.yaw = get_global_position()->yaw;
+				rep->previous.lat = get_global_position()->lat;
+				rep->previous.lon = get_global_position()->lon;
+				rep->previous.alt = get_global_position()->alt;
+
+				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;
+				rep->current.lat = cmd.param5 / (double)1e7;
+				rep->current.lon = cmd.param6 / (double)1e7;
+				rep->current.alt = cmd.param7;
+
+				rep->previous.valid = true;
+				rep->current.valid = true;
+				rep->next.valid = false;
 
-			if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_PAUSE_CONTINUE) {
+			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_PAUSE_CONTINUE) {
 				warnx("navigator: got pause/continue command");
 			}
 		}

src/modules/navigator/takeoff.cpp

@@ -57,7 +57,7 @@ Takeoff::Takeoff(Navigator *navigator, const char *name) :
 	MissionBlock(navigator, name),
 	_param_min_alt(this, "MIS_TAKEOFF_ALT", false)
 {
-	/* load initial params */
+	// load initial params
 	updateParams();
 }
 
@@ -72,6 +72,32 @@ Takeoff::on_inactive()
 
 void
 Takeoff::on_activation()
+{
+	set_takeoff_position();
+}
+
+void
+Takeoff::on_active()
+{
+	struct position_setpoint_triplet_s *rep = _navigator->get_takeoff_triplet();
+	if (rep->current.valid) {
+		// reset the position
+		set_takeoff_position();
+
+	} else if (is_mission_item_reached() && !_navigator->get_mission_result()->finished) {
+		_navigator->get_mission_result()->finished = true;
+		_navigator->set_mission_result_updated();
+
+		// set loiter item so position controllers stop doing takeoff logic
+		set_loiter_item(&_mission_item);
+		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+		_navigator->set_position_setpoint_triplet_updated();
+	}
+}
+
+void
+Takeoff::set_takeoff_position()
 {
 	// set current mission item to takeoff
 	set_takeoff_item(&_mission_item, _param_min_alt.get());
@@ -89,9 +115,22 @@ Takeoff::on_activation()
 	pos_sp_triplet->next.valid = false;
 
 	// check if a specific target altitude has been set
-	struct position_setpoint_triplet_s *rep = _navigator->get_reposition_triplet();
+	struct position_setpoint_triplet_s *rep = _navigator->get_takeoff_triplet();
-	if (rep->current.valid && PX4_ISFINITE(rep->current.alt)) {
+	if (rep->current.valid) {
+		if (PX4_ISFINITE(rep->current.alt)) {
 			pos_sp_triplet->current.alt = rep->current.alt;
+		}
+
+		// Go on and check which changes had been requested
+		if (PX4_ISFINITE(rep->current.yaw)) {
+			pos_sp_triplet->current.yaw = rep->current.yaw;
+		}
+
+		if (PX4_ISFINITE(rep->current.lat) && PX4_ISFINITE(rep->current.lon)) {
+			pos_sp_triplet->current.lat = rep->current.lat;
+			pos_sp_triplet->current.lon = rep->current.lon;
+		}
+
 		// mark this as done
 		memset(rep, 0, sizeof(*rep));
 	}
@@ -100,18 +139,3 @@ Takeoff::on_activation()
 
 	_navigator->set_position_setpoint_triplet_updated();
 }
-
-void
-Takeoff::on_active()
-{
-	if (is_mission_item_reached() && !_navigator->get_mission_result()->finished) {
-		_navigator->get_mission_result()->finished = true;
-		_navigator->set_mission_result_updated();
-
-		/* set loiter item so position controllers stop doing takeoff logic */
-		set_loiter_item(&_mission_item);
-		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
-		_navigator->set_position_setpoint_triplet_updated();
-	}
-}

src/modules/navigator/takeoff.h

@@ -62,6 +62,8 @@ public:
 
 private:
 	control::BlockParamFloat _param_min_alt;
+
+	void set_takeoff_position();
 };
 
 #endif