Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 19 Packages Projects Releases Wiki Activity

Project local coordinates for all fw landing states

Browse Source
This commit is contained in:
Jaeyoung-Lim
2022-03-23 11:11:50 +01:00
committed by JaeyoungLim
parent cb81c6ac8c
commit 194a281fae
1 changed files with 5 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+5 -17
View File
@@ -1697,6 +1697,11 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
curr_wp(1) = lon; curr_wp(1) = 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));
// we want the plane to keep tracking the desired flight path until we start flaring // we want the plane to keep tracking the desired flight path until we start flaring
// if we go into heading hold mode earlier then we risk to be pushed away from the runway by cross winds // if we go into heading hold mode earlier then we risk to be pushed away from the runway by cross winds
if ((_param_fw_lnd_hhdist.get() > 0.0f) && !_land_noreturn_horizontal && if ((_param_fw_lnd_hhdist.get() > 0.0f) && !_land_noreturn_horizontal &&
@@ -1808,10 +1813,6 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
_npfg.navigateHeading(_target_bearing, ground_speed, _wind_vel); _npfg.navigateHeading(_target_bearing, ground_speed, _wind_vel);
} else { } 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 // normal navigation
_npfg.navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); _npfg.navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
} }
@@ -1826,10 +1827,6 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
} else { } else {
// normal navigation // normal navigation
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); _l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed);
} }
@@ -1932,11 +1929,6 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
} else { } else {
// normal navigation // normal navigation
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); _npfg.navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
} }
@@ -1950,10 +1942,6 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
} else { } else {
// normal navigation // normal navigation
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); _l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed);
} }