FlightTask: activate tasks with setpoints in use

src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp

@@ -100,6 +100,7 @@ void FlightTaskAutoLine::_reset()
 	// Set setpoints equal current state.
 	_velocity_setpoint = _velocity;
 	_position_setpoint = _position;
+	_yaw_setpoint = _yaw;
 	_destination = _target;
 	_origin = _prev_wp;
 	_speed_at_target = 0.0f;

src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp

@@ -43,6 +43,9 @@ using namespace matrix;
 bool FlightTaskManualAltitude::activate()
 {
 	bool ret = FlightTaskManualStabilized::activate();
+	_thrust_setpoint(2) = NAN; // altitude is controlled from position/velocity
+	_position_setpoint(2) = _position(2);
+	_velocity_setpoint(2) = 0.0f;
 	_setDefaultConstraints();
 	return ret;
 }

src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp

@@ -51,8 +51,10 @@ bool FlightTaskManualPosition::activate()
 		_constraints.speed_xy = MPC_VEL_MANUAL.get();
 	}
 
+	_position_setpoint(0) = _position(0);
+	_position_setpoint(1) = _position(1);
+	_velocity_setpoint(0) = _velocity_setpoint(1) = 0.0f;
 	_velocity_scale = _constraints.speed_xy;
-
 	return ret;
 }
 

src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp

@@ -58,6 +58,8 @@ bool FlightTaskManualStabilized::activate()
 {
 	bool ret = FlightTaskManual::activate();
 	_thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, -_throttle_hover.get());
+	_yaw_setpoint = _yaw;
+	_yawspeed_setpoint = 0.0f;
 	_constraints.tilt = math::radians(_tilt_max_man.get());
 	return ret;
 }

src/lib/FlightTasks/tasks/FlightTaskOffboard.cpp

@@ -55,6 +55,8 @@ bool FlightTaskOffboard::initializeSubscriptions(SubscriptionArray &subscription
 bool FlightTaskOffboard::activate()
 {
 	bool ret = FlightTask::activate();
+	_position_setpoint = _position;
+	_velocity_setpoint *= 0.0f;
 	_position_lock *= NAN;
 	return ret;
 }