fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-08-21 16:20:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add interface for passing path tangent and closest point directly

Browse Source 
This commit adds an interface to pass the path tangent and closest point directly to NPFG using the offboard interface
This commit is contained in:
Jaeyoung Lim
2022-01-26 15:58:02 +01:00
parent 3f25349eb9
commit 224dc487e6
3 changed files with 69 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/npfg/npfg.cpp
+19
View File
@@ -617,6 +617,25 @@ void NPFG::navigateLoiter(const Vector2d &loiter_center, const Vector2d &vehicle
updateRollSetpoint();
} // navigateLoiter
void NPFG::navigatePathTangent(const matrix::Vector2d &vehicle_pos, const matrix::Vector2d &position_setpoint,
const matrix::Vector2f &tangent_setpoint,
const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel, const float &curvature)
{
path_type_loiter_ = false;
// set unit tangent directly
unit_path_tangent_ = tangent_setpoint.normalized();
// closest point to vehicle
matrix::Vector2f error_vector = getLocalPlanarVector(position_setpoint, vehicle_pos);
signed_track_error_ = cross2D(unit_path_tangent_, error_vector);
guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, curvature);
updateRollSetpoint();
} // navigatePathTangent
void NPFG::navigateHeading(float heading_ref, const Vector2f &ground_vel, const Vector2f &wind_vel)
{
path_type_loiter_ = false;
src/lib/npfg/npfg.hpp
+15
View File
@@ -248,6 +248,21 @@ public:
float radius, int8_t loiter_direction, const matrix::Vector2f &ground_vel,
const matrix::Vector2f &wind_vel);
/*
* Path following logic. Takes poisiton, path tangent, curvature and
* then updates control setpoints to follow a path setpoint.
*
* @param[in] vehicle_pos vehicle_pos Vehicle position in WGS84 coordinates (lat,lon) [deg]
* @param[in] position_setpoint closest point on a path in WGS84 coordinates (lat,lon) [deg]
* @param[in] tangent_setpoint unit tangent vector of the path [m]
* @param[in] ground_vel Vehicle ground velocity vector [m/s]
* @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,
const matrix::Vector2f &tangent_setpoint,
const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel, const float &curvature);
/*
* Navigate on a fixed heading.
*
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+35 -20
View File
@@ -1191,6 +1191,16 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const fl
if (_param_fw_use_npfg.get()) {
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
if (_control_mode.flag_control_offboard_enabled && PX4_ISFINITE(pos_sp_curr.vx) && PX4_ISFINITE(pos_sp_curr.vy)) {
// Navigate directly on position setpoint and path tangent
matrix::Vector2f velocity_2d(pos_sp_curr.vx, pos_sp_curr.vy);
_npfg.navigatePathTangent(curr_pos, curr_wp, velocity_2d.normalized(), ground_speed, _wind_vel, pos_sp_curr.yawspeed);
} else {
_npfg.navigateWaypoints(prev_wp, curr_wp, curr_pos, ground_speed, _wind_vel);
}
_npfg.navigateWaypoints(prev_wp, curr_wp, curr_pos, get_nav_speed_2d(ground_speed), _wind_vel);
_att_sp.roll_body = _npfg.getRollSetpoint();
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
@@ -2219,44 +2229,49 @@ FixedwingPositionControl::Run()
vehicle_local_position_setpoint_s trajectory_setpoint;
if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) {
bool valid_setpoint = false;
_pos_sp_triplet = {}; // clear any existing
_pos_sp_triplet.timestamp = trajectory_setpoint.timestamp;
_pos_sp_triplet.current.timestamp = trajectory_setpoint.timestamp;
_pos_sp_triplet.current.cruising_speed = NAN; // ignored
_pos_sp_triplet.current.cruising_throttle = NAN; // ignored
_pos_sp_triplet.current.vx = NAN;
_pos_sp_triplet.current.vy = NAN;
_pos_sp_triplet.current.vz = NAN;
_pos_sp_triplet.current.lat = static_cast<double>(NAN);
_pos_sp_triplet.current.lon = static_cast<double>(NAN);
_pos_sp_triplet.current.alt = NAN;
if (PX4_ISFINITE(trajectory_setpoint.x) && PX4_ISFINITE(trajectory_setpoint.y) && PX4_ISFINITE(trajectory_setpoint.z)) {
double lat;
double lon;
if (_global_local_proj_ref.isInitialized()) {
double lat;
double lon;
_global_local_proj_ref.reproject(trajectory_setpoint.x, trajectory_setpoint.y, lat, lon);
_pos_sp_triplet = {}; // clear any existing
_pos_sp_triplet.timestamp = trajectory_setpoint.timestamp;
_pos_sp_triplet.current.timestamp = trajectory_setpoint.timestamp;
_pos_sp_triplet.current.valid = true;
valid_setpoint = true;
_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
_pos_sp_triplet.current.lat = lat;
_pos_sp_triplet.current.lon = lon;
_pos_sp_triplet.current.alt = _global_local_alt0 - trajectory_setpoint.z;
_pos_sp_triplet.current.cruising_speed = NAN; // ignored
_pos_sp_triplet.current.cruising_throttle = NAN; // ignored
}
} else if (PX4_ISFINITE(trajectory_setpoint.vx) && PX4_ISFINITE(trajectory_setpoint.vx)
&& PX4_ISFINITE(trajectory_setpoint.vz)) {
_pos_sp_triplet = {}; // clear any existing
}
_pos_sp_triplet.timestamp = trajectory_setpoint.timestamp;
_pos_sp_triplet.current.timestamp = trajectory_setpoint.timestamp;
_pos_sp_triplet.current.valid = true;
if (PX4_ISFINITE(trajectory_setpoint.vx) && PX4_ISFINITE(trajectory_setpoint.vx)
&& PX4_ISFINITE(trajectory_setpoint.vz)) {
valid_setpoint = true;
_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
_pos_sp_triplet.current.vx = trajectory_setpoint.vx;
_pos_sp_triplet.current.vy = trajectory_setpoint.vy;
_pos_sp_triplet.current.vz = trajectory_setpoint.vz;
_pos_sp_triplet.current.alt = NAN;
_pos_sp_triplet.current.cruising_speed = NAN; // ignored
_pos_sp_triplet.current.cruising_throttle = NAN; // ignored
}
} else {
if (!valid_setpoint) {
mavlink_log_critical(&_mavlink_log_pub, "Invalid offboard setpoint\t");
events::send(events::ID("fixedwing_position_control_invalid_offboard_sp"), events::Log::Error,
"Invalid offboard setpoint");
} else {
_pos_sp_triplet.current.valid = valid_setpoint;
}
}