fw pos ctrl: auto landing refactor

- landing slope/curve library removed
- flare curve removed (the position setpoints will not be tracked during a flare, and were being ignored by open-loop maneuvers anyway)
- flare curve replaced by simply commanding a constant glide slope to the ground from the approach entrance, and commanding a sink rate once below flaring alt
- flare is now time-to-touchdown -based to account for differing descent rates (e.g. due to wind)
- flare pitch limits and height rate commands are ramped in from the previous iteration's values at flare onset to avoid jumpy commands
- TECS controls all aspects of the auto landing airspeed and altitude/height rate, and is only constrained by pitch and throttle limits (lessening unintuitive open loop manuever overrides)
- throttle is killed on flare
- flare is the singular point of no return during landing
- lateral manual nudging of the touchdown point is configurable via parameter, allowing the operator to nudge (via remote) either the touchdown point itself (adjusting approach vector) or shifting the entire approach path to the left or right. this helps when GCS map or GNSS uncertainties set the aircraft on a slightly offset approach"
This commit is contained in:
Thomas Stastny 2022-06-14 17:02:36 -05:00 committed by Daniel Agar
parent f962399ba1
commit 87e09ad9f5
22 changed files with 394 additions and 753 deletions

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -1,9 +1,3 @@
uint64 timestamp # time since system start (microseconds)
float32 horizontal_slope_displacement
float32 slope_angle_rad
float32 flare_length
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

View File

@ -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)

View File

@ -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)

View File

@ -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 <thomasgubler@gmail.com>
*/
#include "Landingslope.hpp"
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
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;
}

View File

@ -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 <thomasgubler@gmail.com>
*/
#ifndef LANDINGSLOPE_H_
#define LANDINGSLOPE_H_
#include <math.h>
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_ */

View File

