FlightTasks: switch horizontal stick processing to Vector2f

src/lib/FlightTasks/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp

@@ -66,10 +66,13 @@ bool FlightTaskManualAcceleration::update()
 	_yaw_setpoint = _position_lock.updateYawReset(_yaw_setpoint, _sub_attitude->get().quat_reset_counter,
 			Quatf(_sub_attitude->get().delta_q_reset));
 
-	// Map sticks input to acceleration
-	_acceleration_setpoint = Vector3f(&_sticks(0));
-	_position_lock.limitStickUnitLengthXY(_acceleration_setpoint);
-	_position_lock.rotateIntoHeadingFrameXY(_acceleration_setpoint, _yaw, _yaw_setpoint);
+	// Map stick input to acceleration
+	Vector2f stick_xy(&_sticks(0));
+	_position_lock.limitStickUnitLengthXY(stick_xy);
+	_position_lock.rotateIntoHeadingFrameXY(stick_xy, _yaw, _yaw_setpoint);
+
+	_acceleration_setpoint = Vector3f(stick_xy(0), stick_xy(1), _sticks(2));
+
 	_acceleration_setpoint *= 10;
 
 	// Add drag to limit speed and brake again

src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

@@ -299,10 +299,10 @@ void FlightTaskManualAltitude::_updateSetpoints()
 	// 1 means that maximum thrust along xy is demanded. A magnitude of 0 means no
 	// thrust along xy is demanded. The maximum thrust along xy depends on the thrust
 	// setpoint along z-direction, which is computed in PositionControl.cpp.
-	_thrust_setpoint = Vector3f(&_sticks(0));
-	_position_lock.limitStickUnitLengthXY(_thrust_setpoint);
-	_position_lock.rotateIntoHeadingFrameXY(_thrust_setpoint, _yaw, _yaw_setpoint);
-	_thrust_setpoint(2) = NAN;
+	Vector2f stick_xy(&_sticks(0));
+	_position_lock.limitStickUnitLengthXY(stick_xy);
+	_position_lock.rotateIntoHeadingFrameXY(stick_xy, _yaw, _yaw_setpoint);
+	_thrust_setpoint = Vector3f(stick_xy(0), stick_xy(1), NAN);
 	_acceleration_setpoint = 10.f * _thrust_setpoint;
 
 	_updateAltitudeLock();

src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp

@@ -97,9 +97,9 @@ void FlightTaskManualPosition::_scaleSticks()
 	FlightTaskManualAltitude::_scaleSticks();
 
 	/* Constrain length of stick inputs to 1 for xy*/
-	Vector3f sp(&_sticks_expo(0));
-	_position_lock.limitStickUnitLengthXY(sp);
-	_position_lock.rotateIntoHeadingFrameXY(sp, _yaw, _yaw_setpoint);
+	Vector2f vel_sp_xy(&_sticks_expo(0));
+	_position_lock.limitStickUnitLengthXY(vel_sp_xy);
+	_position_lock.rotateIntoHeadingFrameXY(vel_sp_xy, _yaw, _yaw_setpoint);
 
 	// scale the stick inputs
 	if (PX4_ISFINITE(_sub_vehicle_local_position->get().vxy_max)) {
@@ -110,7 +110,7 @@ void FlightTaskManualPosition::_scaleSticks()
 		// Allow for a minimum of 0.3 m/s for repositioning
 		_velocity_scale = fmaxf(_velocity_scale, 0.3f);
 
-	} else if (Vector2f(sp).length() > 0.5f) {
+	} else if (vel_sp_xy.length() > 0.5f) {
 		// raise the limit at a constant rate up to the user specified value
 
 		if (_velocity_scale < _constraints.speed_xy) {
@@ -123,19 +123,16 @@ void FlightTaskManualPosition::_scaleSticks()
 	}
 
 	// scale velocity to its maximum limits
-	sp *= _velocity_scale;
+	vel_sp_xy *= _velocity_scale;
 
 	// collision prevention
 	if (_collision_prevention.is_active()) {
-		Vector2f vel_sp_xy(sp);
 		_collision_prevention.modifySetpoint(vel_sp_xy, _velocity_scale, Vector2f(_position),
 						     Vector2f(_velocity));
-		sp(0) = vel_sp_xy(0);
-		sp(1) = vel_sp_xy(1);
 	}
 
-	_velocity_setpoint(0) = sp(0);
-	_velocity_setpoint(1) = sp(1);
+	_velocity_setpoint(0) = vel_sp_xy(0);
+	_velocity_setpoint(1) = vel_sp_xy(1);
 }
 
 void FlightTaskManualPosition::_updateXYlock()

src/lib/FlightTasks/tasks/Utility/PositionLock.hpp

@@ -116,13 +116,13 @@ class PositionLock
 	 * Limit the the horizontal input from a square shaped joystick gimbal to a unit circle
 	 * @param v Vector containing x, y, z axis of the joystick gimbal. x, y get adjusted
 	 */
-	void limitStickUnitLengthXY(matrix::Vector3f &v)
+	void limitStickUnitLengthXY(matrix::Vector2f &v)
 	{
-		const matrix::Vector2f v2(v);
-		const float v2l = v2.length();
-		if (v2.length() > 1.0f) {
-			v(1) /= v2l;
-			v(2) /= v2l;
+		const float vl = v.length();
+
+		if (vl > 1.0f) {
+			v(1) /= vl;
+			v(2) /= vl;
 		}
 	}
 
@@ -132,12 +132,13 @@ class PositionLock
 	 * @param yaw Current vehicle yaw heading
 	 * @param yaw_setpoint Current yaw setpoint if it's locked else NAN
 	 */
-	void rotateIntoHeadingFrameXY(matrix::Vector3f &v, const float yaw, const float yaw_setpoint)
+	void rotateIntoHeadingFrameXY(matrix::Vector2f &v, const float yaw, const float yaw_setpoint)
 	{
 		using namespace matrix;
 		// Rotate horizontal acceleration input to body heading
+		Vector3f v3(v(0), v(1), 0.f);
 		const float yaw_rotate = PX4_ISFINITE(yaw_setpoint) ? yaw_setpoint : yaw;
-		v = Dcmf(Eulerf(0.0f, 0.0f, yaw_rotate)) * v;
+		v = Vector2f(Dcmf(Eulerf(0.0f, 0.0f, yaw_rotate)) * v3);
 	}
 
 	void setYawResetCounter(const uint8_t yaw_reset_counter) { _yaw_reset_counter = yaw_reset_counter; }