diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1030_plane b/ROMFS/px4fmu_common/init.d-posix/airframes/1030_plane index 2627ceea60..3c40be1514 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1030_plane +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1030_plane @@ -10,7 +10,6 @@ param set-default EKF2_MAG_YAWLIM 0 param set-default FW_LND_AIRSPD_SC 1 param set-default FW_LND_ANG 8 -param set-default FW_THR_LND_MAX 0 param set-default FW_L1_PERIOD 12 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1033_rascal b/ROMFS/px4fmu_common/init.d-posix/airframes/1033_rascal index e0f8c32954..80aad7ab12 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1033_rascal +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1033_rascal @@ -7,12 +7,9 @@ param set-default FW_LND_AIRSPD_SC 1.1 param set-default FW_LND_ANG 5 -param set-default FW_THR_LND_MAX 0 -param set-default FW_LND_HHDIST 30 param set-default FW_LND_FL_PMIN 9.5 param set-default FW_LND_FL_PMAX 20 param set-default FW_LND_FLALT 5 -param set-default FW_LND_TLALT 15 param set-default FW_L1_PERIOD 25 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1034_rascal-electric b/ROMFS/px4fmu_common/init.d-posix/airframes/1034_rascal-electric index e151ad023e..4344116652 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1034_rascal-electric +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1034_rascal-electric @@ -7,12 +7,9 @@ param set-default FW_LND_AIRSPD_SC 1.1 param set-default FW_LND_ANG 5 -param set-default FW_THR_LND_MAX 0 -param set-default FW_LND_HHDIST 30 param set-default FW_LND_FL_PMIN 9.5 param set-default FW_LND_FL_PMAX 20 param set-default FW_LND_FLALT 5 -param set-default FW_LND_TLALT 15 param set-default FW_L1_PERIOD 25 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1035_techpod b/ROMFS/px4fmu_common/init.d-posix/airframes/1035_techpod index 30d7b926b0..83476e9e93 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1035_techpod +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1035_techpod @@ -10,7 +10,6 @@ param set-default EKF2_MAG_YAWLIM 0 param set-default FW_LND_AIRSPD_SC 1 param set-default FW_LND_ANG 8 -param set-default FW_THR_LND_MAX 0 param set-default FW_L1_PERIOD 15 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1036_malolo b/ROMFS/px4fmu_common/init.d-posix/airframes/1036_malolo index d30dd25eaf..915940a146 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1036_malolo +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1036_malolo @@ -7,12 +7,9 @@ param set-default FW_LND_AIRSPD_SC 1.1 param set-default FW_LND_ANG 5 -param set-default FW_THR_LND_MAX 0 -param set-default FW_LND_HHDIST 30 param set-default FW_LND_FL_PMIN 9.5 param set-default FW_LND_FL_PMAX 20 param set-default FW_LND_FLALT 5 -param set-default FW_LND_TLALT 15 param set-default FW_L1_PERIOD 25 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1037_believer b/ROMFS/px4fmu_common/init.d-posix/airframes/1037_believer index e477d3f138..d836ca8873 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1037_believer +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1037_believer @@ -10,7 +10,6 @@ param set-default EKF2_MAG_YAWLIM 0 param set-default FW_LND_AIRSPD_SC 1 param set-default FW_LND_ANG 8 -param set-default FW_THR_LND_MAX 0 param set-default FW_L1_PERIOD 12 diff --git a/ROMFS/px4fmu_common/init.d/airframes/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/airframes/1000_rc_fw_easystar.hil index c96535bea4..a3650bc621 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1000_rc_fw_easystar.hil @@ -29,9 +29,6 @@ param set-default FW_L1_DAMPING 0.74 param set-default FW_L1_PERIOD 16 param set-default FW_LND_ANG 15 param set-default FW_LND_FLALT 5 -param set-default FW_LND_HVIRT 13 -param set-default FW_LND_TLALT 5 -param set-default FW_THR_LND_MAX 0 param set-default FW_PR_FF 0.35 param set-default FW_PR_P 0.2 param set-default FW_RR_FF 0.6 diff --git a/ROMFS/px4fmu_common/init.d/airframes/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/airframes/3032_skywalker_x5 index f09bba392b..cd4b8529dd 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/airframes/3032_skywalker_x5 @@ -30,9 +30,6 @@ param set-default FW_L1_DAMPING 0.74 param set-default FW_L1_PERIOD 16 param set-default FW_LND_ANG 15 param set-default FW_LND_FLALT 5 -param set-default FW_LND_HVIRT 13 -param set-default FW_LND_TLALT 5 -param set-default FW_THR_LND_MAX 0 param set-default FW_PR_FF 0.35 param set-default FW_RR_FF 0.6 param set-default FW_RR_P 0.04 diff --git a/ROMFS/px4fmu_common/init.d/airframes/3033_wingwing b/ROMFS/px4fmu_common/init.d/airframes/3033_wingwing index d9a18853a2..99673ad470 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/airframes/3033_wingwing @@ -32,9 +32,6 @@ param set-default FW_L1_DAMPING 0.74 param set-default FW_L1_PERIOD 16 param set-default FW_LND_ANG 15 param set-default FW_LND_FLALT 5 -param set-default FW_LND_HVIRT 13 -param set-default FW_LND_TLALT 5 -param set-default FW_THR_LND_MAX 0 param set-default FW_PR_FF 0.35 param set-default FW_RR_FF 0.6 param set-default FW_RR_P 0.04 diff --git a/ROMFS/px4fmu_common/init.d/airframes/3100_tbs_caipirinha b/ROMFS/px4fmu_common/init.d/airframes/3100_tbs_caipirinha index 694af0e14d..8dcad27739 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/3100_tbs_caipirinha +++ b/ROMFS/px4fmu_common/init.d/airframes/3100_tbs_caipirinha @@ -27,9 +27,6 @@ param set-default FW_AIRSPD_TRIM 16.5 param set-default FW_L1_PERIOD 15 param set-default FW_LND_ANG 15 param set-default FW_LND_FLALT 8 -param set-default FW_LND_HVIRT 13 -param set-default FW_LND_TLALT 10 -param set-default FW_THR_LND_MAX 0 param set-default FW_P_LIM_MAX 20 param set-default FW_P_LIM_MIN -30 param set-default FW_R_LIM 45 diff --git a/msg/position_controller_landing_status.msg b/msg/position_controller_landing_status.msg index 8f5ae9654b..71f06a6117 100644 --- a/msg/position_controller_landing_status.msg +++ b/msg/position_controller_landing_status.msg @@ -1,9 +1,3 @@ -uint64 timestamp # time since system start (microseconds) - -float32 horizontal_slope_displacement - -float32 slope_angle_rad - -float32 flare_length - -bool abort_landing # true if landing should be aborted +uint64 timestamp # [us] time since system start +float32 lateral_touchdown_offset # [m] lateral touchdown position offset manually commanded during landing +bool abort_landing # true if landing should be aborted diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 0552b21db7..bb27f0eccb 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -48,7 +48,6 @@ add_subdirectory(field_sensor_bias_estimator) add_subdirectory(geo) add_subdirectory(hysteresis) add_subdirectory(l1) -add_subdirectory(landing_slope) add_subdirectory(led) add_subdirectory(matrix) add_subdirectory(mathlib) diff --git a/src/lib/landing_slope/CMakeLists.txt b/src/lib/landing_slope/CMakeLists.txt deleted file mode 100644 index 55f5940b78..0000000000 --- a/src/lib/landing_slope/CMakeLists.txt +++ /dev/null @@ -1,34 +0,0 @@ -############################################################################ -# -# Copyright (c) 2018 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -px4_add_library(landing_slope Landingslope.cpp) diff --git a/src/lib/landing_slope/Landingslope.cpp b/src/lib/landing_slope/Landingslope.cpp deleted file mode 100644 index 61aeb2ac53..0000000000 --- a/src/lib/landing_slope/Landingslope.cpp +++ /dev/null @@ -1,132 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2017 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* - * @file landingslope.cpp - * - * @author Thomas Gubler - */ - -#include "Landingslope.hpp" - -#include -#include - -void -Landingslope::update(float landing_slope_angle_rad_new, - float flare_relative_alt_new, - float motor_lim_relative_alt_new, - float H1_virt_new) -{ - _landing_slope_angle_rad = landing_slope_angle_rad_new; - _flare_relative_alt = flare_relative_alt_new; - _motor_lim_relative_alt = motor_lim_relative_alt_new; - _H1_virt = H1_virt_new; - - calculateSlopeValues(); -} - -void -Landingslope::calculateSlopeValues() -{ - _H0 = _flare_relative_alt + _H1_virt; - _d1 = _flare_relative_alt / tanf(_landing_slope_angle_rad); - _flare_constant = (_H0 * _d1) / _flare_relative_alt; - _flare_length = -logf(_H1_virt / _H0) * _flare_constant; - _horizontal_slope_displacement = (_flare_length - _d1); -} - -float -Landingslope::getLandingSlopeRelativeAltitude(float wp_landing_distance) -{ - return Landingslope::getLandingSlopeRelativeAltitude(wp_landing_distance, _horizontal_slope_displacement, - _landing_slope_angle_rad); -} - -float -Landingslope::getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, - float bearing_airplane_currwp) -{ - /* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */ - if (fabsf(matrix::wrap_pi(bearing_airplane_currwp - bearing_lastwp_currwp)) < math::radians(90.0f)) { - return getLandingSlopeRelativeAltitude(wp_landing_distance); - - } - - return 0.0f; -} - -float -Landingslope::getFlareCurveRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, - float bearing_airplane_currwp) -{ - /* If airplane is in front of waypoint return flare curve altitude, else return waypoint altitude */ - if (fabsf(matrix::wrap_pi(bearing_airplane_currwp - bearing_lastwp_currwp)) < math::radians(90.0f)) { - return _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance) / _flare_constant) - _H1_virt; - - } - - return 0.0f; -} - -/** - * - * @return Relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance - */ -float Landingslope::getLandingSlopeRelativeAltitude(float wp_landing_distance, float horizontal_slope_displacement, - float landing_slope_angle_rad) -{ - // flare_relative_alt is negative - return (wp_landing_distance - horizontal_slope_displacement) * tanf(landing_slope_angle_rad); -} - -/** - * - * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance - */ -float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude, - float horizontal_slope_displacement, float landing_slope_angle_rad) -{ - return getLandingSlopeRelativeAltitude(wp_landing_distance, horizontal_slope_displacement, - landing_slope_angle_rad) + wp_landing_altitude; -} - -/** - * - * @return distance to landing waypoint of point on landing slope at altitude=slope_altitude - */ -float Landingslope::getLandingSlopeWPDistance(float slope_altitude, float wp_landing_altitude, - float horizontal_slope_displacement, float landing_slope_angle_rad) -{ - return (slope_altitude - wp_landing_altitude) / tanf(landing_slope_angle_rad) + horizontal_slope_displacement; -} diff --git a/src/lib/landing_slope/Landingslope.hpp b/src/lib/landing_slope/Landingslope.hpp deleted file mode 100644 index 6bdda717a1..0000000000 --- a/src/lib/landing_slope/Landingslope.hpp +++ /dev/null @@ -1,118 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2017 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* - * @file landingslope.h - * - * @author Thomas Gubler - */ - -#ifndef LANDINGSLOPE_H_ -#define LANDINGSLOPE_H_ - -#include - -class Landingslope -{ -private: - /* see Documentation/fw_landing.png for a plot of the landing slope */ - float _landing_slope_angle_rad{0.0f}; /**< phi in the plot */ - float _flare_relative_alt{0.0f}; /**< h_flare,rel in the plot */ - float _motor_lim_relative_alt{0.0f}; - float _H1_virt{0.0f}; /**< H1 in the plot */ - float _H0{0.0f}; /**< h_flare,rel + H1 in the plot */ - float _d1{0.0f}; /**< d1 in the plot */ - float _flare_constant{0.0f}; - float _flare_length{0.0f}; /**< d1 + delta d in the plot */ - float _horizontal_slope_displacement{0.0f}; /**< delta d in the plot */ - - void calculateSlopeValues(); - -public: - Landingslope() = default; - ~Landingslope() = default; - - /** - * - * @return relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance - */ - float getLandingSlopeRelativeAltitude(float wp_landing_distance); - - /** - * - * @return relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance - * Performs check if aircraft is in front of waypoint to avoid climbout - */ - float getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, - float bearing_airplane_currwp); - - /** - * - * @return Relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance - */ - static float getLandingSlopeRelativeAltitude(float wp_landing_distance, float horizontal_slope_displacement, - float landing_slope_angle_rad); - - /** - * - * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance - */ - static float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude, - float horizontal_slope_displacement, float landing_slope_angle_rad); - - /** - * - * @return distance to landing waypoint of point on landing slope at altitude=slope_altitude - */ - static float getLandingSlopeWPDistance(float slope_altitude, float wp_landing_altitude, - float horizontal_slope_displacement, float landing_slope_angle_rad); - - float getFlareCurveRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, - float bearing_airplane_currwp); - - void update(float landing_slope_angle_rad_new, - float flare_relative_alt_new, - float motor_lim_relative_alt_new, - float H1_virt_new); - - - float landing_slope_angle_rad() { return _landing_slope_angle_rad; } - float flare_relative_alt() { return _flare_relative_alt; } - float motor_lim_relative_alt() { return _motor_lim_relative_alt; } - float flare_length() { return _flare_length; } - float horizontal_slope_displacement() { return _horizontal_slope_displacement; } - -}; - - -#endif /* LANDINGSLOPE_H_ */ diff --git a/src/modules/fw_pos_control_l1/CMakeLists.txt b/src/modules/fw_pos_control_l1/CMakeLists.txt index 7aff012788..d7deaa9870 100644 --- a/src/modules/fw_pos_control_l1/CMakeLists.txt +++ b/src/modules/fw_pos_control_l1/CMakeLists.txt @@ -43,7 +43,6 @@ px4_add_module( DEPENDS l1 launchdetection - landing_slope npfg runway_takeoff SlewRate diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index f59c75f447..4db7565170 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -144,19 +144,6 @@ FixedwingPositionControl::parameters_update() _tecs.set_speed_derivative_time_constant(_param_tas_rate_time_const.get()); _tecs.set_seb_rate_ff_gain(_param_seb_rate_ff.get()); - // Landing slope - /* check if negative value for 2/3 of flare altitude is set for throttle cut */ - float land_thrust_lim_alt_relative = _param_fw_lnd_tlalt.get(); - - if (land_thrust_lim_alt_relative < 0.0f) { - land_thrust_lim_alt_relative = 0.66f * _param_fw_lnd_flalt.get(); - } - - _landingslope.update(radians(_param_fw_lnd_ang.get()), _param_fw_lnd_flalt.get(), land_thrust_lim_alt_relative, - _param_fw_lnd_hvirt.get()); - - landing_status_publish(); - int check_ret = PX4_OK; // sanity check parameters @@ -617,12 +604,8 @@ FixedwingPositionControl::landing_status_publish() { position_controller_landing_status_s pos_ctrl_landing_status = {}; - pos_ctrl_landing_status.slope_angle_rad = _landingslope.landing_slope_angle_rad(); - pos_ctrl_landing_status.horizontal_slope_displacement = _landingslope.horizontal_slope_displacement(); - pos_ctrl_landing_status.flare_length = _landingslope.flare_length(); - + pos_ctrl_landing_status.lateral_touchdown_offset = _lateral_touchdown_position_offset; pos_ctrl_landing_status.abort_landing = _land_abort; - pos_ctrl_landing_status.timestamp = hrt_absolute_time(); _pos_ctrl_landing_status_pub.publish(pos_ctrl_landing_status); @@ -1625,320 +1608,148 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo void FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const float control_interval, - const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, - const position_setpoint_s &pos_sp_curr) + const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr) { // Enable tighter altitude control for landings _tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get()); - /* current waypoint (the one currently heading for) */ - Vector2d curr_wp(pos_sp_curr.lat, pos_sp_curr.lon); - Vector2d prev_wp{0, 0}; /* previous waypoint */ + 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); - if (pos_sp_prev.valid) { - prev_wp(0) = pos_sp_prev.lat; - prev_wp(1) = pos_sp_prev.lon; + initializeAutoLanding(now, pos_sp_prev, pos_sp_curr, local_position, local_land_point); + + local_land_point = calculateTouchdownPosition(control_interval, local_land_point); + const Vector2f landing_approach_vector = calculateLandingApproachVector(); + + const float airspeed_land = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get(); + float target_airspeed = get_auto_airspeed_setpoint(control_interval, airspeed_land, ground_speed); + + // calculate the altitude setpoint based on the landing glide slope + const float along_track_dist_to_touchdown = -landing_approach_vector.unit_or_zero().dot( + local_position - local_land_point); + const float glide_slope_rel_alt = math::constrain(along_track_dist_to_touchdown * tanf(math::radians( + _param_fw_lnd_ang.get())), + 0.0f, _landing_approach_entrance_rel_alt); + + const float terrain_alt = getLandingTerrainAltitudeEstimate(now, pos_sp_curr.alt); + float altitude_setpoint; + + if (_current_altitude > terrain_alt + glide_slope_rel_alt) { + // descend to the glide slope + altitude_setpoint = terrain_alt + glide_slope_rel_alt; } else { - /* - * No valid previous waypoint, go for the current wp. - * This is automatically handled by the L1 library. - */ - prev_wp(0) = pos_sp_curr.lat; - prev_wp(1) = pos_sp_curr.lon; + // continue horizontally + altitude_setpoint = _current_altitude; } - // save time at which we started landing and reset abort_landing - if (_time_started_landing == 0) { - reset_landing_state(); - _time_started_landing = now; - } + // flare at the maximum of the altitude determined by the time before touchdown and a minimum flare altitude + const float flare_rel_alt = math::max(_param_fw_lnd_fl_time.get() * _local_pos.vz, _param_fw_lnd_flalt.get()); - const float bearing_airplane_currwp = get_bearing_to_next_waypoint((double)curr_pos(0), (double)curr_pos(1), - (double)curr_wp(0), (double)curr_wp(1)); + if ((_current_altitude < terrain_alt + flare_rel_alt) || _flaring) { + // flare and land with minimal speed - float bearing_lastwp_currwp = bearing_airplane_currwp; - - if (pos_sp_prev.valid) { - bearing_lastwp_currwp = get_bearing_to_next_waypoint((double)prev_wp(0), (double)prev_wp(1), (double)curr_wp(0), - (double)curr_wp(1)); - } - - /* Horizontal landing control */ - /* switch to heading hold for the last meters, continue heading hold after */ - float wp_distance = get_distance_to_next_waypoint((double)curr_pos(0), (double)curr_pos(1), (double)curr_wp(0), - (double)curr_wp(1)); - - /* calculate a waypoint distance value which is 0 when the aircraft is behind the waypoint */ - float wp_distance_save = wp_distance; - - if (fabsf(wrap_pi(bearing_airplane_currwp - bearing_lastwp_currwp)) >= radians(90.0f)) { - wp_distance_save = 0.0f; - } - - // create virtual waypoint which is on the desired flight path but - // some distance behind landing waypoint. This will make sure that the plane - // will always follow the desired flight path even if we get close or past - // the landing waypoint - if (pos_sp_prev.valid) { - double lat = pos_sp_curr.lat; - double lon = pos_sp_curr.lon; - - create_waypoint_from_line_and_dist(pos_sp_curr.lat, pos_sp_curr.lon, - pos_sp_prev.lat, pos_sp_prev.lon, -1000.0f, &lat, &lon); - - curr_wp(0) = lat; - 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 - // 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 && - ((wp_distance < _param_fw_lnd_hhdist.get()) || _land_noreturn_vertical)) { - - if (pos_sp_prev.valid) { - /* heading hold, along the line connecting this and the last waypoint */ - _target_bearing = bearing_lastwp_currwp; - - } else { - _target_bearing = _yaw; + // flaring is a "point of no return" + if (!_flaring) { + _flaring = true; + _heightrate_setpoint_at_flare_start = _tecs.hgt_rate_setpoint(); + mavlink_log_info(&_mavlink_log_pub, "Landing, flaring\t"); + events::send(events::ID("fixedwing_position_control_landing_flaring"), events::Log::Info, "Landing, flaring"); } - _land_noreturn_horizontal = true; - mavlink_log_info(&_mavlink_log_pub, "Landing, heading hold\t"); - events::send(events::ID("fixedwing_position_control_landing"), events::Log::Info, "Landing, heading hold"); - } + /* lateral guidance first, because npfg will adjust the airspeed setpoint if necessary */ - /* Vertical landing control */ - /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ + // tune up the lateral position control guidance when on the ground + _npfg.setPeriod(_param_rwto_l1_period.get()); + _l1_control.set_l1_period(_param_rwto_l1_period.get()); - // default to no terrain estimation, just use landing waypoint altitude - float terrain_alt = pos_sp_curr.alt; + // align heading with the approach bearing + const float landing_approach_bearing = atan2f(landing_approach_vector(1), landing_approach_vector(0)); - if (_param_fw_lnd_useter.get() == 1) { - if (_local_pos.dist_bottom_valid) { - // all good, have valid terrain altitude - float terrain_vpos = _local_pos.dist_bottom + _local_pos.z; - terrain_alt = (_local_pos.ref_alt - terrain_vpos); - _last_valid_terrain_alt_estimate = terrain_alt; - _last_time_terrain_alt_was_valid = now; - - } else if (_last_time_terrain_alt_was_valid == 0) { - // we have started landing phase but don't have valid terrain - // wait for some time, maybe we will soon get a valid estimate - // until then just use the altitude of the landing waypoint - if ((now - _time_started_landing) < 10_s) { - terrain_alt = pos_sp_curr.alt; - - } else { - // still no valid terrain, abort landing - terrain_alt = pos_sp_curr.alt; - abort_landing(true); - } - - } else if ((!_local_pos.dist_bottom_valid && (now - _last_time_terrain_alt_was_valid) < TERRAIN_ALT_TIMEOUT) - || _land_noreturn_vertical) { - // use previous terrain estimate for some time and hope to recover - // if we are already flaring (land_noreturn_vertical) then just - // go with the old estimate - terrain_alt = _last_valid_terrain_alt_estimate; - - } else { - // terrain alt was not valid for long time, abort landing - terrain_alt = _last_valid_terrain_alt_estimate; - abort_landing(true); - } - } - - /* Check if we should start flaring with a vertical and a - * horizontal limit (with some tolerance) - * The horizontal limit is only applied when we are in front of the wp - */ - if ((_current_altitude < terrain_alt + _landingslope.flare_relative_alt()) || - _land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out - - /* land with minimal speed */ - - /* force TECS to only control speed with pitch, altitude is only implicitly controlled now */ - // _tecs.set_speed_weight(2.0f); - - /* kill the throttle if param requests it */ - float throttle_max = _param_fw_thr_max.get(); - - if (((_current_altitude < terrain_alt + _landingslope.motor_lim_relative_alt()) && - (wp_distance_save < _landingslope.flare_length() + 5.0f)) || // Only kill throttle when close to WP - _land_motor_lim) { - throttle_max = min(throttle_max, _param_fw_thr_lnd_max.get()); - - if (!_land_motor_lim) { - _land_motor_lim = true; - mavlink_log_info(&_mavlink_log_pub, "Landing, limiting throttle\t"); - events::send(events::ID("fixedwing_position_control_landing_limit_throttle"), events::Log::Info, - "Landing, limiting throttle"); - } - } - - float flare_curve_alt_rel = _landingslope.getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, - bearing_airplane_currwp); - - /* avoid climbout */ - if ((_flare_curve_alt_rel_last < flare_curve_alt_rel && _land_noreturn_vertical) || _land_stayonground) { - flare_curve_alt_rel = 0.0f; // stay on ground - _land_stayonground = true; - } - - const float airspeed_land = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get(); - float target_airspeed = get_auto_airspeed_setpoint(control_interval, airspeed_land, ground_speed); - - /* lateral guidance */ if (_param_fw_use_npfg.get()) { _npfg.setAirspeedNom(target_airspeed * _eas2tas); _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); - - if (_land_noreturn_horizontal) { - // heading hold - _npfg.navigateHeading(_target_bearing, ground_speed, _wind_vel); - - } else { - // normal navigation - _npfg.navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); - } - + _npfg.navigateHeading(landing_approach_bearing, ground_speed, _wind_vel); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; _att_sp.roll_body = _npfg.getRollSetpoint(); } else { - if (_land_noreturn_horizontal) { - // heading hold - _l1_control.navigate_heading(_target_bearing, _yaw, ground_speed); - - } else { - // normal navigation - _l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed); - } - + _l1_control.navigate_heading(landing_approach_bearing, _yaw, ground_speed); _att_sp.roll_body = _l1_control.get_roll_setpoint(); } - /* enable direct yaw control using rudder/wheel */ - if (_land_noreturn_horizontal) { - _att_sp.yaw_body = _target_bearing; - _att_sp.fw_control_yaw = true; + /* longitudinal guidance */ - } else { - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw - } + // ramp in flare limits and setpoints with the flare time, command a soft touchdown + const float seconds_since_flare_start = hrt_elapsed_time(&_time_started_flaring) * 1e-6; + const float flare_ramp_in_scaler = math::constrain(seconds_since_flare_start / _param_fw_lnd_fl_time.get(), 0.0f, 1.0f); + + const float height_rate_setpoint = flare_ramp_in_scaler * (-_param_fw_lnd_fl_sink.get()) + + (1.0f - flare_ramp_in_scaler) * _heightrate_setpoint_at_flare_start; + + const float pitch_min_rad = flare_ramp_in_scaler * radians(_param_fw_lnd_fl_pmin.get()) + + (1.0f - flare_ramp_in_scaler) * radians(_param_fw_p_lim_min.get()); + const float pitch_max_rad = flare_ramp_in_scaler * radians(_param_fw_lnd_fl_pmax.get()) + + (1.0f - flare_ramp_in_scaler) * radians(_param_fw_p_lim_max.get()); tecs_update_pitch_throttle(control_interval, - terrain_alt + flare_curve_alt_rel, + altitude_setpoint, target_airspeed, - radians(_param_fw_lnd_fl_pmin.get()), - radians(_param_fw_lnd_fl_pmax.get()), + pitch_min_rad, + pitch_max_rad, + 0.0f, 0.0f, - throttle_max, false, - _land_motor_lim ? radians(_param_fw_lnd_fl_pmin.get()) : radians(_param_fw_p_lim_min.get()), - true); + pitch_min_rad, + true, + height_rate_setpoint); - if (!_land_noreturn_vertical) { - // just started with the flaring phase - _flare_pitch_sp = radians(_param_fw_psp_off.get()); - _flare_height = _current_altitude - terrain_alt; - mavlink_log_info(&_mavlink_log_pub, "Landing, flaring\t"); - events::send(events::ID("fixedwing_position_control_landing_flaring"), events::Log::Info, "Landing, flaring"); - _land_noreturn_vertical = true; + /* set the attitude and throttle commands */ - } else { - if (_local_pos.vz > 0.1f) { - _flare_pitch_sp = radians(_param_fw_lnd_fl_pmin.get()) * - constrain((_flare_height - (_current_altitude - terrain_alt)) / _flare_height, 0.0f, 1.0f); - } + // TECS has authority (though constrained) over pitch during flare, throttle is killed + _att_sp.pitch_body = get_tecs_pitch(); - // otherwise continue using previous _flare_pitch_sp - } + // flaring is just before touchdown, align the yaw as much as possible with the landing vector + _att_sp.yaw_body = landing_approach_bearing; - _att_sp.pitch_body = _flare_pitch_sp; - _flare_curve_alt_rel_last = flare_curve_alt_rel; + // enable direct yaw control using rudder/wheel + _att_sp.fw_control_yaw = true; + + _att_sp.thrust_body[0] = 0.0f; } else { - /* intersect glide slope: - * minimize speed to approach speed - * if current position is higher than the slope follow the glide slope (sink to the - * glide slope) - * also if the system captures the slope it should stay - * on the slope (bool land_onslope) - * if current position is below the slope continue at previous wp altitude - * until the intersection with slope - * */ - - float altitude_desired = terrain_alt; - - const float landing_slope_alt_rel_desired = _landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, - bearing_lastwp_currwp, bearing_airplane_currwp); - - if (_current_altitude > terrain_alt + landing_slope_alt_rel_desired || _land_onslope) { - /* stay on slope */ - altitude_desired = terrain_alt + landing_slope_alt_rel_desired; - - if (!_land_onslope) { - mavlink_log_info(&_mavlink_log_pub, "Landing, on slope\t"); - events::send(events::ID("fixedwing_position_control_landing_on_slope"), events::Log::Info, "Landing, on slope"); - _land_onslope = true; - } - - } else { - /* continue horizontally */ - if (pos_sp_prev.valid) { - altitude_desired = pos_sp_prev.alt; - - } else { - altitude_desired = _current_altitude; - } - } - - const float airspeed_approach = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get(); - float target_airspeed = get_auto_airspeed_setpoint(control_interval, airspeed_approach, ground_speed); + // follow the glide slope /* lateral guidance */ + + const Vector2f local_approach_entrance = local_land_point - landing_approach_vector; + if (_param_fw_use_npfg.get()) { _npfg.setAirspeedNom(target_airspeed * _eas2tas); _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); - - if (_land_noreturn_horizontal) { - // heading hold - _npfg.navigateHeading(_target_bearing, ground_speed, _wind_vel); - - } else { - // normal navigation - _npfg.navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); - } - + _npfg.navigateWaypoints(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; _att_sp.roll_body = _npfg.getRollSetpoint(); } else { - if (_land_noreturn_horizontal) { - // heading hold - _l1_control.navigate_heading(_target_bearing, _yaw, ground_speed); + // make a fake waypoint beyond the land point in the direction of the landing approach bearing + // (always HDG_HOLD_DIST_NEXT meters in front of the aircraft's progress along the landing approach vector) - } else { - // normal navigation - _l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed); - } + const float along_track_distance_from_entrance = local_approach_entrance.unit_or_zero().dot( + local_position - local_approach_entrance); + const Vector2f virtual_waypoint = local_approach_entrance + (along_track_distance_from_entrance + HDG_HOLD_DIST_NEXT) * + local_approach_entrance.unit_or_zero(); + + _l1_control.navigate_waypoints(local_approach_entrance, virtual_waypoint, local_position, ground_speed); _att_sp.roll_body = _l1_control.get_roll_setpoint(); } - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + /* longitudinal guidance */ tecs_update_pitch_throttle(control_interval, - altitude_desired, + altitude_setpoint, target_airspeed, radians(_param_fw_p_lim_min.get()), radians(_param_fw_p_lim_max.get()), @@ -1946,12 +1757,20 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo _param_fw_thr_max.get(), false, radians(_param_fw_p_lim_min.get())); - _att_sp.pitch_body = get_tecs_pitch(); - } - /* Copy thrust and pitch values from tecs */ - // when we are landed state we want the motor to spin at idle speed - _att_sp.thrust_body[0] = (_landed) ? min(_param_fw_thr_idle.get(), 1.f) : get_tecs_thrust(); + /* set the attitude and throttle commands */ + + // TECS has authority (though constrained) over pitch during flare, throttle is killed + _att_sp.pitch_body = get_tecs_pitch(); + + // yaw is not controlled in nominal flight + _att_sp.yaw_body = _yaw; + + // enable direct yaw control using rudder/wheel + _att_sp.fw_control_yaw = false; + + _att_sp.thrust_body[0] = (_landed) ? 0.0f : get_tecs_thrust(); + } _att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, terrain_alt); @@ -1962,6 +1781,8 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo if (!_vehicle_status.in_transition_to_fw) { publishLocalPositionSetpoint(pos_sp_curr); } + + landing_status_publish(); } void @@ -2372,7 +2193,7 @@ FixedwingPositionControl::Run() } case FW_POSCTRL_MODE_AUTO_LANDING: { - control_auto_landing(_local_pos.timestamp, control_interval, curr_pos, ground_speed, _pos_sp_triplet.previous, + control_auto_landing(_local_pos.timestamp, control_interval, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current); break; } @@ -2460,14 +2281,13 @@ FixedwingPositionControl::reset_landing_state() { _time_started_landing = 0; - // reset terrain estimation relevant values - _last_time_terrain_alt_was_valid = 0; + _flaring = false; + _time_started_flaring = 0; + _heightrate_setpoint_at_flare_start = 0.0f; - _land_noreturn_horizontal = false; - _land_noreturn_vertical = false; - _land_stayonground = false; - _land_motor_lim = false; - _land_onslope = false; + _lateral_touchdown_position_offset = 0.0f; + + _last_time_terrain_alt_was_valid = 0; // reset abort land, unless loitering after an abort if (_land_abort && (_pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_LOITER)) { @@ -2658,6 +2478,146 @@ FixedwingPositionControl::calculateTakeoffBearingVector(const Vector2f &launch_p return takeoff_bearing_vector; } +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) +{ + if (_time_started_landing == 0) { + + float height_above_land_point; + Vector2f local_approach_entrance; + + // set the landing approach entrance location when we have just started the landing and store it + // 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 (pos_sp_prev.valid) { + height_above_land_point = pos_sp_prev.alt - pos_sp_curr.alt; + local_approach_entrance = _global_local_proj_ref.project(pos_sp_prev.lat, pos_sp_prev.lon); + + } else { + // no valid previous waypoint, construct one from the glide slope and direction from current + // position to land point + + // NOTE: this is not really a supported use case at the moment, this is just bandaiding any + // ill-advised usage of the current implementation + + // 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; + local_approach_entrance = local_position; + } + + if (height_above_land_point < FLT_EPSILON) { + height_above_land_point = FLT_EPSILON; + } + + _landing_approach_entrance_rel_alt = height_above_land_point; + + const float landing_approach_distance = _landing_approach_entrance_rel_alt / tanf(math::radians( + _param_fw_lnd_ang.get())); + + const Vector2f vector_aircraft_to_land_point = local_land_point - local_approach_entrance; + + if (vector_aircraft_to_land_point.norm_squared() > FLT_EPSILON) { + _landing_approach_entrance_offset_vector = -vector_aircraft_to_land_point.unit_or_zero() * landing_approach_distance; + + } else { + // land in direction of airframe + _landing_approach_entrance_offset_vector = Vector2f({cosf(_yaw), sinf(_yaw)}) * landing_approach_distance; + } + + // save time at which we started landing and reset abort_landing + reset_landing_state(); + _time_started_landing = now; + } +} + +Vector2f +FixedwingPositionControl::calculateTouchdownPosition(const float control_interval, const Vector2f &local_land_position) +{ + if (fabsf(_manual_control_setpoint.r) > MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE && _param_fw_lnd_nudge.get() > 0) { + // laterally nudge touchdown location with yaw stick + // positive is defined in the direction of a right hand turn starting from the approach vector direction + const float signed_deadzone_threshold = MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE * math::signNoZero( + _manual_control_setpoint.r); + _lateral_touchdown_position_offset += (_manual_control_setpoint.r - signed_deadzone_threshold) * + MAX_TOUCHDOWN_POSITION_NUDGE_RATE * control_interval; + _lateral_touchdown_position_offset = math::constrain(_lateral_touchdown_position_offset, -_param_fw_lnd_td_off.get(), + _param_fw_lnd_td_off.get()); + } + + const Vector2f approach_unit_vector = -_landing_approach_entrance_offset_vector.unit_or_zero(); + const Vector2f approach_unit_normal_vector{-approach_unit_vector(1), approach_unit_vector(0)}; + + return local_land_position + approach_unit_normal_vector * _lateral_touchdown_position_offset; +} + +Vector2f +FixedwingPositionControl::calculateLandingApproachVector() const +{ + Vector2f landing_approach_vector = -_landing_approach_entrance_offset_vector; + const Vector2f approach_unit_vector = landing_approach_vector.unit_or_zero(); + const Vector2f approach_unit_normal_vector{-approach_unit_vector(1), approach_unit_vector(0)}; + + // if _param_fw_lnd_nudge.get() == 0, no nudging + + if (_param_fw_lnd_nudge.get() == 1) { + // nudge the approach angle -- i.e. we adjust the approach vector to reach from the original approach + // entrance position to the newly nudged touchdown point + // NOTE: this lengthens the landing distance.. which will adjust the glideslope height slightly + landing_approach_vector += approach_unit_normal_vector * _lateral_touchdown_position_offset; + } + + // if _param_fw_lnd_nudge.get() == 2, the full path (including approach entrance point) is nudged with the touchdown + // point, which does not require any additions to the approach vector + + return landing_approach_vector; +} + +float +FixedwingPositionControl::getLandingTerrainAltitudeEstimate(const hrt_abstime &now, const float land_point_altitude) +{ + float terrain_alt = land_point_altitude; + + if (_param_fw_lnd_useter.get() == 1) { + if (_local_pos.dist_bottom_valid) { + // all good, have valid terrain altitude + float terrain_vpos = _local_pos.dist_bottom + _local_pos.z; + terrain_alt = (_local_pos.ref_alt - terrain_vpos); + _last_valid_terrain_alt_estimate = terrain_alt; + _last_time_terrain_alt_was_valid = now; + + } else if (_last_time_terrain_alt_was_valid == 0) { + // we have started landing phase but don't have valid terrain + // wait for some time, maybe we will soon get a valid estimate + // until then just use the altitude of the landing waypoint + if ((now - _time_started_landing) < TERRAIN_ALT_FIRST_MEASUREMENT_TIMEOUT) { + terrain_alt = land_point_altitude; + + } else { + // still no valid terrain, abort landing + terrain_alt = land_point_altitude; + abort_landing(true); + } + + } else if ((!_local_pos.dist_bottom_valid && (now - _last_time_terrain_alt_was_valid) < TERRAIN_ALT_TIMEOUT) + || _flaring) { + // use previous terrain estimate for some time and hope to recover + // if we are already flaring (land_noreturn_vertical) then just + // go with the old estimate + terrain_alt = _last_valid_terrain_alt_estimate; + + } else { + // terrain alt was not valid for long time, abort landing + terrain_alt = _last_valid_terrain_alt_estimate; + abort_landing(true); + } + } + + return terrain_alt; +} + void FixedwingPositionControl::publishLocalPositionSetpoint(const position_setpoint_s ¤t_waypoint) { if (_global_local_proj_ref.isInitialized()) { diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 62b3f2ef70..5868705c4b 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -58,7 +58,6 @@ #include #include #include -#include #include #include #include @@ -121,6 +120,9 @@ static constexpr float HDG_HOLD_MAN_INPUT_THRESH = 0.01f; // [us] time after which we abort landing if terrain estimate is not valid static constexpr hrt_abstime TERRAIN_ALT_TIMEOUT = 1_s; +// [us] if no altimeter measurement is made within this timeout, the land waypoint altitude is used for terrain altitude +static constexpr hrt_abstime TERRAIN_ALT_FIRST_MEASUREMENT_TIMEOUT = 10_s; + // [.] max throttle from user which will not lead to motors spinning up in altitude controlled modes static constexpr float THROTTLE_THRESH = 0.05f; @@ -152,6 +154,16 @@ static constexpr float MIN_PITCH_DURING_MANUAL_TAKEOFF = 0.0f; // altitude while waiting for navigator to flag it exceeded static constexpr float kClearanceAltitudeBuffer = 10.0f; +// [m] a very large number to hopefully avoid the "fly back" case in L1 waypoint following logic once passed the second +// waypoint in the segment. this is unecessary with NPFG. +static constexpr float L1_VIRTUAL_TAKEOFF_WP_DIST = 1.0e6f; + +// [m/s] maximum rate at which the touchdown position can be nudged +static constexpr float MAX_TOUCHDOWN_POSITION_NUDGE_RATE = 1.0f; + +// [.] normalized deadzone threshold for manual nudging input +static constexpr float MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE = 0.15f; + class FixedwingPositionControl final : public ModuleBase, public ModuleParams, public px4::WorkItem { @@ -304,18 +316,22 @@ private: // AUTO LANDING - /* Landing */ - bool _land_noreturn_horizontal{false}; - bool _land_noreturn_vertical{false}; - bool _land_stayonground{false}; - bool _land_motor_lim{false}; - bool _land_onslope{false}; + hrt_abstime _time_started_landing{0}; // [us] + + // [m] lateral touchdown position offset manually commanded during landing + float _lateral_touchdown_position_offset{0.0f}; + + // [m] relative vector from land point to approach entrance (NE) + Vector2f _landing_approach_entrance_offset_vector{}; + + // [m] relative height above land point + float _landing_approach_entrance_rel_alt{0.0f}; + bool _land_abort{false}; - Landingslope _landingslope; - - // [us] time at which landing started - hrt_abstime _time_started_landing{0}; + bool _flaring{false}; + hrt_abstime _time_started_flaring{0}; // [us] + float _heightrate_setpoint_at_flare_start{0.0f}; // [m/s] // [m] last terrain estimate which was valid float _last_valid_terrain_alt_estimate{0.0f}; @@ -323,15 +339,6 @@ private: // [us] time at which we had last valid terrain alt hrt_abstime _last_time_terrain_alt_was_valid{0}; - // [m] estimated height to ground at which flare started - float _flare_height{0.0f}; - - // [m] current forced (i.e. not determined using TECS) flare pitch setpoint - float _flare_pitch_sp{0.0f}; - - // [m] estimated height to ground at which flare started - float _flare_curve_alt_rel_last{0.0f}; - // AIRSPEED float _airspeed{0.0f}; @@ -557,14 +564,13 @@ private: * * @param now Current system time [us] * @param control_interval Time since last position control call [s] - * @param curr_pos Current 2D local position vector of vehicle [m] * @param control_interval Time since the last position control update [s] * @param ground_speed Local 2D ground speed of vehicle [m/s] * @param pos_sp_prev previous position setpoint * @param pos_sp_curr current position setpoint */ - void control_auto_landing(const hrt_abstime &now, const float control_interval, const Vector2d &curr_pos, - const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr); + void control_auto_landing(const hrt_abstime &now, const float control_interval, const Vector2f &ground_speed, + const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr); /* manual control methods */ @@ -684,6 +690,49 @@ private: */ Vector2f calculateTakeoffBearingVector(const Vector2f &launch_position, const Vector2f &takeoff_waypoint) const; + /** + * @brief Calculates the touchdown position for landing with optional manual lateral adjustments. + * + * Manual inputs (from the remote) are used to command a rate at which the position moves and the integrated + * position is bounded. This is useful for manually adjusting the landing point in real time when map or GNSS + * errors cause an offset from the desired landing vector. + * + * @param control_interval Time since the last position control update [s] + * @param local_land_position Originally commanded local land position (NE) [m] + * @return (Nudged) Local touchdown position (NE) [m] + */ + Vector2f calculateTouchdownPosition(const float control_interval, const Vector2f &local_land_position); + + /** + * @brief Calculates the vector from landing approach entrance to touchdown point + * + * NOTE: calculateTouchdownPosition() MUST be called before this method + * + * @return Landing approach vector [m] + */ + Vector2f calculateLandingApproachVector() const; + + /** + * @brief Returns a terrain altitude estimate with consideration of altimeter measurements. + * + * @param now Current system time [us] + * @param land_point_altitude Altitude (AMSL) of the land point [m] + * @return Terrain altitude (AMSL) [m] + */ + float getLandingTerrainAltitudeEstimate(const hrt_abstime &now, const float land_point_altitude); + + /** + * @brief Initializes landing states + * + * @param now Current system time [us] + * @param pos_sp_prev Previous position setpoint + * @param pos_sp_curr Current position setpoint + * @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); + DEFINE_PARAMETERS( (ParamFloat) _param_fw_airspd_max, @@ -718,10 +767,7 @@ private: (ParamFloat) _param_fw_lnd_fl_pmax, (ParamFloat) _param_fw_lnd_fl_pmin, (ParamFloat) _param_fw_lnd_flalt, - (ParamFloat) _param_fw_lnd_hhdist, - (ParamFloat) _param_fw_lnd_hvirt, (ParamFloat) _param_fw_thrtc_sc, - (ParamFloat) _param_fw_lnd_tlalt, (ParamBool) _param_fw_lnd_earlycfg, (ParamBool) _param_fw_lnd_useter, @@ -750,7 +796,6 @@ private: (ParamFloat) _param_fw_thr_trim, (ParamFloat) _param_fw_thr_idle, - (ParamFloat) _param_fw_thr_lnd_max, (ParamFloat) _param_fw_thr_max, (ParamFloat) _param_fw_thr_min, (ParamFloat) _param_fw_thr_slew_max, @@ -780,7 +825,12 @@ private: (ParamFloat) _param_fw_wing_height, (ParamFloat) _param_rwto_l1_period, - (ParamBool) _param_rwto_nudge + (ParamBool) _param_rwto_nudge, + + (ParamFloat) _param_fw_lnd_fl_time, + (ParamFloat) _param_fw_lnd_fl_sink, + (ParamFloat) _param_fw_lnd_td_off, + (ParamInt) _param_fw_lnd_nudge ) }; diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 5fa8dba5dd..f0eee7fed8 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -350,21 +350,6 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); */ PARAM_DEFINE_FLOAT(FW_THR_IDLE, 0.15f); -/** - * Throttle limit during landing below throttle limit altitude - * - * During the flare of the autonomous landing process, this value will be set - * as throttle limit when the aircraft altitude is below FW_LND_TLALT. - * - * @unit norm - * @min 0.0 - * @max 1.0 - * @decimal 2 - * @increment 0.01 - * @group FW L1 Control - */ -PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); - /** * Climbout Altitude difference * @@ -406,18 +391,6 @@ PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f); */ PARAM_DEFINE_FLOAT(FW_TKO_PITCH_MIN, 10.0f); -/** - * - * - * @unit m - * @min 1.0 - * @max 15.0 - * @decimal 1 - * @increment 0.5 - * @group FW L1 Control - */ -PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f); - /** * Landing flare altitude (relative to landing altitude) * @@ -430,35 +403,6 @@ PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f); */ PARAM_DEFINE_FLOAT(FW_LND_FLALT, 3.0f); -/** - * Landing throttle limit altitude (relative landing altitude) - * - * Default of -1.0 lets the system default to applying throttle - * limiting at 2/3 of the flare altitude. - * - * @unit m - * @min -1.0 - * @max 30.0 - * @decimal 1 - * @increment 0.5 - * @group FW L1 Control - */ -PARAM_DEFINE_FLOAT(FW_LND_TLALT, -1.0f); - -/** - * Landing heading hold horizontal distance. - * - * Set to 0 to disable heading hold. - * - * @unit m - * @min 0 - * @max 30.0 - * @decimal 1 - * @increment 0.5 - * @group FW L1 Control - */ -PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); - /** * Use terrain estimate during landing. * @@ -1034,3 +978,59 @@ PARAM_DEFINE_FLOAT(FW_WING_SPAN, 3.0); * @group FW Geometry */ PARAM_DEFINE_FLOAT(FW_WING_HEIGHT, 0.5); + +/** + * Landing flare time + * + * Multiplied by the descent rate to calculate a dynamic altitude at which + * to trigger the flare. + * + * @unit s + * @min 0.0 + * @max 5.0 + * @decimal 1 + * @increment 0.1 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_LND_FL_TIME, 1.0f); + +/** + * Landing flare sink rate + * + * TECS will attempt to control the aircraft to this sink rate via pitch angle (throttle killed during flare) + * + * @unit m/s + * @min 0.0 + * @max 1.0 + * @decimal 1 + * @increment 0.1 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_LND_FL_SINK, 0.25f); + +/** + * Maximum lateral position offset for the touchdown point + * + * @unit m + * @min 0.0 + * @max 10.0 + * @decimal 1 + * @increment 1 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_LND_TD_OFF, 3.0); + +/** + * Landing touchdown nudging option. + * + * Approach angle nudging: shifts the touchdown point laterally while keeping the approach entrance point constant + * Approach path nudging: shifts the touchdown point laterally along with the entire approach path + * + * @min 0 + * @max 2 + * @value 0 Disable nudging + * @value 1 Nudge approach angle + * @value 2 Nudge approach path + * @group FW L1 Control + */ +PARAM_DEFINE_INT32(FW_LND_NUDGE, 0); diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 65709ca186..01d5cbe406 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -85,6 +85,7 @@ void LoggedTopics::add_default_topics() add_topic("onboard_computer_status", 10); add_topic("parameter_update"); add_topic("position_controller_status", 500); + add_topic("position_controller_landing_status", 100); add_topic("position_setpoint_triplet", 200); add_optional_topic("px4io_status"); add_topic("radio_status"); diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index 1c658d09d8..ce5abd72bd 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -51,7 +51,6 @@ px4_add_module( vtol_takeoff.cpp DEPENDS geo - landing_slope geofence_breach_avoidance motion_planning ) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 678b133e04..f23d118bda 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -48,10 +48,8 @@ #include #include #include -#include #include #include -#include #include bool @@ -498,63 +496,12 @@ MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool } if (MissionBlock::item_contains_position(missionitem_previous)) { - - uORB::SubscriptionData landing_status{ORB_ID(position_controller_landing_status)}; - - const bool landing_status_valid = (landing_status.get().timestamp > 0); - const float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat, missionitem_previous.lon, - missionitem.lat, missionitem.lon); - - if (landing_status_valid && (wp_distance > landing_status.get().flare_length)) { - /* Last wp is before flare region */ - - const float delta_altitude = missionitem.altitude - missionitem_previous.altitude; - - if (delta_altitude < 0) { - - const float horizontal_slope_displacement = landing_status.get().horizontal_slope_displacement; - const float slope_angle_rad = landing_status.get().slope_angle_rad; - const float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude, - horizontal_slope_displacement, slope_angle_rad); - - if (missionitem_previous.altitude > slope_alt_req + 1.0f) { - /* Landing waypoint is above altitude of slope at the given waypoint distance (with small tolerance for floating point discrepancies) */ - const float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, - missionitem.altitude, horizontal_slope_displacement, slope_angle_rad); - - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: adjust landing approach.\t"); - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Move down %d m or move further away by %d m.\t", - (int)ceilf(slope_alt_req - missionitem_previous.altitude), - (int)ceilf(wp_distance_req - wp_distance)); - /* EVENT - * @description - * The landing waypoint must be above the altitude of slope at the given waypoint distance. - * Move it down {1m_v} or move it further away by {2m}. - */ - events::send(events::ID("navigator_mis_land_approach"), {events::Log::Error, events::LogInternal::Info}, - "Mission rejected: adjust landing approach", - (int)ceilf(slope_alt_req - missionitem_previous.altitude), - (int)ceilf(wp_distance_req - wp_distance)); - - return false; - } - - } else { - /* Landing waypoint is above last waypoint */ - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: landing above last waypoint.\t"); - events::send(events::ID("navigator_mis_land_too_high"), {events::Log::Error, events::LogInternal::Info}, - "Mission rejected: landing waypoint is above the last waypoint"); - return false; - } - - } else { - /* Last wp is in flare region */ - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: waypoint within landing flare.\t"); - events::send(events::ID("navigator_mis_land_within_flare"), {events::Log::Error, events::LogInternal::Info}, - "Mission rejected: waypoint is within landing flare"); - return false; - } - + // exact handling of the previous waypoint is currently done in the fixed-wing + // position control module and primarily supports approach entrances from + // orbit-to-alt mission items. Behavior with other entrance waypoint types, e.g. + // plain position setpoints is not guaranteed. it is the user's responsibility to + // appropriately plan the entrance point if not using orbit-to-alt until extended + // functionality is implemented. landing_valid = true; } else {