fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 07:28:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

fixed-wing runway takeoff: climbout at specified takeoff airspeed and max climb rate

Browse Source 
TECS climbout mode was used for takeoff climbout, which puts throttle to full and does not regulate a specific airspeed.
This commit sets the desired takeoff airspeed explicitly and allows max climb rate to track the ascent.
This commit is contained in:
Thomas Stastny
2022-11-22 13:46:25 -05:00
committed by Daniel Agar
parent 47963b5b67
commit a4349193b5
4 changed files with 31 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+27 -14
View File
@@ -977,7 +977,8 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i
_param_fw_thr_max.get(),
false,
_param_fw_p_lim_min.get(),
_param_sinkrate_target.get());
_param_sinkrate_target.get(),
_param_climbrate_target.get());
_att_sp.roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle
_att_sp.yaw_body = 0.f;
@@ -1012,6 +1013,7 @@ FixedwingPositionControl::control_auto_descend(const float control_interval)
false,
_param_fw_p_lim_min.get(),
_param_sinkrate_target.get(),
_param_climbrate_target.get(),
false,
descend_rate);
@@ -1198,7 +1200,8 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
tecs_fw_thr_max,
false,
radians(_param_fw_p_lim_min.get()),
_param_sinkrate_target.get());
_param_sinkrate_target.get(),
_param_climbrate_target.get());
}
void
@@ -1257,6 +1260,7 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
false,
radians(_param_fw_p_lim_min.get()),
_param_sinkrate_target.get(),
_param_climbrate_target.get(),
tecs_status_s::TECS_MODE_NORMAL,
pos_sp_curr.vz);
}
@@ -1377,7 +1381,8 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
tecs_fw_thr_max,
false,
radians(_param_fw_p_lim_min.get()),
_param_sinkrate_target.get());
_param_sinkrate_target.get(),
_param_climbrate_target.get());
}
void
@@ -1486,8 +1491,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
// update tecs
const float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_param_fw_p_lim_max.get());
const float takeoff_pitch_min_climbout_deg = _runway_takeoff.getMinPitch(_takeoff_pitch_min.get(),
_param_fw_p_lim_min.get());
const float takeoff_pitch_min_deg = _runway_takeoff.getMinPitch(_takeoff_pitch_min.get(),
_param_fw_p_lim_min.get());
if (_runway_takeoff.resetIntegrators()) {
// reset integrals except yaw (which also counts for the wheel controller)
@@ -1500,13 +1505,14 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
tecs_update_pitch_throttle(control_interval,
altitude_setpoint_amsl,
target_airspeed,
radians(_param_fw_p_lim_min.get()),
radians(takeoff_pitch_min_deg),
radians(takeoff_pitch_max_deg),
_param_fw_thr_min.get(),
_param_fw_thr_max.get(),
_runway_takeoff.climbout(),
radians(takeoff_pitch_min_climbout_deg),
_param_sinkrate_target.get());
false,
radians(takeoff_pitch_min_deg),
_param_sinkrate_target.get(),
_param_fw_t_clmb_max.get());
_tecs.set_equivalent_airspeed_min(_param_fw_airspd_min.get()); // reset after TECS calculation
@@ -1603,7 +1609,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
takeoff_throttle,
true,
radians(_takeoff_pitch_min.get()),
_param_sinkrate_target.get());
_param_sinkrate_target.get(),
_param_climbrate_target.get());
} else {
tecs_update_pitch_throttle(control_interval,
@@ -1615,7 +1622,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
takeoff_throttle,
false,
radians(_param_fw_p_lim_min.get()),
_param_sinkrate_target.get());
_param_sinkrate_target.get(),
_param_climbrate_target.get());
}
if (_launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
@@ -1765,6 +1773,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
false,
pitch_min_rad,
_param_sinkrate_target.get(),
_param_climbrate_target.get(),
true,
height_rate_setpoint);
@@ -1834,7 +1843,8 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
_param_fw_thr_max.get(),
false,
radians(_param_fw_p_lim_min.get()),
desired_max_sinkrate);
desired_max_sinkrate,
_param_climbrate_target.get());
/* set the attitude and throttle commands */
@@ -1897,6 +1907,7 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval,
false,
min_pitch,
_param_sinkrate_target.get(),
_param_climbrate_target.get(),
false,
height_rate_sp);
@@ -2005,6 +2016,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
false,
min_pitch,
_param_sinkrate_target.get(),
_param_climbrate_target.get(),
false,
height_rate_sp);
@@ -2447,7 +2459,8 @@ float FixedwingPositionControl::compensateTrimThrottleForDensityAndWeight(float
void
FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp,
float pitch_min_rad, float pitch_max_rad, float throttle_min, float throttle_max, bool climbout_mode,
float climbout_pitch_min_rad, const float desired_max_sinkrate, bool disable_underspeed_detection, float hgt_rate_sp)
float climbout_pitch_min_rad, const float desired_max_sinkrate, const float desired_max_climbrate,
bool disable_underspeed_detection, float hgt_rate_sp)
{
_tecs_is_running = true;
@@ -2527,7 +2540,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
throttle_trim_comp,
pitch_min_rad - radians(_param_fw_psp_off.get()),
pitch_max_rad - radians(_param_fw_psp_off.get()),
_param_climbrate_target.get(),
desired_max_climbrate,
desired_max_sinkrate,
hgt_rate_sp);
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+4 -2
View File
@@ -698,12 +698,14 @@ private:
* @param climbout_mode True if TECS should engage climbout mode
* @param climbout_pitch_min_rad Minimum pitch angle command in climbout mode [rad]
* @param desired_max_sink_rate The desired max sink rate commandable when altitude errors are large [m/s]
* @param desired_max_climb_rate The desired max climb rate commandable when altitude errors are large [m/s]
* @param disable_underspeed_detection True if underspeed detection should be disabled
* @param hgt_rate_sp Height rate setpoint [m/s]
*/
void tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp, float pitch_min_rad,
float pitch_max_rad, float throttle_min, float throttle_max, bool climbout_mode, float climbout_pitch_min_rad,
const float desired_max_sink_rate, bool disable_underspeed_detection = false, float hgt_rate_sp = NAN);
float pitch_max_rad, float throttle_min, float throttle_max, bool climbout_mode,
float climbout_pitch_min_rad, const float desired_max_sink_rate, const float desired_max_climb_rate,
bool disable_underspeed_detection = false, float hgt_rate_sp = NAN);
/**
* @brief Constrains the roll angle setpoint near ground to avoid wingtip strike.
src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp
-2
View File
@@ -57,7 +57,6 @@ void RunwayTakeoff::init(const hrt_abstime &time_now, const float initial_yaw, c
initial_yaw_ = initial_yaw;
start_pos_global_ = start_pos_global;
takeoff_state_ = RunwayTakeoffState::THROTTLE_RAMP;
climbout_ = true; // this is true until climbout is finished
initialized_ = true;
time_initialized_ = time_now;
takeoff_time_ = 0;
@@ -91,7 +90,6 @@ void RunwayTakeoff::update(const hrt_abstime &time_now, const float takeoff_airs
case RunwayTakeoffState::CLIMBOUT:
if (vehicle_altitude > clearance_altitude) {
climbout_ = false;
takeoff_state_ = RunwayTakeoffState::FLY;
mavlink_log_info(mavlink_log_pub, "Reached clearance altitude\t");
events::send(events::ID("runway_takeoff_reached_clearance_altitude"), events::Log::Info, "Reached clearance altitude");
src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h
-10
View File
@@ -114,11 +114,6 @@ public:
*/
bool controlYaw();
/**
* @return TECS should be commanded to climbout mode
*/
bool climbout() { return climbout_; }
/**
* @param external_pitch_setpoint Externally commanded pitch angle setpoint (usually from TECS) [rad]
* @return Pitch angle setpoint (limited while plane is on runway) [rad]
@@ -225,11 +220,6 @@ private:
*/
float initial_yaw_{0.f};
/**
* True if TECS should be commanded to "climbout" mode.
*/
bool climbout_{false};
/**
* The global (lat, lon) position of the vehicle on first pass through the runway takeoff state machine. The
* takeoff path emanates from this point to correct for any GNSS uncertainty from the planned takeoff point. The