Navigator: Handle reposition commands. Untested

src/modules/navigator/loiter.cpp

@@ -74,22 +74,43 @@ Loiter::on_inactive()
 void
 Loiter::on_activation()
 {
-	/* set current mission item to loiter */
-	set_loiter_item(&_mission_item, _param_min_alt.get());
+	if (_navigator->get_reposition_triplet()->current.valid) {
+		reposition();
+	} else {
+		/* set current mission item to loiter */
+		set_loiter_item(&_mission_item, _param_min_alt.get());
 
-	/* 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;
-	pos_sp_triplet->previous.valid = false;
-	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
-	pos_sp_triplet->next.valid = false;
+		/* 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;
+		pos_sp_triplet->previous.valid = false;
+		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+		pos_sp_triplet->next.valid = false;
 
-	_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);
+		_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);
 
-	_navigator->set_position_setpoint_triplet_updated();
+		_navigator->set_position_setpoint_triplet_updated();
+	}
 }
 
 void
 Loiter::on_active()
 {
+	if (_navigator->get_reposition_triplet()->current.valid) {
+		reposition();
+	}
+}
+
+void
+Loiter::reposition()
+{
+
+	struct position_setpoint_triplet_s *rep = _navigator->get_reposition_triplet();
+
+	if (rep->current.valid) {
+		// set loiter position based on reposition command
+
+		// mark this as done
+		memset(rep, 0, sizeof(*rep));
+	}
 }

src/modules/navigator/loiter.h

@@ -61,6 +61,12 @@ public:
 	virtual void on_active();
 
 private:
+	/**
+	 * Use the stored reposition location of the navigator
+	 * to move to a new location.
+	 */
+	void reposition();
+
 	control::BlockParamFloat _param_min_alt;
 };
 

src/modules/navigator/navigator.h

@@ -142,6 +142,7 @@ public:
 	struct home_position_s*		    get_home_position() { return &_home_pos; }
 	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 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; }
@@ -227,6 +228,7 @@ private:
 	mission_item_s 					_mission_item;		/**< current mission item */
 	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 */
 
 	mission_result_s				_mission_result;
 	geofence_result_s				_geofence_result;

src/modules/navigator/navigator_main.cpp

@@ -128,6 +128,7 @@ Navigator::Navigator() :
 	_mission_item{},
 	_nav_caps{},
 	_pos_sp_triplet{},
+	_reposition_triplet{},
 	_mission_result{},
 	_att_sp{},
 	_mission_item_valid(false),
@@ -409,6 +410,23 @@ Navigator::task_main()
 
 			if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) {
 				warnx("navigator: got reposition command");
+
+				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.lat = get_global_position()->lat;
+				rep->previous.lon = get_global_position()->lon;
+				rep->previous.alt = get_global_position()->alt;
+
+				rep->current.yaw = cmd.param4;
+				rep->current.lat = cmd.param5;
+				rep->current.lon = cmd.param6;
+				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) {