mc_pos_control: set previous triplet point to invalid when switching to manual

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -753,13 +753,19 @@ MulticopterPositionControl::poll_subscriptions()
 	if (updated) {
 		orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
 
-		//set current position setpoint invalid if none of them (lat, lon and alt) is finite
-		if (!PX4_ISFINITE(_pos_sp_triplet.current.lat) &&
-		    !PX4_ISFINITE(_pos_sp_triplet.current.lon) &&
-		    !PX4_ISFINITE(_pos_sp_triplet.current.alt)) {
-			_pos_sp_triplet.current.valid = false;
+		/* we need either a valid position setpoint or a valid velocity setpoint */
+		_pos_sp_triplet.current.valid = false;
+		_pos_sp_triplet.previous.valid = false;
+
+		if (PX4_ISFINITE(_pos_sp_triplet.current.lat) && PX4_ISFINITE(_pos_sp_triplet.current.lon)
+		    && PX4_ISFINITE(_pos_sp_triplet.current.alt)) {
+			_pos_sp_triplet.current.valid = true;
 		}
 
+		if (PX4_ISFINITE(_pos_sp_triplet.previous.lat) && PX4_ISFINITE(_pos_sp_triplet.previous.lon)
+		    && PX4_ISFINITE(_pos_sp_triplet.previous.alt)) {
+			_pos_sp_triplet.previous.valid = true;
+		}
 	}
 
 	orb_check(_home_pos_sub, &updated);
@@ -1999,6 +2005,7 @@ MulticopterPositionControl::do_control(float dt)
 		 * controller will not use the old triplets but waits until triplets
 		 * have been updated */
 		_pos_sp_triplet.current.valid = false;
+		_pos_sp_triplet.previous.valid = false;
 
 		_hold_offboard_xy = false;
 		_hold_offboard_z = false;