Refactor: Use new matrix::Vector2f constructor

src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp

@@ -124,7 +124,7 @@ bool FlightTaskAuto::_evaluateTriplets()
 	}
 
 	// Temporary target variable where we save the local reprojection of the latest navigator current triplet.
-	matrix::Vector3f tmp_target;
+	Vector3f tmp_target;
 
 	if (!PX4_ISFINITE(_sub_triplet_setpoint->get().current.lat)
 	    || !PX4_ISFINITE(_sub_triplet_setpoint->get().current.lon)) {
@@ -235,18 +235,18 @@ bool FlightTaskAuto::_evaluateTriplets()
 void FlightTaskAuto::_set_heading_from_mode()
 {
 
-	matrix::Vector2f v; // Vector that points towards desired location
+	Vector2f v; // Vector that points towards desired location
 
 	switch (MPC_YAW_MODE.get()) {
 
 	case 0: { // Heading points towards the current waypoint.
-			v = Vector2f(_target(0), _target(1)) - Vector2f(_position(0), _position(1));
+			v = Vector2f(_target) - Vector2f(_position);
 			break;
 		}
 
 	case 1: { // Heading points towards home.
 			if (_sub_home_position->get().valid_hpos) {
-				v = Vector2f(_sub_home_position->get().x, _sub_home_position->get().y) - Vector2f(&_position(0));
+				v = Vector2f(&_sub_home_position->get().x) - Vector2f(_position);
 			}
 
 			break;
@@ -254,7 +254,7 @@ void FlightTaskAuto::_set_heading_from_mode()
 
 	case 2: { // Heading point away from home.
 			if (_sub_home_position->get().valid_hpos) {
-				v = Vector2f(&_position(0)) - Vector2f(_sub_home_position->get().x, _sub_home_position->get().y);
+				v = Vector2f(_position) - Vector2f(&_sub_home_position->get().x);
 			}
 
 			break;
@@ -318,13 +318,13 @@ void FlightTaskAuto::_checkAvoidanceProgress()
 	pos_control_status.timestamp = hrt_absolute_time();
 
 	// vector from previous triplet to current target
-	Vector2f prev_to_target = Vector2f(_triplet_target(0) - _triplet_prev_wp(0), _triplet_target(1) - _triplet_prev_wp(1));
+	Vector2f prev_to_target = Vector2f(_triplet_target - _triplet_prev_wp);
 	// vector from previous triplet to the vehicle projected position on the line previous-target triplet
-	Vector2f prev_to_closest_pt = Vector2f(_closest_pt(0) - _triplet_prev_wp(0), _closest_pt(1) - _triplet_prev_wp(1));
+	Vector2f prev_to_closest_pt = _closest_pt - Vector2f(_triplet_prev_wp);
 	// fraction of the previous-tagerget line that has been flown
 	const float prev_curr_travelled = prev_to_closest_pt.length() / prev_to_target.length();
 
-	Vector2f pos_to_target = Vector2f(_triplet_target(0) - _position(0), _triplet_target(1) - _position(1));
+	Vector2f pos_to_target = Vector2f(_triplet_target - _position);
 
 	if (prev_curr_travelled > 1.0f) {
 		// if the vehicle projected position on the line previous-target is past the target waypoint,
@@ -412,7 +412,7 @@ void FlightTaskAuto::_setDefaultConstraints()
 	}
 }
 
-matrix::Vector2f FlightTaskAuto::_getTargetVelocityXY()
+Vector2f FlightTaskAuto::_getTargetVelocityXY()
 {
 	// guard against any bad velocity values
 	const float vx = _sub_triplet_setpoint->get().current.vx;
@@ -421,22 +421,22 @@ matrix::Vector2f FlightTaskAuto::_getTargetVelocityXY()
 			      _sub_triplet_setpoint->get().current.velocity_valid;
 
 	if (velocity_valid) {
-		return matrix::Vector2f(vx, vy);
+		return Vector2f(vx, vy);
 
 	} else {
 		// just return zero speed
-		return matrix::Vector2f{};
+		return Vector2f{};
 	}
 }
 
 State FlightTaskAuto::_getCurrentState()
 {
 	// Calculate the vehicle current state based on the Navigator triplets and the current position.
-	Vector2f u_prev_to_target = Vector2f(&(_triplet_target - _triplet_prev_wp)(0)).unit_or_zero();
-	Vector2f pos_to_target = Vector2f(&(_triplet_target - _position)(0));
-	Vector2f prev_to_pos = Vector2f(&(_position - _triplet_prev_wp)(0));
+	Vector2f u_prev_to_target = Vector2f(_triplet_target - _triplet_prev_wp).unit_or_zero();
+	Vector2f pos_to_target(_triplet_target - _position);
+	Vector2f prev_to_pos(_position - _triplet_prev_wp);
 	// Calculate the closest point to the vehicle position on the line prev_wp - target
-	_closest_pt = Vector2f(&_triplet_prev_wp(0)) + u_prev_to_target * (prev_to_pos * u_prev_to_target);
+	_closest_pt = Vector2f(_triplet_prev_wp) + u_prev_to_target * (prev_to_pos * u_prev_to_target);
 
 	State return_state = State::none;
 
@@ -448,7 +448,7 @@ State FlightTaskAuto::_getCurrentState()
 		// Current position is more than cruise speed in front of previous setpoint.
 		return_state = State::previous_infront;
 
-	} else if (Vector2f(Vector2f(&_position(0)) - _closest_pt).length() > _mc_cruise_speed) {
+	} else if (Vector2f(Vector2f(_position) - _closest_pt).length() > _mc_cruise_speed) {
 		// Vehicle is more than cruise speed off track.
 		return_state = State::offtrack;
 
@@ -481,11 +481,11 @@ void FlightTaskAuto::_updateInternalWaypoints()
 			// angle = cos(x) + 1.0
 			// angle goes from 0 to 2 with 0 = large angle, 2 = small angle:   0 = PI ; 2 = PI*0
 
-			if (Vector2f(&(_target - _next_wp)(0)).length() > 0.001f &&
-			    (Vector2f(&(_target - _prev_wp)(0)).length() > NAV_ACC_RAD.get())) {
+			if (Vector2f(_target - _next_wp).length() > 0.001f &&
+			    (Vector2f(_target - _prev_wp).length() > NAV_ACC_RAD.get())) {
 
-				angle = Vector2f(&(_target - _prev_wp)(0)).unit_or_zero()
-					* Vector2f(&(_target - _next_wp)(0)).unit_or_zero()
+				angle = Vector2f(_target - _prev_wp).unit_or_zero()
+					* Vector2f(_target - _next_wp).unit_or_zero()
 					+ 1.0f;
 				_speed_at_target = _getVelocityFromAngle(angle);
 			}
@@ -502,11 +502,11 @@ void FlightTaskAuto::_updateInternalWaypoints()
 
 			// angle = cos(x) + 1.0
 			// angle goes from 0 to 2 with 0 = large angle, 2 = small angle:   0 = PI ; 2 = PI*0
-			if (Vector2f(&(_target - _next_wp)(0)).length() > 0.001f &&
-			    (Vector2f(&(_target - _prev_wp)(0)).length() > NAV_ACC_RAD.get())) {
+			if (Vector2f(_target - _next_wp).length() > 0.001f &&
+			    (Vector2f(_target - _prev_wp).length() > NAV_ACC_RAD.get())) {
 
-				angle = Vector2f(&(_target - _prev_wp)(0)).unit_or_zero()
-					* Vector2f(&(_target - _next_wp)(0)).unit_or_zero()
+				angle = Vector2f(_target - _prev_wp).unit_or_zero()
+					* Vector2f(_target - _next_wp).unit_or_zero()
 					+ 1.0f;
 				_speed_at_target = _getVelocityFromAngle(angle);
 			}
@@ -515,7 +515,7 @@ void FlightTaskAuto::_updateInternalWaypoints()
 
 	case State::offtrack: {
 			_next_wp = _triplet_target;
-			_target = matrix::Vector3f(_closest_pt(0), _closest_pt(1), _triplet_target(2));
+			_target = Vector3f(_closest_pt(0), _closest_pt(1), _triplet_target(2));
 			_prev_wp = _position;
 
 			float angle = 2.0f;
@@ -523,11 +523,11 @@ void FlightTaskAuto::_updateInternalWaypoints()
 
 			// angle = cos(x) + 1.0
 			// angle goes from 0 to 2 with 0 = large angle, 2 = small angle:   0 = PI ; 2 = PI*0
-			if (Vector2f(&(_target - _next_wp)(0)).length() > 0.001f &&
-			    (Vector2f(&(_target - _prev_wp)(0)).length() > NAV_ACC_RAD.get())) {
+			if (Vector2f(_target - _next_wp).length() > 0.001f &&
+			    (Vector2f(_target - _prev_wp).length() > NAV_ACC_RAD.get())) {
 
-				angle = Vector2f(&(_target - _prev_wp)(0)).unit_or_zero()
-					* Vector2f(&(_target - _next_wp)(0)).unit_or_zero()
+				angle = Vector2f(_target - _prev_wp).unit_or_zero()
+					* Vector2f(_target - _next_wp).unit_or_zero()
 					+ 1.0f;
 				_speed_at_target = _getVelocityFromAngle(angle);
 			}
@@ -544,12 +544,12 @@ void FlightTaskAuto::_updateInternalWaypoints()
 
 			// angle = cos(x) + 1.0
 			// angle goes from 0 to 2 with 0 = large angle, 2 = small angle:   0 = PI ; 2 = PI*0
-			if (Vector2f(&(_target - _next_wp)(0)).length() > 0.001f &&
-			    (Vector2f(&(_target - _prev_wp)(0)).length() > NAV_ACC_RAD.get())) {
+			if (Vector2f(_target - _next_wp).length() > 0.001f &&
+			    (Vector2f(_target - _prev_wp).length() > NAV_ACC_RAD.get())) {
 
 				angle =
-					Vector2f(&(_target - _prev_wp)(0)).unit_or_zero()
-					* Vector2f(&(_target - _next_wp)(0)).unit_or_zero()
+					Vector2f(_target - _prev_wp).unit_or_zero()
+					* Vector2f(_target - _next_wp).unit_or_zero()
 					+ 1.0f;
 				_speed_at_target = _getVelocityFromAngle(angle);
 			}
@@ -562,7 +562,7 @@ void FlightTaskAuto::_updateInternalWaypoints()
 	}
 }
 
-bool FlightTaskAuto::_compute_heading_from_2D_vector(float &heading, matrix::Vector2f v)
+bool FlightTaskAuto::_compute_heading_from_2D_vector(float &heading, Vector2f v)
 {
 	if (PX4_ISFINITE(v.length()) && v.length() > SIGMA_NORM) {
 		v.normalize();

src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp

@@ -55,14 +55,14 @@ void FlightTaskAutoLine::_generateSetpoints()
 
 void FlightTaskAutoLine::_generateHeadingAlongTrack()
 {
-	Vector2f prev_to_dest = Vector2f(&(_target - _prev_wp)(0));
+	Vector2f prev_to_dest(_target - _prev_wp);
 	_compute_heading_from_2D_vector(_yaw_setpoint, prev_to_dest);
 
 }
 
 void FlightTaskAutoLine::_generateXYsetpoints()
 {
-	Vector2f pos_sp_to_dest = Vector2f(&(_target - _position_setpoint)(0));
+	Vector2f pos_sp_to_dest(_target - _position_setpoint);
 	const bool has_reached_altitude = fabsf(_target(2) - _position(2)) < NAV_ACC_RAD.get();
 
 	if ((_speed_at_target < 0.001f && pos_sp_to_dest.length() < NAV_ACC_RAD.get()) ||
@@ -76,13 +76,13 @@ void FlightTaskAutoLine::_generateXYsetpoints()
 	} else {
 
 		// Get various path specific vectors. */
-		Vector2f u_prev_to_dest = Vector2f(&(_target - _prev_wp)(0)).unit_or_zero();
-		Vector2f prev_to_pos(&(_position - _prev_wp)(0));
-		Vector2f closest_pt = Vector2f(&_prev_wp(0)) + u_prev_to_dest * (prev_to_pos * u_prev_to_dest);
-		Vector2f closest_to_dest = Vector2f(&(_target - _position)(0));
-		Vector2f prev_to_dest = Vector2f(&(_target - _prev_wp)(0));
+		Vector2f u_prev_to_dest = Vector2f(_target - _prev_wp).unit_or_zero();
+		Vector2f prev_to_pos(_position - _prev_wp);
+		Vector2f closest_pt = Vector2f(_prev_wp) + u_prev_to_dest * (prev_to_pos * u_prev_to_dest);
+		Vector2f closest_to_dest(_target - _position);
+		Vector2f prev_to_dest(_target - _prev_wp);
 		float speed_sp_track = _mc_cruise_speed;
-		float speed_sp_prev_track = math::max(Vector2f(&_velocity_setpoint(0)) * u_prev_to_dest, 0.0f);
+		float speed_sp_prev_track = math::max(Vector2f(_velocity_setpoint) * u_prev_to_dest, 0.0f);
 
 		// Distance to target when brake should occur. The assumption is made that
 		// 1.5 * cruising speed is enough to break.

src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp

@@ -143,7 +143,7 @@ void FlightTaskAutoMapper::_generateVelocitySetpoints()
 	// TODO: Remove velocity force logic from navigator, since
 	// navigator should only send out waypoints.
 	_position_setpoint = Vector3f(NAN, NAN, _position(2));
-	Vector2f vel_sp_xy = Vector2f(&_velocity(0)).unit_or_zero() * _mc_cruise_speed;
+	Vector2f vel_sp_xy = Vector2f(_velocity).unit_or_zero() * _mc_cruise_speed;
 	_velocity_setpoint = Vector3f(vel_sp_xy(0), vel_sp_xy(1), NAN);
 }
 

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

@@ -101,7 +101,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
 	// when terrain hold behaviour has been selected.
 	if (MPC_ALT_MODE.get() == 2) {
 		// Use horizontal speed as a transition criteria
-		float spd_xy = Vector2f(&_velocity(0)).length();
+		float spd_xy = Vector2f(_velocity).length();
 
 		// Use presence of horizontal stick inputs as a transition criteria
 		float stick_xy = Vector2f(&_sticks_expo(0)).length();
@@ -260,7 +260,7 @@ void FlightTaskManualAltitude::_updateSetpoints()
 	// thrust along xy is demanded. The maximum thrust along xy depends on the thrust
 	// setpoint along z-direction, which is computed in PositionControl.cpp.
 
-	Vector2f sp{_sticks(0), _sticks(1)};
+	Vector2f sp(&_sticks(0));
 	_rotateIntoHeadingFrame(sp);
 
 	if (sp.length() > 1.0f) {

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

@@ -78,7 +78,7 @@ void FlightTaskManualPosition::_scaleSticks()
 	FlightTaskManualAltitude::_scaleSticks();
 
 	/* Constrain length of stick inputs to 1 for xy*/
-	Vector2f stick_xy(_sticks_expo(0), _sticks_expo(1));
+	Vector2f stick_xy(&_sticks_expo(0));
 
 	float mag = math::constrain(stick_xy.length(), 0.0f, 1.0f);
 
@@ -119,8 +119,8 @@ void FlightTaskManualPosition::_scaleSticks()
 void FlightTaskManualPosition::_updateXYlock()
 {
 	/* If position lock is not active, position setpoint is set to NAN.*/
-	const float vel_xy_norm = Vector2f(&_velocity(0)).length();
-	const bool apply_brake = Vector2f(&_velocity_setpoint(0)).length() < FLT_EPSILON;
+	const float vel_xy_norm = Vector2f(_velocity).length();
+	const bool apply_brake = Vector2f(_velocity_setpoint).length() < FLT_EPSILON;
 	const bool stopped = (MPC_HOLD_MAX_XY.get() < FLT_EPSILON || vel_xy_norm < MPC_HOLD_MAX_XY.get());
 
 	if (apply_brake && stopped && !PX4_ISFINITE(_position_setpoint(0))) {

src/lib/FlightTasks/tasks/ManualPositionSmooth/FlightTaskManualPositionSmooth.cpp

@@ -40,7 +40,7 @@
 using namespace matrix;
 
 FlightTaskManualPositionSmooth::FlightTaskManualPositionSmooth() :
-	_smoothingXY(this, matrix::Vector2f(&_velocity(0))),
+	_smoothingXY(this, Vector2f(_velocity)),
 	_smoothingZ(this, _velocity(2), _sticks(2))
 {}
 
@@ -50,8 +50,8 @@ void FlightTaskManualPositionSmooth::_updateSetpoints()
 	FlightTaskManualPosition::_updateSetpoints();
 
 	/* Smooth velocity setpoint in xy.*/
-	matrix::Vector2f vel(&_velocity(0));
-	Vector2f vel_sp_xy = Vector2f(&_velocity_setpoint(0));
+	Vector2f vel(_velocity);
+	Vector2f vel_sp_xy(_velocity_setpoint);
 	_smoothingXY.updateMaxVelocity(_constraints.speed_xy);
 	_smoothingXY.smoothVelocity(vel_sp_xy, vel, _yaw, _yawspeed_setpoint, _deltatime);
 	_velocity_setpoint(0) = vel_sp_xy(0);

src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.cpp

@@ -99,7 +99,7 @@ void FlightTaskManualStabilized::_updateHeadingSetpoints()
 void FlightTaskManualStabilized::_updateThrustSetpoints()
 {
 	/* Rotate setpoint into local frame. */
-	Vector2f sp{_sticks(0), _sticks(1)};
+	Vector2f sp(&_sticks(0));
 	_rotateIntoHeadingFrame(sp);
 
 	/* Ensure that maximum tilt is in [0.001, Pi] */

src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp

@@ -122,7 +122,7 @@ bool FlightTaskOrbit::activate()
 	bool ret = FlightTaskManualAltitudeSmooth::activate();
 	_r = _radius_min;
 	_v =  1.f;
-	_center = Vector2f(_position.data());
+	_center = Vector2f(_position);
 	_center(0) -= _r;
 
 	// need a valid position and velocity
@@ -149,8 +149,8 @@ bool FlightTaskOrbit::update()
 	setVelocity(v);
 
 	// xy velocity to go around in a circle
-	Vector2f center_to_position = Vector2f(_position.data()) - _center;
-	Vector2f velocity_xy = Vector2f(-center_to_position(1), center_to_position(0));
+	Vector2f center_to_position = Vector2f(_position) - _center;
+	Vector2f velocity_xy(-center_to_position(1), center_to_position(0));
 	velocity_xy = velocity_xy.unit_or_zero();
 	velocity_xy *= _v;
 

src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp

@@ -40,7 +40,9 @@
 #include <mathlib/mathlib.h>
 #include <float.h>
 
-ManualSmoothingXY::ManualSmoothingXY(ModuleParams *parent, const matrix::Vector2f &vel) :
+using namespace matrix;
+
+ManualSmoothingXY::ManualSmoothingXY(ModuleParams *parent, const Vector2f &vel) :
 	ModuleParams(parent), 	_vel_sp_prev(vel)
 
 {
@@ -49,7 +51,7 @@ ManualSmoothingXY::ManualSmoothingXY(ModuleParams *parent, const matrix::Vector2
 }
 
 void
-ManualSmoothingXY::smoothVelocity(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw,
+ManualSmoothingXY::smoothVelocity(Vector2f &vel_sp, const Vector2f &vel, const float &yaw,
 				  const float &yawrate_sp, const float dt)
 {
 	_updateAcceleration(vel_sp, vel, yaw, yawrate_sp, dt);
@@ -58,7 +60,7 @@ ManualSmoothingXY::smoothVelocity(matrix::Vector2f &vel_sp, const matrix::Vector
 }
 
 void
-ManualSmoothingXY::_updateAcceleration(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel,  const float &yaw,
+ManualSmoothingXY::_updateAcceleration(Vector2f &vel_sp, const Vector2f &vel,  const float &yaw,
 				       const float &yawrate_sp, const float dt)
 {
 	Intention intention = _getIntention(vel_sp, vel, yaw, yawrate_sp);
@@ -69,7 +71,7 @@ ManualSmoothingXY::_updateAcceleration(matrix::Vector2f &vel_sp, const matrix::V
 }
 
 ManualSmoothingXY::Intention
-ManualSmoothingXY::_getIntention(const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw,
+ManualSmoothingXY::_getIntention(const Vector2f &vel_sp, const Vector2f &vel, const float &yaw,
 				 const float &yawrate_sp)
 {
 
@@ -81,8 +83,8 @@ ManualSmoothingXY::_getIntention(const matrix::Vector2f &vel_sp, const matrix::V
 		// that the user demanded a direction change.
 		// The detection is done in body frame.
 		// Rotate velocity setpoint into body frame
-		matrix::Vector2f vel_sp_heading = _getWorldToHeadingFrame(vel_sp, yaw);
-		matrix::Vector2f vel_heading = _getWorldToHeadingFrame(vel, yaw);
+		Vector2f vel_sp_heading = _getWorldToHeadingFrame(vel_sp, yaw);
+		Vector2f vel_heading = _getWorldToHeadingFrame(vel, yaw);
 
 		if (vel_sp_heading.length() > FLT_EPSILON) {
 			vel_sp_heading.normalize();
@@ -117,7 +119,7 @@ ManualSmoothingXY::_getIntention(const matrix::Vector2f &vel_sp, const matrix::V
 }
 
 void
-ManualSmoothingXY::_setStateAcceleration(const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel,
+ManualSmoothingXY::_setStateAcceleration(const Vector2f &vel_sp, const Vector2f &vel,
 		const Intention &intention, const float dt)
 {
 	switch (intention) {
@@ -194,10 +196,10 @@ ManualSmoothingXY::_setStateAcceleration(const matrix::Vector2f &vel_sp, const m
 }
 
 void
-ManualSmoothingXY::_velocitySlewRate(matrix::Vector2f &vel_sp, const float dt)
+ManualSmoothingXY::_velocitySlewRate(Vector2f &vel_sp, const float dt)
 {
 	// Adjust velocity setpoint if demand exceeds acceleration. /
-	matrix::Vector2f acc{};
+	Vector2f acc{};
 
 	if (dt > FLT_EPSILON) {
 		acc = (vel_sp - _vel_sp_prev) / dt;
@@ -208,18 +210,18 @@ ManualSmoothingXY::_velocitySlewRate(matrix::Vector2f &vel_sp, const float dt)
 	}
 }
 
-matrix::Vector2f
-ManualSmoothingXY::_getWorldToHeadingFrame(const matrix::Vector2f &vec, const float &yaw)
+Vector2f
+ManualSmoothingXY::_getWorldToHeadingFrame(const Vector2f &vec, const float &yaw)
 {
-	matrix::Quatf q_yaw = matrix::AxisAnglef(matrix::Vector3f(0.0f, 0.0f, 1.0f), yaw);
-	matrix::Vector3f vec_heading = q_yaw.conjugate_inversed(matrix::Vector3f(vec(0), vec(1), 0.0f));
-	return matrix::Vector2f(&vec_heading(0));
+	Quatf q_yaw = AxisAnglef(Vector3f(0.0f, 0.0f, 1.0f), yaw);
+	Vector3f vec_heading = q_yaw.conjugate_inversed(Vector3f(vec(0), vec(1), 0.0f));
+	return Vector2f(vec_heading);
 }
 
-matrix::Vector2f
-ManualSmoothingXY::_getHeadingToWorldFrame(const matrix::Vector2f &vec, const float &yaw)
+Vector2f
+ManualSmoothingXY::_getHeadingToWorldFrame(const Vector2f &vec, const float &yaw)
 {
-	matrix::Quatf q_yaw = matrix::AxisAnglef(matrix::Vector3f(0.0f, 0.0f, 1.0f), yaw);
-	matrix::Vector3f vec_world = q_yaw.conjugate(matrix::Vector3f(vec(0), vec(1), 0.0f));
-	return matrix::Vector2f(&vec_world(0));
+	Quatf q_yaw = AxisAnglef(Vector3f(0.0f, 0.0f, 1.0f), yaw);
+	Vector3f vec_world = q_yaw.conjugate(Vector3f(vec(0), vec(1), 0.0f));
+	return Vector2f(vec_world);
 }

src/lib/FlightTasks/tasks/Utility/StraightLine.cpp

@@ -113,7 +113,7 @@ float StraightLine::getMaxAcc()
 	Vector3f u_orig_to_target = (_target - _origin).unit_or_zero();
 
 	// calculate the maximal horizontal acceleration
-	float divider = Vector2f(u_orig_to_target(0), u_orig_to_target(1)).length();
+	float divider = Vector2f(u_orig_to_target).length();
 	float max_acc_hor = MPC_ACC_HOR_MAX.get();
 
 	if (divider > FLT_EPSILON) {
@@ -148,7 +148,7 @@ float StraightLine::getMaxVel()
 	Vector3f u_orig_to_target = (_target - _origin).unit_or_zero();
 
 	// calculate the maximal horizontal velocity
-	float divider = Vector2f(u_orig_to_target.data()).length();
+	float divider = Vector2f(u_orig_to_target).length();
 	float max_vel_hor = MPC_XY_VEL_MAX.get();
 
 	if (divider > FLT_EPSILON) {