From 2586900c2638c9a1dc44fad9c45f34db1d26d0d9 Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Tue, 22 Nov 2022 08:19:38 +0100 Subject: [PATCH] Log local position setpoint reference for fixedwings when running NPFG (#20512) * Log position setpoint reference of npfg This commit logs the local position setpoint reference when using NPFG * Address review comments This commit address review comments from @tstastny --- src/lib/npfg/npfg.cpp | 9 ++++ src/lib/npfg/npfg.hpp | 3 ++ .../FixedwingPositionControl.cpp | 47 +++++++++++-------- 3 files changed, 40 insertions(+), 19 deletions(-) diff --git a/src/lib/npfg/npfg.cpp b/src/lib/npfg/npfg.cpp index 3f4da2df46..03ba6939fb 100644 --- a/src/lib/npfg/npfg.cpp +++ b/src/lib/npfg/npfg.cpp @@ -535,6 +535,7 @@ void NPFG::navigateWaypoints(const Vector2f &waypoint_A, const Vector2f &waypoin // single waypoint, fly directly to it unit_path_tangent_ = -vector_A_to_vehicle.normalized(); signed_track_error_ = vector_A_to_vehicle.norm(); + closest_point_on_path_ = waypoint_A; guideToPoint(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_); } else if (vector_A_to_B.dot(vector_A_to_vehicle) < 0.0f) { @@ -545,6 +546,7 @@ void NPFG::navigateWaypoints(const Vector2f &waypoint_A, const Vector2f &waypoin // guidance to the line through A and B unit_path_tangent_ = vector_A_to_B.normalized(); signed_track_error_ = unit_path_tangent_.cross(vector_A_to_vehicle); + closest_point_on_path_ = waypoint_A + vector_A_to_vehicle.dot(unit_path_tangent_) * unit_path_tangent_; guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, 0.0f); const Vector2f bearing_vec_to_point = -vector_A_to_vehicle.normalized(); @@ -560,6 +562,7 @@ void NPFG::navigateWaypoints(const Vector2f &waypoint_A, const Vector2f &waypoin unit_path_tangent_ = bearing_vec_to_point; signed_track_error_ = vector_A_to_vehicle.norm(); + closest_point_on_path_ = waypoint_A; guideToPoint(ground_vel, wind_vel, bearing_vec_to_point, signed_track_error_); } @@ -567,6 +570,7 @@ void NPFG::navigateWaypoints(const Vector2f &waypoint_A, const Vector2f &waypoin // track the line segment between A and B unit_path_tangent_ = vector_A_to_B.normalized(); signed_track_error_ = unit_path_tangent_.cross(vector_A_to_vehicle); + closest_point_on_path_ = waypoint_A + vector_A_to_vehicle.dot(unit_path_tangent_) * unit_path_tangent_; guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, 0.0f); } @@ -612,6 +616,8 @@ void NPFG::navigateLoiter(const Vector2f &loiter_center, const Vector2f &vehicle // positive in direction of path normal signed_track_error_ = -loiter_direction_multiplier * (dist_to_center - radius); + closest_point_on_path_ = unit_vec_center_to_closest_pt * radius + loiter_center; + float path_curvature = loiter_direction_multiplier / radius; guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, path_curvature); @@ -631,6 +637,7 @@ void NPFG::navigatePathTangent(const matrix::Vector2f &vehicle_pos, const matrix // closest point to vehicle matrix::Vector2f error_vector = vehicle_pos - position_setpoint; + closest_point_on_path_ = position_setpoint; signed_track_error_ = unit_path_tangent_.cross(error_vector); guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, curvature); @@ -646,6 +653,8 @@ void NPFG::navigateHeading(float heading_ref, const Vector2f &ground_vel, const unit_path_tangent_ = Vector2f{cosf(heading_ref), sinf(heading_ref)}; signed_track_error_ = 0.0f; + closest_point_on_path_.setNaN(); + // use the guidance law to regulate heading error - ignoring wind or inertial position guideToPath(air_vel, Vector2f{0.0f, 0.0f}, unit_path_tangent_, signed_track_error_, 0.0f); diff --git a/src/lib/npfg/npfg.hpp b/src/lib/npfg/npfg.hpp index 4d4a5a2772..decce96f20 100644 --- a/src/lib/npfg/npfg.hpp +++ b/src/lib/npfg/npfg.hpp @@ -180,6 +180,8 @@ public: */ float getHeadingRef() const { return atan2f(air_vel_ref_(1), air_vel_ref_(0)); } + matrix::Vector2f getClosestPoint() const { return closest_point_on_path_;} + /* * @return Bearing angle [rad] */ @@ -422,6 +424,7 @@ private: matrix::Vector2f unit_path_tangent_{matrix::Vector2f{1.0f, 0.0f}}; // unit path tangent vector float signed_track_error_{0.0f}; // signed track error [m] matrix::Vector2f bearing_vec_{matrix::Vector2f{1.0f, 0.0f}}; // bearing unit vector + matrix::Vector2f closest_point_on_path_{matrix::Vector2f{NAN, NAN}}; // instantaneous position setpoint [m] /* * guidance outputs diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 21cdf50364..3f4bea7885 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -2733,26 +2733,35 @@ bool FixedwingPositionControl::checkLandingAbortBitMask(const uint8_t automatic_ void FixedwingPositionControl::publishLocalPositionSetpoint(const position_setpoint_s ¤t_waypoint) { - if (_global_local_proj_ref.isInitialized()) { - Vector2f current_setpoint = _global_local_proj_ref.project(current_waypoint.lat, current_waypoint.lon); - vehicle_local_position_setpoint_s local_position_setpoint{}; - local_position_setpoint.timestamp = hrt_absolute_time(); - local_position_setpoint.x = current_setpoint(0); - local_position_setpoint.y = current_setpoint(1); - local_position_setpoint.z = _global_local_alt0 - current_waypoint.alt; - local_position_setpoint.yaw = NAN; - local_position_setpoint.yawspeed = NAN; - local_position_setpoint.vx = NAN; - local_position_setpoint.vy = NAN; - local_position_setpoint.vz = NAN; - local_position_setpoint.acceleration[0] = NAN; - local_position_setpoint.acceleration[1] = NAN; - local_position_setpoint.acceleration[2] = NAN; - local_position_setpoint.thrust[0] = _att_sp.thrust_body[0]; - local_position_setpoint.thrust[1] = _att_sp.thrust_body[1]; - local_position_setpoint.thrust[2] = _att_sp.thrust_body[2]; - _local_pos_sp_pub.publish(local_position_setpoint); + vehicle_local_position_setpoint_s local_position_setpoint{}; + local_position_setpoint.timestamp = hrt_absolute_time(); + + Vector2f current_setpoint; + + if (!_param_fw_use_npfg.get()) { + if (_global_local_proj_ref.isInitialized()) { + current_setpoint = _global_local_proj_ref.project(current_waypoint.lat, current_waypoint.lon); + } + + } else { + current_setpoint = _npfg.getClosestPoint(); } + + local_position_setpoint.x = current_setpoint(0); + local_position_setpoint.y = current_setpoint(1); + local_position_setpoint.z = _global_local_alt0 - current_waypoint.alt; + local_position_setpoint.yaw = NAN; + local_position_setpoint.yawspeed = NAN; + local_position_setpoint.vx = NAN; + local_position_setpoint.vy = NAN; + local_position_setpoint.vz = NAN; + local_position_setpoint.acceleration[0] = NAN; + local_position_setpoint.acceleration[1] = NAN; + local_position_setpoint.acceleration[2] = NAN; + local_position_setpoint.thrust[0] = _att_sp.thrust_body[0]; + local_position_setpoint.thrust[1] = _att_sp.thrust_body[1]; + local_position_setpoint.thrust[2] = _att_sp.thrust_body[2]; + _local_pos_sp_pub.publish(local_position_setpoint); } void FixedwingPositionControl::publishOrbitStatus(const position_setpoint_s pos_sp)