NPFG: use cross product for matrix library

This removes the use of cross products from NPFG and takes advantage of the matrix library

src/lib/npfg/npfg.cpp

@@ -58,7 +58,7 @@ void NPFG::guideToPath(const Vector2f &ground_vel, const Vector2f &wind_vel, con
 	const float wind_speed = wind_vel.norm();
 
 	// on-track wind triangle projections
-	const float wind_cross_upt = cross2D(wind_vel, unit_path_tangent);
+	const float wind_cross_upt = wind_vel.cross(unit_path_tangent);
 	const float wind_dot_upt = wind_vel.dot(unit_path_tangent);
 
 	// calculate the bearing feasibility on the track at the current closest point
@@ -85,7 +85,7 @@ void NPFG::guideToPath(const Vector2f &ground_vel, const Vector2f &wind_vel, con
 	bearing_vec_ = bearingVec(unit_path_tangent, look_ahead_ang, signed_track_error);
 
 	// wind triangle projections
-	const float wind_cross_bearing = cross2D(wind_vel, bearing_vec_);
+	const float wind_cross_bearing = wind_vel.cross(bearing_vec_);
 	const float wind_dot_bearing = wind_vel.dot(bearing_vec_);
 
 	// continuous representation of the bearing feasibility
@@ -130,7 +130,7 @@ void NPFG::guideToPoint(const Vector2f &ground_vel, const Vector2f &wind_vel, co
 	const float wind_speed = wind_vel.norm();
 
 	// wind triangle projections
-	const float wind_cross_bearing = cross2D(wind_vel, bearing_vec);
+	const float wind_cross_bearing = wind_vel.cross(bearing_vec);
 	const float wind_dot_bearing = wind_vel.dot(bearing_vec);
 
 	// continuous representation of the bearing feasibility
@@ -501,7 +501,7 @@ float NPFG::lateralAccel(const Vector2f &air_vel, const Vector2f &air_vel_ref, c
 	// lateral acceleration demand only from the heading error
 
 	const float dot_air_vel_err = air_vel.dot(air_vel_ref);
-	const float cross_air_vel_err = cross2D(air_vel, air_vel_ref);
+	const float cross_air_vel_err = air_vel.cross(air_vel_ref);
 
 	if (dot_air_vel_err < 0.0f) {
 		// hold max lateral acceleration command above 90 deg heading error
@@ -544,7 +544,7 @@ void NPFG::navigateWaypoints(const Vector2d &waypoint_A, const Vector2d &waypoin
 
 		// guidance to the line through A and B
 		unit_path_tangent_ = vector_A_to_B.normalized();
-		signed_track_error_ = cross2D(unit_path_tangent_, vector_A_to_vehicle);
+		signed_track_error_ = unit_path_tangent_.cross(vector_A_to_vehicle);
 		guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, 0.0f);
 
 		const Vector2f bearing_vec_to_point = -vector_A_to_vehicle.normalized();
@@ -566,7 +566,7 @@ void NPFG::navigateWaypoints(const Vector2d &waypoint_A, const Vector2d &waypoin
 	} else {
 		// track the line segment between A and B
 		unit_path_tangent_ = vector_A_to_B.normalized();
-		signed_track_error_ = cross2D(unit_path_tangent_, vector_A_to_vehicle);
+		signed_track_error_ = unit_path_tangent_.cross(vector_A_to_vehicle);
 		guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, 0.0f);
 	}
 
@@ -629,7 +629,7 @@ void NPFG::navigatePathTangent(const matrix::Vector2d &vehicle_pos, const matrix
 
 	// closest point to vehicle
 	matrix::Vector2f error_vector = getLocalPlanarVector(position_setpoint, vehicle_pos);
-	signed_track_error_ = cross2D(unit_path_tangent_, error_vector);
+	signed_track_error_ = unit_path_tangent_.cross(error_vector);
 
 	guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, curvature);
 

src/lib/npfg/npfg.hpp

@@ -717,16 +717,6 @@ private:
 	float lateralAccel(const matrix::Vector2f &air_vel, const matrix::Vector2f &air_vel_ref,
 			   const float airspeed) const;
 
-	/*
-	 * Calculates two-dimensional "cross product" of two vectors.
-	 * TODO: move to matrix lib (Vector2 operation)
-	 *
-	 * @param[in] vec_1 Vector 1
-	 * @param[in] vec_2 Vector 2
-	 * @return 2D cross product
-	 */
-	float cross2D(const matrix::Vector2f &vec_1, const matrix::Vector2f &vec_2) const { return vec_1(0) * vec_2(1) - vec_1(1) * vec_2(0); }
-
 	/*******************************************************************************
 	 * PX4 POSITION SETPOINT INTERFACE FUNCTIONS
 	 */