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 f92e585386..bfd9ae3fa8 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -159,7 +159,7 @@ private: (ParamInt) MPC_POS_MODE, (ParamInt) MPC_AUTO_MODE, (ParamInt) MPC_ALT_MODE, - (ParamFloat) MPC_IDLE_TKO, /**< time constant for smooth takeoff ramp */ + (ParamFloat) MPC_SPOOLUP_TIME, /**< time to let motors spool up after arming */ (ParamInt) MPC_OBS_AVOID, /**< enable obstacle avoidance */ (ParamFloat) MPC_TILTMAX_LND /**< maximum tilt for landing and smooth takeoff */ ); @@ -185,14 +185,7 @@ private: /** During smooth-takeoff, below ALTITUDE_THRESHOLD the yaw-control is turned off ant tilt is limited */ static constexpr float ALTITUDE_THRESHOLD = 0.3f; - /** - * Hysteresis that turns true once vehicle is armed for MPC_IDLE_TKO seconds. - * A real vehicle requires some time to accelerates the propellers to IDLE speed. To ensure - * that the propellers reach idle speed before initiating a takeoff, a delay of MPC_IDLE_TKO - * is added. - */ - systemlib::Hysteresis _arm_hysteresis{false}; /**< becomes true once vehicle is armed for MPC_IDLE_TKO seconds */ - + systemlib::Hysteresis _spoolup_time_hysteresis{false}; /**< becomes true MPC_SPOOLUP_TIME seconds after the vehicle was armed */ systemlib::Hysteresis _failsafe_land_hysteresis{false}; /**< becomes true if task did not update correctly for LOITER_TIME_BEFORE_DESCEND */ WeatherVane *_wv_controller{nullptr}; @@ -411,8 +404,8 @@ MulticopterPositionControl::parameters_update(bool force) _tko_speed.set(math::min(_tko_speed.get(), _vel_max_up.get())); _land_speed.set(math::min(_land_speed.get(), _vel_max_down.get())); - // set trigger time for arm hysteresis - _arm_hysteresis.set_hysteresis_time_from(false, (int)(MPC_IDLE_TKO.get() * (float)1_s)); + // set trigger time for takeoff delay + _spoolup_time_hysteresis.set_hysteresis_time_from(false, (int)(MPC_SPOOLUP_TIME.get() * (float)1_s)); if (_wv_controller != nullptr) { _wv_controller->update_parameters(); @@ -649,16 +642,15 @@ MulticopterPositionControl::run() _wv_controller->update(matrix::Quatf(_att_sp.q_d), _states.yaw); } - // arm hysteresis prevents vehicle to takeoff - // before propeller reached idle speed. - _arm_hysteresis.set_state_and_update(_control_mode.flag_armed); + // start takeoff after delay to allow motors to reach idle speed + _spoolup_time_hysteresis.set_state_and_update(_control_mode.flag_armed); - if (_arm_hysteresis.get_state()) { - // as soon vehicle is armed check for flighttask + if (_spoolup_time_hysteresis.get_state()) { + // when vehicle is ready switch to the required flighttask start_flight_task(); } else { - // disable flighttask + // stop flighttask while disarmed _flight_tasks.switchTask(FlightTaskIndex::None); } @@ -706,9 +698,8 @@ MulticopterPositionControl::run() // check if all local states are valid and map accordingly set_vehicle_states(setpoint.vz); - // we can only do a smooth takeoff if a valid velocity or position is available and are - // armed long enough - if (_arm_hysteresis.get_state() && PX4_ISFINITE(_states.position(2)) && PX4_ISFINITE(_states.velocity(2))) { + // do smooth takeoff after delay if there's a valid vertical velocity or position + if (_spoolup_time_hysteresis.get_state() && PX4_ISFINITE(_states.position(2)) && PX4_ISFINITE(_states.velocity(2))) { check_for_smooth_takeoff(setpoint.z, setpoint.vz, constraints); update_smooth_takeoff(setpoint.z, setpoint.vz); } diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 9b91cf426a..f87debef19 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -658,7 +658,7 @@ PARAM_DEFINE_FLOAT(MPC_LAND_ALT2, 5.0f); PARAM_DEFINE_FLOAT(MPC_TKO_RAMP_T, 0.4f); /** - * Manual-Position control sub-mode. + * Manual-Position control sub-mode * * The supported sub-modes are: * 0 Default position control where sticks map to position/velocity directly. Maximum speeds @@ -679,7 +679,7 @@ PARAM_DEFINE_FLOAT(MPC_TKO_RAMP_T, 0.4f); PARAM_DEFINE_INT32(MPC_POS_MODE, 1); /** - * Auto sub-mode. + * Auto sub-mode * * @value 0 Default line tracking * @value 1 Jerk-limited trajectory @@ -688,20 +688,20 @@ PARAM_DEFINE_INT32(MPC_POS_MODE, 1); PARAM_DEFINE_INT32(MPC_AUTO_MODE, 1); /** - * Delay from idle state to arming state. + * Enforced delay between arming and takeoff * - * For altitude controlled modes, the transition from - * idle to armed state is delayed by MPC_IDLE_TKO time to ensure - * that the propellers have reached idle speed before attempting a - * takeoff. This delay is particularly useful for vehicles with large - * propellers. + * For altitude controlled modes the time from arming the motors until + * a takeoff is possible gets forced to be at least MPC_SPOOLUP_TIME seconds + * to ensure the motors and propellers can sppol up and reach idle speed before + * getting commanded to spin faster. This delay is particularly useful for vehicles + * with slow motor spin-up e.g. because of large propellers. * * @min 0 * @max 10 - * @unit sec + * @unit s * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_IDLE_TKO, 0.0f); +PARAM_DEFINE_FLOAT(MPC_SPOOLUP_TIME, 0.0f); /** * Flag to enable obstacle avoidance @@ -713,7 +713,7 @@ PARAM_DEFINE_FLOAT(MPC_IDLE_TKO, 0.0f); PARAM_DEFINE_INT32(MPC_OBS_AVOID, 0); /** - * Yaw mode. + * Yaw mode * * Specifies the heading in Auto. *