Convert NPFG to local coordinates

src/lib/npfg/npfg.cpp

@@ -518,8 +518,8 @@ float NPFG::lateralAccel(const Vector2f &air_vel, const Vector2f &air_vel_ref, c
  * PX4 NAVIGATION INTERFACE FUNCTIONS (provide similar functionality to ECL_L1_Pos_Controller)
  */
 
-void NPFG::navigateWaypoints(const Vector2d &waypoint_A, const Vector2d &waypoint_B,
-			     const Vector2d &vehicle_pos, const Vector2f &ground_vel, const Vector2f &wind_vel)
+void NPFG::navigateWaypoints(const Vector2f &waypoint_A, const Vector2f &waypoint_B,
+			     const Vector2f &vehicle_pos, const Vector2f &ground_vel, const Vector2f &wind_vel)
 {
 	// similar to logic found in ECL_L1_Pos_Controller method of same name
 	// BUT no arbitrary max approach angle, approach entirely determined by generated
@@ -527,8 +527,8 @@ void NPFG::navigateWaypoints(const Vector2d &waypoint_A, const Vector2d &waypoin
 
 	path_type_loiter_ = false;
 
-	Vector2f vector_A_to_B = getLocalPlanarVector(waypoint_A, waypoint_B);
-	Vector2f vector_A_to_vehicle = getLocalPlanarVector(waypoint_A, vehicle_pos);
+	Vector2f vector_A_to_B = waypoint_B - waypoint_A;
+	Vector2f vector_A_to_vehicle = vehicle_pos - waypoint_A;
 
 	if (vector_A_to_B.norm() < NPFG_EPSILON) {
 		// the waypoints are on top of each other and should be considered as a
@@ -573,14 +573,14 @@ void NPFG::navigateWaypoints(const Vector2d &waypoint_A, const Vector2d &waypoin
 	updateRollSetpoint();
 } // navigateWaypoints
 
-void NPFG::navigateLoiter(const Vector2d &loiter_center, const Vector2d &vehicle_pos,
+void NPFG::navigateLoiter(const Vector2f &loiter_center, const Vector2f &vehicle_pos,
 			  float radius, int8_t loiter_direction, const Vector2f &ground_vel, const Vector2f &wind_vel)
 {
 	path_type_loiter_ = true;
 
 	radius = math::max(radius, MIN_RADIUS);
 
-	Vector2f vector_center_to_vehicle = getLocalPlanarVector(loiter_center, vehicle_pos);
+	Vector2f vector_center_to_vehicle = vehicle_pos - loiter_center;
 	const float dist_to_center = vector_center_to_vehicle.norm();
 
 	// find the direction from the circle center to the closest point on its perimeter
@@ -618,7 +618,7 @@ void NPFG::navigateLoiter(const Vector2d &loiter_center, const Vector2d &vehicle
 } // navigateLoiter
 
 
-void NPFG::navigatePathTangent(const matrix::Vector2d &vehicle_pos, const matrix::Vector2d &position_setpoint,
+void NPFG::navigatePathTangent(const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &position_setpoint,
 			       const matrix::Vector2f &tangent_setpoint,
 			       const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel, const float &curvature)
 {
@@ -628,7 +628,7 @@ void NPFG::navigatePathTangent(const matrix::Vector2d &vehicle_pos, const matrix
 	unit_path_tangent_ = tangent_setpoint.normalized();
 
 	// closest point to vehicle
-	matrix::Vector2f error_vector = getLocalPlanarVector(position_setpoint, vehicle_pos);
+	matrix::Vector2f error_vector = vehicle_pos - position_setpoint;
 	signed_track_error_ = unit_path_tangent_.cross(error_vector);
 
 	guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, curvature);
@@ -684,19 +684,6 @@ float NPFG::switchDistance(float wp_radius) const
 	return math::min(wp_radius, track_error_bound_ * switch_distance_multiplier_);
 } // switchDistance
 
-Vector2f NPFG::getLocalPlanarVector(const Vector2d &origin, const Vector2d &target) const
-{
-	/* this is an approximation for small angles, proposed by [2] */
-	const double x_angle = math::radians(target(0) - origin(0));
-	const double y_angle = math::radians(target(1) - origin(1));
-	const double x_origin_cos = cos(math::radians(origin(0)));
-
-	return Vector2f{
-		static_cast<float>(x_angle * CONSTANTS_RADIUS_OF_EARTH),
-		static_cast<float>(y_angle *x_origin_cos * CONSTANTS_RADIUS_OF_EARTH),
-	};
-} // getLocalPlanarVector
-
 void NPFG::updateRollSetpoint()
 {
 	float roll_new = atanf(lateral_accel_ * 1.0f / CONSTANTS_ONE_G);

src/lib/npfg/npfg.hpp

@@ -228,8 +228,8 @@ public:
 	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
 	 * @param[in] wind_vel Wind velocity vector [m/s]
 	 */
-	void navigateWaypoints(const matrix::Vector2d &waypoint_A, const matrix::Vector2d &waypoint_B,
-			       const matrix::Vector2d &vehicle_pos, const matrix::Vector2f &ground_vel,
+	void navigateWaypoints(const matrix::Vector2f &waypoint_A, const matrix::Vector2f &waypoint_B,
+			       const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &ground_vel,
 			       const matrix::Vector2f &wind_vel);
 
 	/*
@@ -244,7 +244,7 @@ public:
 	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
 	 * @param[in] wind_vel Wind velocity vector [m/s]
 	 */
-	void navigateLoiter(const matrix::Vector2d &loiter_center, const matrix::Vector2d &vehicle_pos,
+	void navigateLoiter(const matrix::Vector2f &loiter_center, const matrix::Vector2f &vehicle_pos,
 			    float radius, int8_t loiter_direction, const matrix::Vector2f &ground_vel,
 			    const matrix::Vector2f &wind_vel);
 
@@ -259,7 +259,7 @@ public:
 	 * @param[in] wind_vel Wind velocity vector [m/s]
 	 * @param[in] curvature of the path setpoint [1/m]
 	 */
-	void navigatePathTangent(const matrix::Vector2d &vehicle_pos, const matrix::Vector2d &position_setpoint,
+	void navigatePathTangent(const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &position_setpoint,
 				 const matrix::Vector2f &tangent_setpoint,
 				 const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel, const float &curvature);
 
@@ -721,21 +721,6 @@ private:
 	 * PX4 POSITION SETPOINT INTERFACE FUNCTIONS
 	 */
 
-	/**
-	 * [Copied directly from ECL_L1_Pos_Controller]
-	 *
-	 * Convert a 2D vector from WGS84 to planar coordinates.
-	 *
-	 * This converts from latitude and longitude to planar
-	 * coordinates with (0,0) being at the position of ref and
-	 * returns a vector in meters towards wp.
-	 *
-	 * @param ref The reference position in WGS84 coordinates
-	 * @param wp The point to convert to into the local coordinates, in WGS84 coordinates
-	 * @return The vector in meters pointing from the reference position to the coordinates
-	 */
-	matrix::Vector2f getLocalPlanarVector(const matrix::Vector2d &origin, const matrix::Vector2d &target) const;
-
 	/**
 	 * [Copied directly from ECL_L1_Pos_Controller]
 	 *

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -1187,6 +1187,9 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const fl
 	}
 
 	float target_airspeed = get_auto_airspeed_setpoint(now, pos_sp_curr.cruising_speed, ground_speed, dt);
+	Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
+	Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
+	Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0), prev_wp(1));
 
 	if (_param_fw_use_npfg.get()) {
 		_npfg.setAirspeedNom(target_airspeed * _eas2tas);
@@ -1197,20 +1200,17 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const fl
 			matrix::Vector2f velocity_2d(pos_sp_curr.vx, pos_sp_curr.vy);
 			float curvature = PX4_ISFINITE(_pos_sp_triplet.current.loiter_radius) ? 1 / _pos_sp_triplet.current.loiter_radius :
 					  0.0f;
-			_npfg.navigatePathTangent(curr_pos, curr_wp, velocity_2d.normalized(), get_nav_speed_2d(ground_speed), _wind_vel,
-						  curvature);
+			_npfg.navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), get_nav_speed_2d(ground_speed),
+						  _wind_vel, curvature);
 
 		} else {
-			_npfg.navigateWaypoints(prev_wp, curr_wp, curr_pos, get_nav_speed_2d(ground_speed), _wind_vel);
+			_npfg.navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, get_nav_speed_2d(ground_speed), _wind_vel);
 		}
 
 		_att_sp.roll_body = _npfg.getRollSetpoint();
 		target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
 
 	} else {
-		Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
-		Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
-		Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0), prev_wp(1));
 		_l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, get_nav_speed_2d(ground_speed));
 		_att_sp.roll_body = _l1_control.get_roll_setpoint();
 	}
@@ -1372,16 +1372,18 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const floa
 
 	float target_airspeed = get_auto_airspeed_setpoint(now, airspeed_sp, ground_speed, dt);
 
+	Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
+	Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
+
 	if (_param_fw_use_npfg.get()) {
 		_npfg.setAirspeedNom(target_airspeed * _eas2tas);
 		_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
-		_npfg.navigateLoiter(curr_wp, curr_pos, loiter_radius, loiter_direction, get_nav_speed_2d(ground_speed), _wind_vel);
+		_npfg.navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, loiter_direction, get_nav_speed_2d(ground_speed),
+				     _wind_vel);
 		_att_sp.roll_body = _npfg.getRollSetpoint();
 		target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
 
 	} else {
-		Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
-		Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
 		_l1_control.navigate_loiter(curr_wp_local, curr_pos_local, loiter_radius, loiter_direction,
 					    get_nav_speed_2d(ground_speed));
 		_att_sp.roll_body = _l1_control.get_roll_setpoint();
@@ -1478,18 +1480,19 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 		 * Update navigation: _runway_takeoff returns the start WP according to mode and phase.
 		 * If we use the navigator heading or not is decided later.
 		 */
+		Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
+		Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
+		Vector2f prev_wp_local = _global_local_proj_ref.project(_runway_takeoff.getStartWP()(0),
+					 _runway_takeoff.getStartWP()(1));
+
 		if (_param_fw_use_npfg.get()) {
 			_npfg.setAirspeedNom(target_airspeed * _eas2tas);
 			_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
-			_npfg.navigateWaypoints(_runway_takeoff.getStartWP(), curr_wp, curr_pos, ground_speed, _wind_vel);
+			_npfg.navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
 			_att_sp.roll_body = _runway_takeoff.getRoll(_npfg.getRollSetpoint());
 			target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
 
 		} else {
-			Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
-			Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
-			Vector2f prev_wp_local = _global_local_proj_ref.project(_runway_takeoff.getStartWP()(0),
-						 _runway_takeoff.getStartWP()(1));
 			_l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed);
 			_att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.get_roll_setpoint());
 		}
@@ -1552,18 +1555,19 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 			float target_airspeed = get_auto_airspeed_setpoint(now, _param_fw_airspd_trim.get(), ground_speed,
 						dt);
 
+			Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
+			Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
+			Vector2f prev_wp_local = _global_local_proj_ref.project(_runway_takeoff.getStartWP()(0),
+						 _runway_takeoff.getStartWP()(1));
+
 			if (_param_fw_use_npfg.get()) {
 				_npfg.setAirspeedNom(target_airspeed * _eas2tas);
 				_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
-				_npfg.navigateWaypoints(prev_wp, curr_wp, curr_pos, ground_speed, _wind_vel);
+				_npfg.navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
 				_att_sp.roll_body = _npfg.getRollSetpoint();
 				target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
 
 			} else {
-				Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
-				Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
-				Vector2f prev_wp_local = _global_local_proj_ref.project(_runway_takeoff.getStartWP()(0),
-							 _runway_takeoff.getStartWP()(1));
 				_l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed);
 				_att_sp.roll_body = _l1_control.get_roll_setpoint();
 			}
@@ -1807,8 +1811,12 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
 				_npfg.navigateHeading(_target_bearing, ground_speed, _wind_vel);
 
 			} else {
+				Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
+				Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
+				Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0),
+							 prev_wp(1));
 				// normal navigation
-				_npfg.navigateWaypoints(prev_wp, curr_wp, curr_pos, ground_speed, _wind_vel);
+				_npfg.navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
 			}
 
 			target_airspeed = _npfg.getAirspeedRef() /  _eas2tas;
@@ -1927,7 +1935,12 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
 
 			} else {
 				// normal navigation
-				_npfg.navigateWaypoints(prev_wp, curr_wp, curr_pos, ground_speed, _wind_vel);
+				Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
+				Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
+				Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0),
+							 prev_wp(1));
+				// normal navigation
+				_npfg.navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
 			}
 
 			target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
@@ -2099,19 +2112,20 @@ FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const
 			Vector2d prev_wp{_hdg_hold_prev_wp.lat, _hdg_hold_prev_wp.lon};
 			Vector2d curr_wp{_hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon};
 
+			Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
+			Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
+			Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0),
+						 prev_wp(1));
+
 			if (_param_fw_use_npfg.get()) {
 				_npfg.setAirspeedNom(target_airspeed * _eas2tas);
 				_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
-				_npfg.navigateWaypoints(prev_wp, curr_wp, curr_pos, ground_speed, _wind_vel);
+				_npfg.navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
 				_att_sp.roll_body = _npfg.getRollSetpoint();
 				target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
 
 			} else {
 				/* populate l1 control setpoint */
-				Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
-				Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
-				Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0),
-							 prev_wp(1));
 				_l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed);
 				_att_sp.roll_body = _l1_control.get_roll_setpoint();
 			}