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);