fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 11:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added PerformanceModel for fixed wing (#22091)

Browse Source 
* created a Performance Model for fixed wing vehicle
- added compensation for maximum climbrate, minimum sinkrate, minimum airspeed and trim airspeed based on weight ratio and air density
- added atmosphere lib to standard atmosphere calculations


---------

Signed-off-by: RomanBapst <bapstroman@gmail.com>
Co-authored-by: Thomas Stastny <thomas.stastny@auterion.com>
This commit is contained in:
Roman Bapst
2023-11-21 17:13:50 +01:00
committed by GitHub
co-authored by Thomas Stastny
parent 70cd224236
commit dd2322d622
40 changed files with 800 additions and 474 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control/FixedwingPositionControl.hpp
+7 -37
View File
@@ -49,11 +49,13 @@
#include "launchdetection/LaunchDetector.h"
#include "runway_takeoff/RunwayTakeoff.h"
#include <lib/fw_performance_model/PerformanceModel.hpp>
#include <float.h>
#include <drivers/drv_hrt.h>
#include <lib/geo/geo.h>
#include <lib/atmosphere/atmosphere.h>
#include <lib/npfg/npfg.hpp>
#include <lib/tecs/TECS.hpp>
#include <lib/mathlib/mathlib.h>
@@ -146,15 +148,6 @@ static constexpr float MIN_AUTO_TIMESTEP = 0.01f;
// [s] maximum time step between auto control updates
static constexpr float MAX_AUTO_TIMESTEP = 0.05f;
// [.] minimum ratio between the actual vehicle weight and the vehicle nominal weight (weight at which the performance limits are derived)
static constexpr float MIN_WEIGHT_RATIO = 0.5f;
// [.] maximum ratio between the actual vehicle weight and the vehicle nominal weight (weight at which the performance limits are derived)
static constexpr float MAX_WEIGHT_RATIO = 2.0f;
// air density of standard athmosphere at 5000m above mean sea level [kg/m^3]
static constexpr float AIR_DENSITY_STANDARD_ATMOS_5000_AMSL = 0.7363f;
// [rad] minimum pitch while airspeed has not yet reached a controllable value in manual position controlled takeoff modes
static constexpr float MIN_PITCH_DURING_MANUAL_TAKEOFF = 0.0f;
@@ -375,7 +368,7 @@ private:
float _airspeed{0.0f};
float _eas2tas{1.0f};
bool _airspeed_valid{false};
float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C};
float _air_density{atmosphere::kAirDensitySeaLevelStandardAtmos};
// [us] last time airspeed was received. used to detect timeouts.
hrt_abstime _time_airspeed_last_valid{0};
@@ -423,6 +416,8 @@ private:
// nonlinear path following guidance - lateral-directional position control
NPFG _npfg;
PerformanceModel _performance_model;
// LANDING GEAR
int8_t _new_landing_gear_position{landing_gear_s::GEAR_KEEP};
@@ -454,7 +449,7 @@ private:
#endif // CONFIG_FIGURE_OF_EIGHT
// Update our local parameter cache.
int parameters_update();
void parameters_update();
// Update subscriptions
void airspeed_poll();
@@ -471,6 +466,7 @@ private:
void tecs_status_publish(float alt_sp, float equivalent_airspeed_sp, float true_airspeed_derivative_raw,
float throttle_trim);
void publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint);
float getLoadFactor();
/**
* @brief Sets the landing abort status and publishes landing status.
@@ -713,17 +709,6 @@ private:
*/
void set_control_mode_current(const hrt_abstime &now);
/**
* @brief Estimate trim throttle for air density, vehicle weight and current airspeed
*
* @param throttle_min Minimum allowed trim throttle.
* @param throttle_max Maximum allowed trim throttle.
* @param airspeed_sp Current airspeed setpoint (CAS) [m/s]
* @return Estimated trim throttle
*/
float calculateTrimThrottle(float throttle_min, float throttle_max,
float airspeed_sp);
void publishOrbitStatus(const position_setpoint_s pos_sp);
SlewRate<float> _airspeed_slew_rate_controller;
@@ -909,12 +894,6 @@ private:
const matrix::Vector2f &wind_vel);
DEFINE_PARAMETERS(
(ParamFloat<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max,
(ParamFloat<px4::params::FW_AIRSPD_MIN>) _param_fw_airspd_min,
(ParamFloat<px4::params::FW_AIRSPD_TRIM>) _param_fw_airspd_trim,
(ParamFloat<px4::params::FW_AIRSPD_STALL>) _param_fw_airspd_stall,
(ParamFloat<px4::params::FW_GND_SPD_MIN>) _param_fw_gnd_spd_min,
(ParamFloat<px4::params::FW_PN_R_SLEW_MAX>) _param_fw_pn_r_slew_max,
@@ -944,7 +923,6 @@ private:
(ParamFloat<px4::params::FW_P_LIM_MAX>) _param_fw_p_lim_max,
(ParamFloat<px4::params::FW_P_LIM_MIN>) _param_fw_p_lim_min,
(ParamFloat<px4::params::FW_T_CLMB_MAX>) _param_fw_t_clmb_max,
(ParamFloat<px4::params::FW_T_HRATE_FF>) _param_fw_t_hrate_ff,
(ParamFloat<px4::params::FW_T_ALT_TC>) _param_fw_t_h_error_tc,
(ParamFloat<px4::params::FW_T_I_GAIN_THR>) _param_fw_t_I_gain_thr,
@@ -952,7 +930,6 @@ private:
(ParamFloat<px4::params::FW_T_PTCH_DAMP>) _param_fw_t_ptch_damp,
(ParamFloat<px4::params::FW_T_RLL2THR>) _param_fw_t_rll2thr,
(ParamFloat<px4::params::FW_T_SINK_MAX>) _param_fw_t_sink_max,
(ParamFloat<px4::params::FW_T_SINK_MIN>) _param_fw_t_sink_min,
(ParamFloat<px4::params::FW_T_SPDWEIGHT>) _param_fw_t_spdweight,
(ParamFloat<px4::params::FW_T_TAS_TC>) _param_fw_t_tas_error_tc,
(ParamFloat<px4::params::FW_T_THR_DAMP>) _param_fw_t_thr_damp,
@@ -965,10 +942,6 @@ private:
(ParamFloat<px4::params::FW_T_SPD_DEV_STD>) _param_speed_rate_standard_dev,
(ParamFloat<px4::params::FW_T_SPD_PRC_STD>) _param_process_noise_standard_dev,
(ParamFloat<px4::params::FW_THR_ASPD_MIN>) _param_fw_thr_aspd_min,
(ParamFloat<px4::params::FW_THR_ASPD_MAX>) _param_fw_thr_aspd_max,
(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_MAX>) _param_fw_thr_max,
(ParamFloat<px4::params::FW_THR_MIN>) _param_fw_thr_min,
@@ -995,9 +968,6 @@ private:
(ParamFloat<px4::params::NAV_FW_ALT_RAD>) _param_nav_fw_alt_rad,
(ParamFloat<px4::params::WEIGHT_BASE>) _param_weight_base,
(ParamFloat<px4::params::WEIGHT_GROSS>) _param_weight_gross,
(ParamFloat<px4::params::FW_WING_SPAN>) _param_fw_wing_span,
(ParamFloat<px4::params::FW_WING_HEIGHT>) _param_fw_wing_height,