forked from Archive/PX4-Autopilot
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
Daniel Agar
parent
f962399ba1
commit
87e09ad9f5
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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_ */
|
||||
|
|
@ -43,7 +43,6 @@ px4_add_module(
|
|||
DEPENDS
|
||||
l1
|
||||
launchdetection
|
||||
landing_slope
|
||||
npfg
|
||||
runway_takeoff
|
||||
SlewRate
|
||||
|
|
|
|||
|
|
@ -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 ¤t_waypoint)
|
||||
{
|
||||
if (_global_local_proj_ref.isInitialized()) {
|
||||
|
|
|
|||
|
|
@ -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
|
||||
)
|
||||
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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");
|
||||
|
|
|
|||
|
|
@ -51,7 +51,6 @@ px4_add_module(
|
|||
vtol_takeoff.cpp
|
||||
DEPENDS
|
||||
geo
|
||||
landing_slope
|
||||
geofence_breach_avoidance
|
||||
motion_planning
|
||||
)
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
|
|
|
|||
Loading…
Reference in New Issue