fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-03 13:30:35 +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
committed by JaeyoungLim
parent f460611098
commit e2741f988a
3 changed files with 69 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
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;
}
}