@ -43,7 +43,6 @@ px4_add_module(
DEPENDS
l1
launchdetection
landing_slope
npfg
runway_takeoff
SlewRate

View File

@ -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);
} 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;
}
// save time at which we started landing and reset abort_landing
if (_time_started_landing == 0) {
reset_landing_state();
_time_started_landing = now;
}
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));
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;
}
_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");
}
/* Vertical landing control */
/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
// default to no terrain estimation, just use landing waypoint altitude
float terrain_alt = pos_sp_curr.alt;
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;
}
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);
/* lateral guidance */
if (_param_fw_use_npfg.get()) {
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
// 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);
if (_land_noreturn_horizontal) {
// heading hold
_npfg.navigateHeading(_target_bearing, ground_speed, _wind_vel);
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 {
// normal navigation
_npfg.navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
// continue horizontally
altitude_setpoint = _current_altitude;
}
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
_att_sp.roll_body = _npfg.getRollSetpoint();
// 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());
} else {
if (_land_noreturn_horizontal) {
// heading hold
_l1_control.navigate_heading(_target_bearing, _yaw, ground_speed);
if ((_current_altitude < terrain_alt + flare_rel_alt) || _flaring) {
// flare and land with minimal speed
} else {
// normal navigation
_l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, 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;
} else {
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
}
tecs_update_pitch_throttle(control_interval,
terrain_alt + flare_curve_alt_rel,
target_airspeed,
radians(_param_fw_lnd_fl_pmin.get()),
radians(_param_fw_lnd_fl_pmax.get()),
0.0f,
throttle_max,
false,
_land_motor_lim ? radians(_param_fw_lnd_fl_pmin.get()) : radians(_param_fw_p_lim_min.get()),
true);
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;
// 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_vertical = true;
} 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);
}
// otherwise continue using previous _flare_pitch_sp
}
/* lateral guidance first, because npfg will adjust the airspeed setpoint if necessary */
_att_sp.pitch_body = _flare_pitch_sp;
_flare_curve_alt_rel_last = flare_curve_alt_rel;
// 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());
} else {
// align heading with the approach bearing
const float landing_approach_bearing = atan2f(landing_approach_vector(1), landing_approach_vector(0));
/* 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);
/* 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();
}
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
/* longitudinal guidance */
// 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,
altitude_desired,
altitude_setpoint,
target_airspeed,
pitch_min_rad,
pitch_max_rad,
0.0f,
0.0f,
false,
pitch_min_rad,
true,
height_rate_setpoint);
/* 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();
// flaring is just before touchdown, align the yaw as much as possible with the landing vector
_att_sp.yaw_body = landing_approach_bearing;
// enable direct yaw control using rudder/wheel
_att_sp.fw_control_yaw = true;
_att_sp.thrust_body[0] = 0.0f;
} else {
// 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);
_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 {
// 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)
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();
}
/* longitudinal guidance */
tecs_update_pitch_throttle(control_interval,
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 &current_waypoint)
{
if (_global_local_proj_ref.isInitialized()) {

View File

@ -58,7 +58,6 @@
#include <lib/l1/ECL_L1_Pos_Controller.hpp>
#include <lib/npfg/npfg.hpp>
#include <lib/tecs/TECS.hpp>
#include <lib/landing_slope/Landingslope.hpp>
#include <lib/mathlib/mathlib.h>
#include <lib/perf/perf_counter.h>
#include <lib/slew_rate/SlewRate.hpp>
@ -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<FixedwingPositionControl>, 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<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max,
@ -718,10 +767,7 @@ private:
(ParamFloat<px4::params::FW_LND_FL_PMAX>) _param_fw_lnd_fl_pmax,
(ParamFloat<px4::params::FW_LND_FL_PMIN>) _param_fw_lnd_fl_pmin,
(ParamFloat<px4::params::FW_LND_FLALT>) _param_fw_lnd_flalt,
(ParamFloat<px4::params::FW_LND_HHDIST>) _param_fw_lnd_hhdist,
(ParamFloat<px4::params::FW_LND_HVIRT>) _param_fw_lnd_hvirt,
(ParamFloat<px4::params::FW_LND_THRTC_SC>) _param_fw_thrtc_sc,
(ParamFloat<px4::params::FW_LND_TLALT>) _param_fw_lnd_tlalt,
(ParamBool<px4::params::FW_LND_EARLYCFG>) _param_fw_lnd_earlycfg,
(ParamBool<px4::params::FW_LND_USETER>) _param_fw_lnd_useter,
@ -750,7 +796,6 @@ private:
(ParamFloat<px4::params::FW_THR_TRIM>) _param_fw_thr_trim,
(ParamFloat<px4::params::FW_THR_IDLE>) _param_fw_thr_idle,
(ParamFloat<px4::params::FW_THR_LND_MAX>) _param_fw_thr_lnd_max,
(ParamFloat<px4::params::FW_THR_MAX>) _param_fw_thr_max,
(ParamFloat<px4::params::FW_THR_MIN>) _param_fw_thr_min,
(ParamFloat<px4::params::FW_THR_SLEW_MAX>) _param_fw_thr_slew_max,
@ -780,7 +825,12 @@ private:
(ParamFloat<px4::params::FW_WING_HEIGHT>) _param_fw_wing_height,
(ParamFloat<px4::params::RWTO_L1_PERIOD>) _param_rwto_l1_period,
(ParamBool<px4::params::RWTO_NUDGE>) _param_rwto_nudge
(ParamBool<px4::params::RWTO_NUDGE>) _param_rwto_nudge,
(ParamFloat<px4::params::FW_LND_FL_TIME>) _param_fw_lnd_fl_time,
(ParamFloat<px4::params::FW_LND_FL_SINK>) _param_fw_lnd_fl_sink,
(ParamFloat<px4::params::FW_LND_TD_OFF>) _param_fw_lnd_td_off,
(ParamInt<px4::params::FW_LND_NUDGE>) _param_fw_lnd_nudge
)
};

View File

@ -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);

View File

@ -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");

View File

@ -51,7 +51,6 @@ px4_add_module(
vtol_takeoff.cpp
DEPENDS
geo
landing_slope
geofence_breach_avoidance
motion_planning
)

View File

@ -48,10 +48,8 @@
#include <drivers/drv_pwm_output.h>
#include <lib/geo/geo.h>
#include <lib/mathlib/mathlib.h>
#include <lib/landing_slope/Landingslope.hpp>
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/position_controller_landing_status.h>
#include <px4_platform_common/events.h>
bool
@ -498,63 +496,12 @@ MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool
}
if (MissionBlock::item_contains_position(missionitem_previous)) {
uORB::SubscriptionData<position_controller_landing_status_s> 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<int32_t, int32_t>(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 {