diff --git a/src/lib/npfg/npfg.cpp b/src/lib/npfg/npfg.cpp index a54c268b5c..afb224388b 100644 --- a/src/lib/npfg/npfg.cpp +++ b/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(x_angle * CONSTANTS_RADIUS_OF_EARTH), - static_cast(y_angle *x_origin_cos * CONSTANTS_RADIUS_OF_EARTH), - }; -} // getLocalPlanarVector - void NPFG::updateRollSetpoint() { float roll_new = atanf(lateral_accel_ * 1.0f / CONSTANTS_ONE_G); diff --git a/src/lib/npfg/npfg.hpp b/src/lib/npfg/npfg.hpp index b7c1048024..2e75164dc5 100644 --- a/src/lib/npfg/npfg.hpp +++ b/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] * diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index e0fc1c2427..59a7bec55c 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/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(); }