From 47963b5b67af791b1d5fd042872fc1ffa0db36ee Mon Sep 17 00:00:00 2001 From: Thomas Stastny Date: Wed, 5 Oct 2022 20:51:31 +0200 Subject: [PATCH] fixed-wing runway takeoff: define explicit takeoff speeds previously a scale factor param on min airspeed was used to define the climbout airspeed for runway takeoff additionally, the rotation speed was defined by another hardcoded scale on top of the previously scaled min airspeed this commit explicitly defines a takeoff speed and rotation speed for runway takeoff in params, with option to disable --- .../FixedwingPositionControl.cpp | 6 ++-- .../FixedwingPositionControl.hpp | 4 ++- .../fw_pos_control_l1_params.c | 15 +++++++++ .../runway_takeoff/RunwayTakeoff.cpp | 26 +++++++++------- .../runway_takeoff/RunwayTakeoff.h | 13 +++----- .../runway_takeoff/runway_takeoff_params.c | 31 ++++++++++--------- 6 files changed, 57 insertions(+), 38 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 3f4bea7885..65ce5a289a 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -1412,11 +1412,13 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _runway_takeoff.forceSetFlyState(); } - _runway_takeoff.update(now, _airspeed, _current_altitude - _takeoff_ground_alt, + const float takeoff_airspeed = (_param_fw_tko_airspd.get() > FLT_EPSILON) ? _param_fw_tko_airspd.get() : + _param_fw_airspd_min.get(); + + _runway_takeoff.update(now, takeoff_airspeed, _airspeed, _current_altitude - _takeoff_ground_alt, clearance_altitude_amsl - _takeoff_ground_alt, &_mavlink_log_pub); - const float takeoff_airspeed = _runway_takeoff.getMinAirspeedScaling() * _param_fw_airspd_min.get(); float adjusted_min_airspeed = _param_fw_airspd_min.get(); if (takeoff_airspeed < _param_fw_airspd_min.get()) { diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 1e9cc2348b..c67fa81a00 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -865,7 +865,9 @@ private: (ParamInt) _param_fw_lnd_nudge, (ParamInt) _param_fw_lnd_abort, - (ParamFloat) _param_fw_wind_arsp_sc + (ParamFloat) _param_fw_wind_arsp_sc, + + (ParamFloat) _param_fw_tko_airspd ) }; diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 33c37e97ea..3edfc1d6f5 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -400,6 +400,21 @@ PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f); */ PARAM_DEFINE_FLOAT(FW_TKO_PITCH_MIN, 10.0f); +/** + * Takeoff Airspeed + * + * The calibrated airspeed setpoint TECS will stabilize to during the takeoff climbout. + * + * If set <= 0.0, FW_AIRSPD_MIN will be set by default. + * + * @unit m/s + * @min -1.0 + * @decimal 1 + * @increment 0.1 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_TKO_AIRSPD, -1.0f); + /** * Landing flare altitude (relative to landing altitude) * diff --git a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp index 1c23dcaaac..ce19edd425 100644 --- a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp +++ b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp @@ -63,8 +63,8 @@ void RunwayTakeoff::init(const hrt_abstime &time_now, const float initial_yaw, c takeoff_time_ = 0; } -void RunwayTakeoff::update(const hrt_abstime &time_now, const float calibrated_airspeed, const float vehicle_altitude, - const float clearance_altitude, orb_advert_t *mavlink_log_pub) +void RunwayTakeoff::update(const hrt_abstime &time_now, const float takeoff_airspeed, const float calibrated_airspeed, + const float vehicle_altitude, const float clearance_altitude, orb_advert_t *mavlink_log_pub) { switch (takeoff_state_) { case RunwayTakeoffState::THROTTLE_RAMP: @@ -74,16 +74,20 @@ void RunwayTakeoff::update(const hrt_abstime &time_now, const float calibrated_a break; - case RunwayTakeoffState::CLAMPED_TO_RUNWAY: - if (calibrated_airspeed > param_fw_airspd_min_.get() * param_rwto_airspd_scl_.get()) { - takeoff_time_ = time_now; - takeoff_state_ = RunwayTakeoffState::CLIMBOUT; - mavlink_log_info(mavlink_log_pub, "Takeoff airspeed reached, climbout\t"); - events::send(events::ID("runway_takeoff_reached_airspeed"), events::Log::Info, - "Takeoff airspeed reached, climbout"); - } + case RunwayTakeoffState::CLAMPED_TO_RUNWAY: { + const float rotation_airspeed = (param_rwto_rot_airspd_.get() > FLT_EPSILON) ? math::min(param_rwto_rot_airspd_.get(), + takeoff_airspeed) : 0.9f * takeoff_airspeed; - break; + if (calibrated_airspeed > rotation_airspeed) { + takeoff_time_ = time_now; + takeoff_state_ = RunwayTakeoffState::CLIMBOUT; + mavlink_log_info(mavlink_log_pub, "Takeoff airspeed reached, climbout\t"); + events::send(events::ID("runway_takeoff_reached_airspeed"), events::Log::Info, + "Takeoff airspeed reached, climbout"); + } + + break; + } case RunwayTakeoffState::CLIMBOUT: if (vehicle_altitude > clearance_altitude) { diff --git a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h index fbcabf0092..ffa73b8ea3 100644 --- a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h +++ b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h @@ -80,13 +80,14 @@ public: * @brief Updates the state machine based on the current vehicle condition. * * @param time_now Absolute time since system boot [us] + * @param takeoff_airspeed Calibrated airspeed setpoint for the takeoff climbout [m/s] * @param calibrated_airspeed Vehicle calibrated airspeed [m/s] * @param vehicle_altitude Vehicle altitude (AGL) [m] * @param clearance_altitude Altitude (AGL) above which we have cleared all occlusions in the runway path [m] * @param mavlink_log_pub */ - void update(const hrt_abstime &time_now, const float calibrated_airspeed, const float vehicle_altitude, - const float clearance_altitude, orb_advert_t *mavlink_log_pub); + void update(const hrt_abstime &time_now, const float takeoff_airspeed, const float calibrated_airspeed, + const float vehicle_altitude, const float clearance_altitude, orb_advert_t *mavlink_log_pub); /** * @return Current takeoff state @@ -103,11 +104,6 @@ public: */ bool runwayTakeoffEnabled() { return param_rwto_tkoff_.get(); } - /** - * @return Scale factor for minimum indicated airspeed - */ - float getMinAirspeedScaling() { return param_rwto_airspd_scl_.get(); } - /** * @return Initial vehicle yaw angle [rad] */ @@ -247,9 +243,8 @@ private: (ParamFloat) param_rwto_max_thr_, (ParamFloat) param_rwto_psp_, (ParamFloat) param_rwto_max_pitch_, - (ParamFloat) param_rwto_airspd_scl_, (ParamFloat) param_rwto_ramp_time_, - (ParamFloat) param_fw_airspd_min_ + (ParamFloat) param_rwto_rot_airspd_ ) }; diff --git a/src/modules/fw_pos_control_l1/runway_takeoff/runway_takeoff_params.c b/src/modules/fw_pos_control_l1/runway_takeoff/runway_takeoff_params.c index cef0b0461c..7da1693e6e 100644 --- a/src/modules/fw_pos_control_l1/runway_takeoff/runway_takeoff_params.c +++ b/src/modules/fw_pos_control_l1/runway_takeoff/runway_takeoff_params.c @@ -107,21 +107,6 @@ PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0); */ PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0); -/** - * Min airspeed scaling factor for takeoff. - * - * Pitch up will be commanded when the following airspeed is reached: - * FW_AIRSPD_MIN * RWTO_AIRSPD_SCL - * - * @unit norm - * @min 0.0 - * @max 2.0 - * @decimal 2 - * @increment 0.01 - * @group Runway Takeoff - */ -PARAM_DEFINE_FLOAT(RWTO_AIRSPD_SCL, 1.3); - /** * Throttle ramp up time for runway takeoff * @@ -156,3 +141,19 @@ PARAM_DEFINE_FLOAT(RWTO_L1_PERIOD, 5.0f); * @group Runway Takeoff */ PARAM_DEFINE_INT32(RWTO_NUDGE, 1); + +/** + * Takeoff rotation airspeed + * + * The calibrated airspeed threshold during the takeoff ground roll when the plane should start rotating (pitching up). + * Must be less than the takeoff airspeed, will otherwise be capped at the takeoff airpeed (see FW_TKO_AIRSPD). + * + * If set <= 0.0, defaults to 0.9 * takeoff airspeed (see FW_TKO_AIRSPD) + * + * @unit m/s + * @min -1.0 + * @decimal 1 + * @increment 0.1 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_ROT_AIRSPD, -1.0f);