FlightTaskAuto: use longerThan() when possible in vector calculations

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp

@@ -40,8 +40,6 @@
 
 using namespace matrix;
 
-static constexpr float SIGMA_NORM	= 0.001f;
-
 FlightTaskAuto::FlightTaskAuto() :
 	_obstacle_avoidance(this),
 	_sticks(this),
@@ -536,17 +534,18 @@ void FlightTaskAuto::_set_heading_from_mode()
 		break;
 	}
 
-	if (PX4_ISFINITE(v.length())) {
+	if (PX4_ISFINITE(v.norm_squared())) {
 		// We only adjust yaw if vehicle is outside of acceptance radius. Once we enter acceptance
 		// radius, lock yaw to current yaw.
 		// This prevents excessive yawing.
 		if (!_yaw_lock) {
-			if (v.length() < _target_acceptance_radius) {
+			if (v.longerThan(_target_acceptance_radius)) {
+				_compute_heading_from_2D_vector(_yaw_setpoint, v);
+
+			} else {
 				_yaw_setpoint = _yaw;
 				_yaw_lock = true;
 
-			} else {
-				_compute_heading_from_2D_vector(_yaw_setpoint, v);
 			}
 		}
 
@@ -630,11 +629,11 @@ State FlightTaskAuto::_getCurrentState()
 		// Target is behind.
 		return_state = State::target_behind;
 
-	} else if (u_prev_to_target * prev_to_pos < 0.0f && prev_to_pos.length() > _target_acceptance_radius) {
+	} else if (u_prev_to_target * prev_to_pos < 0.0f && prev_to_pos.longerThan(_target_acceptance_radius)) {
 		// Current position is more than cruise speed in front of previous setpoint.
 		return_state = State::previous_infront;
 
-	} else if ((_position - _closest_pt).length() > _target_acceptance_radius) {
+	} else if ((_position - _closest_pt).longerThan(_target_acceptance_radius)) {
 		// Vehicle is more than cruise speed off track.
 		return_state = State::offtrack;
 
@@ -683,13 +682,13 @@ void FlightTaskAuto::_updateInternalWaypoints()
 
 bool FlightTaskAuto::_compute_heading_from_2D_vector(float &heading, Vector2f v)
 {
-	if (PX4_ISFINITE(v.length()) && v.length() > SIGMA_NORM) {
+	if (PX4_ISFINITE(v.norm_squared()) && v.longerThan(1e-3f)) {
 		v.normalize();
 		// To find yaw: take dot product of x = (1,0) and v
 		// and multiply by the sign given of cross product of x and v.
 		// Dot product: (x(0)*v(0)+(x(1)*v(1)) = v(0)
 		// Cross product: x(0)*v(1) - v(0)*x(1) = v(1)
-		heading =  sign(v(1)) * wrap_pi(acosf(v(0)));
+		heading = sign(v(1)) * wrap_pi(acosf(v(0)));
 		return true;
 	}
 
@@ -758,8 +757,8 @@ bool FlightTaskAuto::_generateHeadingAlongTraj()
 	Vector2f vel_sp_xy(_velocity_setpoint);
 	Vector2f traj_to_target = Vector2f(_target) - Vector2f(_position);
 
-	if ((vel_sp_xy.length() > .1f) &&
-	    (traj_to_target.length() > 2.f)) {
+	if ((vel_sp_xy.longerThan(.1f)) &&
+	    (traj_to_target.longerThan(2.f))) {
 		// Generate heading from velocity vector, only if it is long enough
 		// and if the drone is far enough from the target
 		_compute_heading_from_2D_vector(_yaw_setpoint, vel_sp_xy);