Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-22 06:14:14 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

FW Position Control: initializeAutoLanding(): pass only alt value of pos_sp_curr

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2023-01-25 15:13:28 +01:00
parent 805de8a6d9
commit b94ed34406
2 changed files with 6 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_path_navigation/FixedwingPositionControl.cpp
+4 -4
View File
@@ -1507,7 +1507,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
const Vector2f local_position{_local_pos.x, _local_pos.y};
Vector2f local_land_point = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon);
initializeAutoLanding(now, pos_sp_prev, pos_sp_curr, local_position, local_land_point);
initializeAutoLanding(now, pos_sp_prev, pos_sp_curr.alt, local_position, local_land_point);
// touchdown may get nudged by manual inputs
local_land_point = calculateTouchdownPosition(control_interval, local_land_point);
@@ -2360,7 +2360,7 @@ FixedwingPositionControl::calculateTakeoffBearingVector(const Vector2f &launch_p
void
FixedwingPositionControl::initializeAutoLanding(const hrt_abstime &now, const position_setpoint_s &pos_sp_prev,
const position_setpoint_s &pos_sp_curr, const Vector2f &local_position, const Vector2f &local_land_point)
const float land_point_altitdue, const Vector2f &local_position, const Vector2f &local_land_point)
{
if (_time_started_landing == 0) {
@@ -2371,7 +2371,7 @@ FixedwingPositionControl::initializeAutoLanding(const hrt_abstime &now, const po
// NOTE: the landing approach vector is relative to the land point. ekf resets may cause a local frame
// jump, so we reference to the land point, which is globally referenced and will update
if (_position_setpoint_previous_valid) {
height_above_land_point = pos_sp_prev.alt - pos_sp_curr.alt;
height_above_land_point = pos_sp_prev.alt - land_point_altitdue;
local_approach_entrance = _global_local_proj_ref.project(pos_sp_prev.lat, pos_sp_prev.lon);
} else {
@@ -2384,7 +2384,7 @@ FixedwingPositionControl::initializeAutoLanding(const hrt_abstime &now, const po
// TODO: proper handling of on-the-fly landing points would need to involve some more sophisticated
// landing pattern generation and corresponding logic
height_above_land_point = _current_altitude - pos_sp_curr.alt;
height_above_land_point = _current_altitude - land_point_altitdue;
local_approach_entrance = local_position;
}
src/modules/fw_path_navigation/FixedwingPositionControl.hpp
+2 -2
View File
@@ -748,12 +748,12 @@ private:
*
* @param now Current system time [us]
* @param pos_sp_prev Previous position setpoint
* @param pos_sp_curr Current position setpoint
* @param land_point_alt Landing point altitude setpoint AMSL [m]
* @param local_position Local aircraft position (NE) [m]
* @param local_land_point Local land point (NE) [m]
*/
void initializeAutoLanding(const hrt_abstime &now, const position_setpoint_s &pos_sp_prev,
const position_setpoint_s &pos_sp_curr, const Vector2f &local_position, const Vector2f &local_land_point);
const float land_point_alt, const Vector2f &local_position, const Vector2f &local_land_point);
/*
* Waypoint handling logic following closely to the ECL_L1_Pos_Controller