mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-30 18:06:39 +08:00
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
committed by
Daniel Agar
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
|
||||
|
||||
bool abort_landing # true if landing should be aborted
|
||||
uint64 timestamp # [us] time since system start
|
||||
float32 lateral_touchdown_offset # [m] lateral touchdown position offset manually commanded during landing
|
||||
bool abort_landing # true if landing should be aborted
|
||||
|
||||
@@ -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
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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 {
|
||||
|
||||
Reference in New Issue
Block a user