From 57f3be3a6959bbf98e7184ea676f09312956d9e8 Mon Sep 17 00:00:00 2001 From: Jin Chengde Date: Fri, 3 Jan 2020 06:05:35 +0000 Subject: [PATCH] fix bug if speed reach minium during ramp --- src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 9c15497a25..5cd36bf6f3 100644 --- a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp +++ b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp @@ -78,7 +78,8 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, switch (_state) { case RunwayTakeoffState::THROTTLE_RAMP: - if (hrt_elapsed_time(&_initialized_time) > _param_rwto_ramp_time.get() * 1e6f) { + if (hrt_elapsed_time(&_initialized_time) > _param_rwto_ramp_time.get() * 1e6f + || airspeed > _param_fw_airspd_min.get() * _param_rwto_airspd_scl.get() * 0.9) { _state = RunwayTakeoffState::CLAMPED_TO_RUNWAY; }