diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index 599cb721d0..7b2ab97e8e 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -65,6 +65,7 @@ RunwayTakeoff::RunwayTakeoff() : _runway_takeoff_nav_alt(this, "NAV_ALT"), _runway_takeoff_throttle(this, "MAX_THR"), _runway_takeoff_pitch_sp(this, "PSP"), + _runway_takeoff_max_pitch(this, "MAX_PITCH"), _airspeed_min(this, "FW_AIRSPD_MIN", false), _climbout_diff(this, "FW_CLMBOUT_DIFF", false) { @@ -83,6 +84,7 @@ void RunwayTakeoff::init(float yaw) _initialized = true; _state = RunwayTakeoffState::THROTTLE_RAMP; _initialized_time = hrt_absolute_time(); + _climbout = true; } void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) @@ -98,7 +100,6 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) case RunwayTakeoffState::CLAMPED_TO_RUNWAY: if (airspeed > _airspeed_min.get() * _min_airspeed_scaling) { - _climbout = true; _state = RunwayTakeoffState::TAKEOFF; mavlink_log_info(mavlink_fd, "#Takeoff airspeed reached"); } @@ -218,6 +219,17 @@ float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min) } } +float RunwayTakeoff::getMaxPitch(float max) +{ + if (_climbout && _runway_takeoff_max_pitch.get() > 0.1f) { + return _runway_takeoff_max_pitch.get(); + } + + else { + return max; + } +} + void RunwayTakeoff::reset() { _initialized = false; diff --git a/src/lib/runway_takeoff/RunwayTakeoff.h b/src/lib/runway_takeoff/RunwayTakeoff.h index 265ff24cb3..86b8c01b6e 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.h +++ b/src/lib/runway_takeoff/RunwayTakeoff.h @@ -85,6 +85,7 @@ public: bool resetIntegrators(); float getMinAirspeedScaling() { return _min_airspeed_scaling; }; float getMinPitch(float sp_min, float climbout_min, float min); + float getMaxPitch(float max); void reset(); @@ -107,6 +108,7 @@ private: control::BlockParamFloat _runway_takeoff_nav_alt; control::BlockParamFloat _runway_takeoff_throttle; control::BlockParamFloat _runway_takeoff_pitch_sp; + control::BlockParamFloat _runway_takeoff_max_pitch; control::BlockParamFloat _airspeed_min; control::BlockParamFloat _climbout_diff; diff --git a/src/lib/runway_takeoff/runway_takeoff_params.c b/src/lib/runway_takeoff/runway_takeoff_params.c index d3ce5d238d..137732c074 100644 --- a/src/lib/runway_takeoff/runway_takeoff_params.c +++ b/src/lib/runway_takeoff/runway_takeoff_params.c @@ -88,7 +88,7 @@ PARAM_DEFINE_FLOAT(RWTO_NAV_ALT, 5.0); PARAM_DEFINE_FLOAT(RWTO_MAX_THR, 1.0); /** - * Pitch setpoint during runway takeoff. + * Pitch setpoint during taxi / before takeoff airspeed is reached. * A taildragger with stearable wheel might need to pitch up * a little to keep it's wheel on the ground before airspeed * to takeoff is reached. @@ -98,3 +98,14 @@ PARAM_DEFINE_FLOAT(RWTO_MAX_THR, 1.0); * @group Runway Takeoff */ PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0); + +/** + * Max pitch during takeoff. + * Fixed-wing settings are used if set to 0. Note that there is also a minimum + * pitch of 10 degrees during takeoff, so this must be larger if set. + * + * @min 0.0 + * @max 60.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index ed71cf2955..711e888416 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1382,7 +1382,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _mavlink_fd); // update tecs - float takeoff_pitch_max_deg = _parameters.pitch_limit_max; + float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_parameters.pitch_limit_max); float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,