Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 08:36:08 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

fw_pos_control_l1: integrate optional use of npfg

Browse Source 
uorb: npfg_status msg
This commit is contained in:
Thomas Stastny
2021-08-24 13:48:14 +02:00
committed by JaeyoungLim
parent ef2cc9abb7
commit d240ee9448
6 changed files with 492 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/CMakeLists.txt
+1
View File
@@ -100,6 +100,7 @@ set(msg_files
mount_orientation.msg
multirotor_motor_limits.msg
navigator_mission_item.msg
npfg_status.msg
obstacle_distance.msg
offboard_control_mode.msg
onboard_computer_status.msg
msg/npfg_status.msg
+16
View File
@@ -0,0 +1,16 @@
uint64 timestamp # time since system start (microseconds)
uint8 wind_est_valid # (boolean) true = wind estimate is valid and being used by controller
float32 lat_accel # resultant lateral acceleration reference [m/s^2]
float32 lat_accel_ff # lateral acceleration demand only for maintaining curvature [m/s^2]
float32 bearing_feas # bearing feasibility [0,1]
float32 bearing_feas_on_track # on-track bearing feasibility [0,1]
float32 signed_track_error # signed track error [m]
float32 track_error_bound # track error bound [m]
float32 airspeed_ref # airspeed reference [m/s]
float32 bearing # bearing angle [rad]
float32 heading_ref # heading angle reference [rad]
float32 min_ground_speed_ref # minimum forward ground speed reference [m/s]
float32 adapted_period # adapted period (if auto-tuning enabled) [s]
float32 p_gain # controller proportional gain [rad/s]
float32 time_const # controller time constant [s]
src/modules/fw_pos_control_l1/CMakeLists.txt
+1
View File
@@ -44,6 +44,7 @@ px4_add_module(
l1
launchdetection
landing_slope
npfg
runway_takeoff
tecs
)
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+297 -63
View File
File diff suppressed because it is too large Load Diff
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+26
View File
@@ -56,6 +56,7 @@
#include <drivers/drv_hrt.h>
#include <lib/ecl/geo/geo.h>
#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>
@@ -71,6 +72,7 @@
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/airspeed_validated.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/npfg_status.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_controller_landing_status.h>
#include <uORB/topics/position_controller_status.h>
@@ -87,6 +89,7 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/wind.h>
#include <uORB/uORB.h>
#include <vtol_att_control/vtol_type.h>
@@ -151,8 +154,10 @@ private:
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _wind_sub{ORB_ID(wind)};
uORB::Publication<vehicle_attitude_setpoint_s> _attitude_sp_pub;
uORB::Publication<npfg_status_s> _npfg_status_pub{ORB_ID(npfg_status)}; ///< NPFG status publication
uORB::Publication<position_controller_status_s> _pos_ctrl_status_pub{ORB_ID(position_controller_status)}; ///< navigation capabilities publication
uORB::Publication<position_controller_landing_status_s> _pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)}; ///< landing status publication
uORB::Publication<tecs_status_s> _tecs_status_pub{ORB_ID(tecs_status)}; ///< TECS status publication
@@ -228,6 +233,11 @@ private:
float _airspeed{0.0f};
float _eas2tas{1.0f};
/* wind estimates */
Vector2f _wind_vel{0.0f, 0.0f}; ///< wind velocity vector [m/s]
bool _wind_valid{false}; ///< flag if a valid wind estimate exists
hrt_abstime _time_wind_last_received{0}; ///< last time wind estimate was received in microseconds. Used to detect timeouts.
float _pitch{0.0f};
float _yaw{0.0f};
float _yawrate{0.0f};
@@ -254,6 +264,7 @@ private:
float _manual_control_setpoint_airspeed{0.0f};
ECL_L1_Pos_Controller _l1_control;
NPFG _npfg;
TECS _tecs;
uint8_t _type{0};
@@ -278,6 +289,7 @@ private:
void vehicle_command_poll();
void vehicle_control_mode_poll();
void vehicle_status_poll();
void wind_poll();
void status_publish();
void landing_status_publish();
@@ -363,6 +375,20 @@ private:
(ParamFloat<px4::params::FW_L1_R_SLEW_MAX>) _param_fw_l1_r_slew_max,
(ParamFloat<px4::params::FW_R_LIM>) _param_fw_r_lim,
(ParamBool<px4::params::FW_USE_NPFG>) _param_fw_use_npfg,
(ParamFloat<px4::params::NPFG_PERIOD>) _param_npfg_period,
(ParamFloat<px4::params::NPFG_DAMPING>) _param_npfg_damping,
(ParamBool<px4::params::NPFG_LB_PERIOD>) _param_npfg_en_period_lb,
(ParamBool<px4::params::NPFG_UB_PERIOD>) _param_npfg_en_period_ub,
(ParamBool<px4::params::NPFG_RAMP_PERIOD>) _param_npfg_ramp_adapted_period,
(ParamBool<px4::params::NPFG_TRACK_KEEP>) _param_npfg_en_track_keeping,
(ParamBool<px4::params::NPFG_EN_MIN_GSP>) _param_npfg_en_min_gsp,
(ParamBool<px4::params::NPFG_WIND_REG>) _param_npfg_en_wind_reg,
(ParamFloat<px4::params::NPFG_GSP_MAX_TK>) _param_npfg_track_keeping_gsp_max,
(ParamFloat<px4::params::NPFG_NTE_FRAC>) _param_npfg_nte_fraction,
(ParamFloat<px4::params::NPFG_ROLL_TC>) _param_npfg_roll_time_const,
(ParamFloat<px4::params::NPFG_WR_BUF>) _param_npfg_wind_ratio_buf,
(ParamFloat<px4::params::FW_LND_AIRSPD_SC>) _param_fw_lnd_airspd_sc,
(ParamFloat<px4::params::FW_LND_ANG>) _param_fw_lnd_ang,
(ParamFloat<px4::params::FW_LND_FL_PMAX>) _param_fw_lnd_fl_pmax,
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+151
View File
@@ -85,6 +85,157 @@ PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f);
*/
PARAM_DEFINE_FLOAT(FW_L1_R_SLEW_MAX, 90.0f);
/**
* Use NPFG as lateral-directional guidance law for fixed-wing vehicles
*
* Replaces L1.
*
* @boolean
* @group FW NPFG Control
*/
PARAM_DEFINE_INT32(FW_USE_NPFG, 0);
/**
* NPFG period
*
* Period of the NPFG control law.
*
* @unit s
* @min 5.0
* @max 40.0
* @decimal 1
* @increment 0.5
* @group FW NPFG Control
*/
PARAM_DEFINE_FLOAT(NPFG_PERIOD, 30.0f);
/**
* NPFG damping ratio
*
* Damping ratio of the NPFG control law.
*
* @min 0.10
* @max 1.00
* @decimal 2
* @increment 0.05
* @group FW NPFG Control
*/
PARAM_DEFINE_FLOAT(NPFG_DAMPING, 0.25f);
/**
* Enable automatic lower bound on the NPFG period
*
* Avoids limit cycling from a too aggressively tuned period/damping combination.
* If set to false, also disables the upper bound NPFG_PERIOD_UB.
*
* @boolean
* @group FW NPFG Control
*/
PARAM_DEFINE_INT32(NPFG_LB_PERIOD, 1);
/**
* Enable automatic upper bound on the NPFG period
*
* Adapts period to maintain track keeping in variable winds and path curvature.
*
* @boolean
* @group FW NPFG Control
*/
PARAM_DEFINE_INT32(NPFG_UB_PERIOD, 1);
/**
* Ramp in automatic period adaptations with track proximity
*
* @boolean
* @group FW NPFG Control
*/
PARAM_DEFINE_INT32(NPFG_RAMP_PERIOD, 1);
/**
* Enable track keeping excess wind handling logic.
*
* @boolean
* @group FW NPFG Control
*/
PARAM_DEFINE_INT32(NPFG_TRACK_KEEP, 1);
/**
* Enable minimum ground speed maintaining excess wind handling logic
*
* @boolean
* @group FW NPFG Control
*/
PARAM_DEFINE_INT32(NPFG_EN_MIN_GSP, 1);
/**
* Enable wind excess regulation.
*
* Disabling this parameter further disables all other airspeed incrementation options.
*
* @boolean
* @group FW NPFG Control
*/
PARAM_DEFINE_INT32(NPFG_WIND_REG, 1);
/**
* Maximum, minimum forward ground speed for track keeping in excess wind
*
* The maximum value of the minimum forward ground speed that may be commanded
* by the track keeping excess wind handling logic. Occurs at the track error
* boundary * normalized track error fraction and is reduced to zero on track.
*
* @unit m/s
* @min 0.0
* @max 10.0
* @decimal 1
* @increment 0.5
* @group FW NPFG Control
*/
PARAM_DEFINE_FLOAT(NPFG_GSP_MAX_TK, 5.0f);
/**
* Normalized track error fraction
*
* Determines at what fraction of the normalized track error the maximum track keeping
* forward ground speed demand is reached.
*
* @min 0.1
* @max 1.0
* @decimal 1
* @increment 0.1
* @group FW NPFG Control
*/
PARAM_DEFINE_FLOAT(NPFG_NTE_FRAC, 0.5f);
/**
* Roll time constant
*
* Time constant of roll controller command / response, modeled as first order delay.
*
* @unit s
* @min 0.05
* @max 2.00
* @decimal 2
* @increment 0.05
* @group FW NPFG Control
*/
PARAM_DEFINE_FLOAT(NPFG_ROLL_TC, 0.5f);
/**
* NPFG wind ratio buffer
*
* The size of the feasibility transition region at cross wind angle >= 90 deg.
* This must be non-zero to avoid jumpy airspeed incrementation while using wind
* excess handling logic.
*
* @min 0.01
* @max 0.30
* @decimal 2
* @increment 0.01
* @group FW NPFG Control
*/
PARAM_DEFINE_FLOAT(NPFG_WR_BUF, 0.1f);
/**
* Cruise throttle
*