mc_pos_control: refactor spoolup time naming

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -159,7 +159,7 @@ private:
 		(ParamInt<px4::params::MPC_POS_MODE>) MPC_POS_MODE,
 		(ParamInt<px4::params::MPC_AUTO_MODE>) MPC_AUTO_MODE,
 		(ParamInt<px4::params::MPC_ALT_MODE>) MPC_ALT_MODE,
-		(ParamFloat<px4::params::MPC_IDLE_TKO>) MPC_IDLE_TKO, /**< time constant for smooth takeoff ramp */
+		(ParamFloat<px4::params::MPC_SPOOLUP_TIME>) MPC_SPOOLUP_TIME, /**< time to let motors spool up after arming */
 		(ParamInt<px4::params::MPC_OBS_AVOID>) MPC_OBS_AVOID, /**< enable obstacle avoidance */
 		(ParamFloat<px4::params::MPC_TILTMAX_LND>) 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;
 
-	/**
+	systemlib::Hysteresis _spoolup_time_hysteresis{false}; /**< becomes true MPC_SPOOLUP_TIME seconds after the vehicle was armed */
-	 * 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 _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
+		// set trigger time for takeoff delay
-		_arm_hysteresis.set_hysteresis_time_from(false, (int)(MPC_IDLE_TKO.get() * (float)1_s));
+		_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
+		// start takeoff after delay to allow motors to reach idle speed
-		// before propeller reached idle speed.
+		_spoolup_time_hysteresis.set_state_and_update(_control_mode.flag_armed);
-		_arm_hysteresis.set_state_and_update(_control_mode.flag_armed);
 
-		if (_arm_hysteresis.get_state()) {
+		if (_spoolup_time_hysteresis.get_state()) {
-			// as soon vehicle is armed check for flighttask
+			// 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
+			// do smooth takeoff after delay if there's a valid vertical velocity or position
-			// armed long enough
+			if (_spoolup_time_hysteresis.get_state() && PX4_ISFINITE(_states.position(2)) && PX4_ISFINITE(_states.velocity(2))) {
-			if (_arm_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);
 			}

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
+ * For altitude controlled modes the time from arming the motors until
- * idle to armed state is delayed by MPC_IDLE_TKO time to ensure
+ * a takeoff is possible gets forced to be at least MPC_SPOOLUP_TIME seconds
- * that the propellers have reached idle speed before attempting a
+ * to ensure the motors and propellers can sppol up and reach idle speed before
- * takeoff. This delay is particularly useful for vehicles with large
+ * getting commanded to spin faster. This delay is particularly useful for vehicles
- * propellers.
+ * 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.
  *