dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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
This commit is contained in:
JaeyoungLim 2022-11-22 08:19:38 +01:00 committed by GitHub
parent c24f9561e9
commit 2586900c26
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 40 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/lib/npfg/npfg.cpp
View File

@ -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);

3
src/lib/npfg/npfg.hpp
View File

@ -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

47
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

@ -2733,26 +2733,35 @@ bool FixedwingPositionControl::checkLandingAbortBitMask(const uint8_t automatic_
void FixedwingPositionControl::publishLocalPositionSetpoint(const position_setpoint_s &current_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)