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 322f1bff5c..9c15497a25 100644 --- a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp +++ b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp @@ -57,8 +57,7 @@ RunwayTakeoff::RunwayTakeoff(ModuleParams *parent) : _initialized(false), _initialized_time(0), _init_yaw(0), - _climbout(false), - _throttle_ramp_time(2 * 1e6) + _climbout(false) { } @@ -79,7 +78,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, switch (_state) { case RunwayTakeoffState::THROTTLE_RAMP: - if (hrt_elapsed_time(&_initialized_time) > _throttle_ramp_time) { + if (hrt_elapsed_time(&_initialized_time) > _param_rwto_ramp_time.get() * 1e6f) { _state = RunwayTakeoffState::CLAMPED_TO_RUNWAY; } @@ -195,7 +194,7 @@ float RunwayTakeoff::getThrottle(float tecsThrottle) { switch (_state) { case RunwayTakeoffState::THROTTLE_RAMP: { - float throttle = hrt_elapsed_time(&_initialized_time) / (float)_throttle_ramp_time * + float throttle = (hrt_elapsed_time(&_initialized_time) / (float)_param_rwto_ramp_time.get() * 1e6f) * _param_rwto_max_thr.get(); return throttle < _param_rwto_max_thr.get() ? throttle : 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 15c9e8b005..b4f9c981ac 100644 --- a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h +++ b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h @@ -98,7 +98,6 @@ private: hrt_abstime _initialized_time; float _init_yaw; bool _climbout; - unsigned _throttle_ramp_time; matrix::Vector2f _start_wp; DEFINE_PARAMETERS( @@ -110,6 +109,7 @@ private: (ParamFloat) _param_rwto_max_pitch, (ParamFloat) _param_rwto_max_roll, (ParamFloat) _param_rwto_airspd_scl, + (ParamFloat) _param_rwto_ramp_time, (ParamFloat) _param_fw_airspd_min, (ParamFloat) _param_fw_clmbout_diff ) 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 7b91c8865b..2ea76c35ed 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 @@ -144,3 +144,15 @@ PARAM_DEFINE_FLOAT(RWTO_MAX_ROLL, 25.0); * @group Runway Takeoff */ PARAM_DEFINE_FLOAT(RWTO_AIRSPD_SCL, 1.3); + +/** + * Throttle ramp up time for runway takeoff + * + * @unit s + * @min 1.0 + * @max 15.0 + * @decimal 2 + * @increment 0.1 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_RAMP_TIME, 2.0f);