diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index 7b2ab97e8e..c04ad780f2 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -58,14 +58,14 @@ RunwayTakeoff::RunwayTakeoff() : _initialized_time(0), _init_yaw(0), _climbout(false), - _min_airspeed_scaling(1.3f), - _max_takeoff_roll(15.0f), _runway_takeoff_enabled(this, "TKOFF"), - _runway_takeoff_heading(this, "HDG"), - _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"), + _heading_mode(this, "HDG"), + _nav_alt(this, "NAV_ALT"), + _takeoff_throttle(this, "MAX_THR"), + _runway_pitch_sp(this, "PSP"), + _max_takeoff_pitch(this, "MAX_PITCH"), + _max_takeoff_roll(this, "MAX_ROLL"), + _min_airspeed_scaling(this, "AIRSPD_SCL"), _airspeed_min(this, "FW_AIRSPD_MIN", false), _climbout_diff(this, "FW_CLMBOUT_DIFF", false) { @@ -99,7 +99,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) break; case RunwayTakeoffState::CLAMPED_TO_RUNWAY: - if (airspeed > _airspeed_min.get() * _min_airspeed_scaling) { + if (airspeed > _airspeed_min.get() * _min_airspeed_scaling.get()) { _state = RunwayTakeoffState::TAKEOFF; mavlink_log_info(mavlink_fd, "#Takeoff airspeed reached"); } @@ -107,7 +107,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) break; case RunwayTakeoffState::TAKEOFF: - if (alt_agl > _runway_takeoff_nav_alt.get()) { + if (alt_agl > _nav_alt.get()) { _state = RunwayTakeoffState::CLIMBOUT; mavlink_log_info(mavlink_fd, "#Climbout"); } @@ -137,7 +137,7 @@ bool RunwayTakeoff::controlYaw() float RunwayTakeoff::getPitch(float tecsPitch) { if (_state <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) { - return math::radians(_runway_takeoff_pitch_sp.get()); + return math::radians(_runway_pitch_sp.get()); } return tecsPitch; @@ -152,14 +152,14 @@ float RunwayTakeoff::getRoll(float navigatorRoll) // allow some roll during climbout if waypoint heading is targeted else if (_state < RunwayTakeoffState::FLY) { - if (_runway_takeoff_heading.get() == 0) { + if (_heading_mode.get() == 0) { // otherwise stay at 0 roll return 0.0f; - } else if (_runway_takeoff_heading.get() == 1) { + } else if (_heading_mode.get() == 1) { return math::constrain(navigatorRoll, - math::radians(-_max_takeoff_roll), - math::radians(_max_takeoff_roll)); + math::radians(-_max_takeoff_roll.get()), + math::radians(_max_takeoff_roll).get()); } } @@ -169,11 +169,11 @@ float RunwayTakeoff::getRoll(float navigatorRoll) float RunwayTakeoff::getYaw(float navigatorYaw) { if (_state < RunwayTakeoffState::FLY) { - if (_runway_takeoff_heading.get() == 0) { + if (_heading_mode.get() == 0) { // fix heading in the direction the airframe points return _init_yaw; - } else if (_runway_takeoff_heading.get() == 1) { + } else if (_heading_mode.get() == 1) { // or head into the direction of the takeoff waypoint // XXX this needs a check if the deviation from actual heading is too big (else we do a full throttle wheel turn on the ground) return navigatorYaw; @@ -188,14 +188,14 @@ float RunwayTakeoff::getThrottle(float tecsThrottle) switch (_state) { case RunwayTakeoffState::THROTTLE_RAMP: { float throttle = hrt_elapsed_time(&_initialized_time) / (float)2000000 * - _runway_takeoff_throttle.get(); - return throttle < _runway_takeoff_throttle.get() ? + _takeoff_throttle.get(); + return throttle < _takeoff_throttle.get() ? throttle : - _runway_takeoff_throttle.get(); + _takeoff_throttle.get(); } case RunwayTakeoffState::CLAMPED_TO_RUNWAY: - return _runway_takeoff_throttle.get(); + return _takeoff_throttle.get(); default: return tecsThrottle; @@ -221,8 +221,8 @@ 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(); + if (_climbout && _max_takeoff_pitch.get() > 0.1f) { + return _max_takeoff_pitch.get(); } else { diff --git a/src/lib/runway_takeoff/RunwayTakeoff.h b/src/lib/runway_takeoff/RunwayTakeoff.h index 86b8c01b6e..8e3cd35528 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.h +++ b/src/lib/runway_takeoff/RunwayTakeoff.h @@ -83,7 +83,7 @@ public: float getYaw(float navigatorYaw); float getThrottle(float tecsThrottle); bool resetIntegrators(); - float getMinAirspeedScaling() { return _min_airspeed_scaling; }; + float getMinAirspeedScaling() { return _min_airspeed_scaling.get(); }; float getMinPitch(float sp_min, float climbout_min, float min); float getMaxPitch(float max); @@ -98,17 +98,15 @@ private: float _init_yaw; bool _climbout; - /** magic values **/ - float _min_airspeed_scaling; - float _max_takeoff_roll; - /** parameters **/ control::BlockParamInt _runway_takeoff_enabled; - control::BlockParamInt _runway_takeoff_heading; - control::BlockParamFloat _runway_takeoff_nav_alt; - control::BlockParamFloat _runway_takeoff_throttle; - control::BlockParamFloat _runway_takeoff_pitch_sp; - control::BlockParamFloat _runway_takeoff_max_pitch; + control::BlockParamInt _heading_mode; + control::BlockParamFloat _nav_alt; + control::BlockParamFloat _takeoff_throttle; + control::BlockParamFloat _runway_pitch_sp; + control::BlockParamFloat _max_takeoff_pitch; + control::BlockParamFloat _max_takeoff_roll; + control::BlockParamFloat _min_airspeed_scaling; 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 137732c074..05110dddc2 100644 --- a/src/lib/runway_takeoff/runway_takeoff_params.c +++ b/src/lib/runway_takeoff/runway_takeoff_params.c @@ -109,3 +109,36 @@ PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0); * @group Runway Takeoff */ PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.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); + +/** + * Max roll during climbout. + * Roll is limited during climbout to ensure enough lift and prevents aggressive + * navigation before we're on a safe height. + * + * @min 0.0 + * @max 60.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_MAX_ROLL, 25.0); + +/** + * Min. airspeed scaling factor for takeoff. + * Pitch up will be commanded when the following airspeed is reached: + * FW_AIRSPD_MIN * RWTO_AIRSPD_SCL + * + * @min 0.0 + * @max 2.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_AIRSPD_SCL, 1.3);