From 856d129bf89d2ad4dfb80ba11cc13b1975f3d974 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 16 May 2019 12:49:25 +0100 Subject: [PATCH] mc_pos_control: fix updating takeoff state when no flight task is running Without always updating the takeoff state it will not get skipped when the takeoff happened manually and when you switch from manual to position mode the drone goes to idle and falls. --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 4918238446ef..5d7316c8c800 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -581,10 +581,11 @@ MulticopterPositionControl::run() _wv_controller->update(matrix::Quatf(_att_sp.q_d), _states.yaw); } - // takeoff delay for motors to reach idle speed - _takeoff._spoolup_time_hysteresis.set_state_and_update(_control_mode.flag_armed); + // an update is necessary here because otherwise the takeoff state doesn't get skiped with non-altitude-controlled modes + _takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, false, -1.f, !_control_mode.flag_control_climb_rate_enabled); - if (_takeoff._spoolup_time_hysteresis.get_state()) { + // takeoff delay for motors to reach idle speed + if (_takeoff.getTakeoffState() >= TakeoffState::ready_for_takeoff) { // when vehicle is ready switch to the required flighttask start_flight_task();