mc_pos_control: refactor smooth takeoff names

The comments and variable names were partly misleading.
I grouped all members, hopefully gave them more
understandable names and adjusted the comments.

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -102,8 +102,11 @@ public:
 	int print_status() override;
 
 private:
-
+	// smooth takeoff vertical velocity ramp state
-	bool 		_in_smooth_takeoff = false; 		/**< true if takeoff ramp is applied */
+	bool _in_takeoff_ramp = false; /**< true while takeoff ramp is applied */
+	bool _velocity_triggered_takeoff = false; /**< true if takeoff was triggered by a velocity setpoint */
+	float _takeoff_ramp_velocity = -1.f; /**< current value of the smooth takeoff ramp */
+	float _takeoff_reference_z; /**< z-position when takeoff ramp was initiated */
 
 	orb_advert_t	_att_sp_pub{nullptr};			/**< attitude setpoint publication */
 	orb_advert_t	_traj_sp_pub{nullptr};		/**< trajectory setpoints publication */
@@ -122,11 +125,6 @@ private:
 
 	int _task_failure_count{0};         /**< counter for task failures */
 
-	float _takeoff_speed = -1.f; /**< For flighttask interface used only. It can be thrust or velocity setpoints */
-	float _takeoff_reference_z; /**< Z-position when takeoff was initiated */
-	bool _smooth_velocity_takeoff =
-		false; /**< Smooth velocity takeoff can be initiated either through position or velocity setpoint */
-
 	vehicle_status_s 			_vehicle_status{};		/**< vehicle status */
 	/**< vehicle-land-detection: initialze to landed */
 	vehicle_land_detected_s _vehicle_land_detected = {
@@ -233,13 +231,13 @@ private:
 	void publish_trajectory_sp(const vehicle_local_position_setpoint_s &traj);
 
 	/**
-	 * Update smooth takeoff setpoint ramp to bring the vehicle off the ground without step.
+	 * Update smooth takeoff vertical velocity ramp to bring the vehicle off the ground without step
 	 * @param z_sp position setpoint in the z-Direction
 	 * @param vz_sp velocity setpoint in the z-Direction
 	 * @param jz_sp jerk setpoint in the z-Direction
 	 * @param min_ground_clearance minimal distance to the ground in which e.g. optical flow works correctly
 	 */
-	void update_smooth_takeoff(const float z_sp, const float vz_sp, const float jz_sp, const float min_ground_clearance);
+	void update_takeoff_ramp(const float z_sp, const float vz_sp, const float jz_sp, const float min_distance_to_ground);
 
 	/**
 	 * Adjust the setpoint during landing.
@@ -649,14 +647,14 @@ MulticopterPositionControl::run()
 
 			// 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))) {
-				update_smooth_takeoff(setpoint.z, setpoint.vz, setpoint.jerk_z, constraints.min_distance_to_ground);
+				update_takeoff_ramp(setpoint.z, setpoint.vz, setpoint.jerk_z, constraints.min_distance_to_ground);
 			}
 
 			// disable horizontal / yaw control during smooth takeoff and limit maximum speed upwards
-			if (_in_smooth_takeoff) {
+			if (_in_takeoff_ramp) {
 
-				// during smooth takeoff, constrain speed to takeoff speed
+				// during smooth takeoff, constrain speed to ramp state
-				constraints.speed_up = _takeoff_speed;
+				constraints.speed_up = _takeoff_ramp_velocity;
 				// altitude above reference takeoff
 				const float alt_above_tko = -(_states.position(2) - _takeoff_reference_z);
 
@@ -675,7 +673,7 @@ MulticopterPositionControl::run()
 				}
 			}
 
-			if (_vehicle_land_detected.landed && !_in_smooth_takeoff && !PX4_ISFINITE(setpoint.thrust[2])) {
+			if (_vehicle_land_detected.landed && !_in_takeoff_ramp && !PX4_ISFINITE(setpoint.thrust[2])) {
 				// Keep throttle low when landed and NOT in smooth takeoff
 				reset_setpoint_to_nan(setpoint);
 				setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = 0.0f;
@@ -734,7 +732,7 @@ MulticopterPositionControl::run()
 			// Part of landing logic: if ground-contact/maybe landed was detected, turn off
 			// controller. This message does not have to be logged as part of the vehicle_local_position_setpoint topic.
 			// Note: only adust thrust output if there was not thrust-setpoint demand in D-direction.
-			if (!_in_smooth_takeoff && !PX4_ISFINITE(setpoint.thrust[2])) {
+			if (!_in_takeoff_ramp && !PX4_ISFINITE(setpoint.thrust[2])) {
 				limit_thrust_during_landing(local_pos_sp);
 			}
 
@@ -980,61 +978,60 @@ MulticopterPositionControl::start_flight_task()
 }
 
 void
-MulticopterPositionControl::update_smooth_takeoff(const float z_sp, const float vz_sp, const float jz_sp, const float min_ground_clearance)
+MulticopterPositionControl::update_takeoff_ramp(const float z_sp, const float vz_sp, const float jz_sp, const float min_distance_to_ground)
 {
-	// Check for smooth takeoff
+	// check if takeoff is triggered
-	if (_vehicle_land_detected.landed && !_in_smooth_takeoff) {
+	if (_vehicle_land_detected.landed && !_in_takeoff_ramp) {
-		// Vehicle is still landed and no takeoff was initiated yet.
+		// vehicle is still landed and no takeoff was initiated yet, check if takeoff is triggered
-		// Adjust for different takeoff cases.
+		// minimum takeoff altitude either 20cm or if valid above minimum altitude specified by flight task/estimator
-		// The minimum takeoff altitude needs to be at least 20cm above minimum distance or, if valid, above minimum distance
+		float min_altitude = 0.2f;
-		// above ground.
+		if (PX4_ISFINITE(min_distance_to_ground)) {
-		float min_altitude = PX4_ISFINITE(min_ground_clearance) ? (min_ground_clearance + 0.05f) :
+			min_altitude = min_distance_to_ground + 0.05f;
-				     0.2f;
+		}
-
-		// takeoff was initiated through velocity setpoint
-		_smooth_velocity_takeoff = PX4_ISFINITE(vz_sp) && vz_sp < -0.1f;
-		bool jerk_triggered_takeoff = PX4_ISFINITE(jz_sp) && jz_sp < -FLT_EPSILON;
-		_smooth_velocity_takeoff |= jerk_triggered_takeoff;
-
-		if ((PX4_ISFINITE(z_sp) && z_sp < _states.position(2) - min_altitude) ||  _smooth_velocity_takeoff) {
-			// There is a position setpoint above current position or velocity setpoint larger than
-			// takeoff speed. Enable smooth takeoff.
-			_in_smooth_takeoff = true;
-			_takeoff_speed = 0.f;
-			_takeoff_reference_z = _states.position(2);
 
+		// upwards velocity setpoint larger than a minimal speed
+		_velocity_triggered_takeoff = PX4_ISFINITE(vz_sp) && vz_sp < -0.1f;
+		// upwards jerk setpoint
+		const bool jerk_triggered_takeoff = PX4_ISFINITE(jz_sp) && jz_sp < -FLT_EPSILON;
+		// position setpoint above the minimum altitude
+		const bool position_triggered_takeoff = PX4_ISFINITE(z_sp) && (z_sp < (_states.position(2) - min_altitude));
+
+		_velocity_triggered_takeoff |= jerk_triggered_takeoff;
+		if (_velocity_triggered_takeoff || position_triggered_takeoff) {
+			// takeoff was triggered, start velocity ramp
+			_takeoff_ramp_velocity = 0.f;
+			_in_takeoff_ramp = true;
+			_takeoff_reference_z = _states.position(2);
 		}
 	}
 
-	// If in smooth takeoff, adjust setpoints based on what is valid:
+	// if in smooth takeoff limit upwards velocity setpoint to a smooth ramp
-	// 1. position setpoint is valid -> go with takeoffspeed to specific altitude
+	if (_in_takeoff_ramp) {
-	// 2. position setpoint not valid but velocity setpoint valid: ramp up velocity
+		float takeoff_desired_velocity = -vz_sp;
-	if (_in_smooth_takeoff) {
-		float desired_tko_speed = -vz_sp;
 
-		// If there is a valid position setpoint, then set the desired speed to the takeoff speed.
+		// if takeoff was triggered by a position setpoint, ramp to the takeoff speed parameter
-		if (!_smooth_velocity_takeoff) {
+		if (!_velocity_triggered_takeoff) {
-			desired_tko_speed = _param_mpc_tko_speed.get();
+			takeoff_desired_velocity = _param_mpc_tko_speed.get();
 		}
 
-		// Ramp up takeoff speed.
+		// ramp up vrtical velocity in TKO_RAMP_T seconds
 		if (_param_mpc_tko_ramp_t.get() > _dt) {
-			_takeoff_speed += desired_tko_speed * _dt / _param_mpc_tko_ramp_t.get();
+			_takeoff_ramp_velocity += takeoff_desired_velocity * _dt / _param_mpc_tko_ramp_t.get();
 
 		} else {
-			// No ramp, directly go to desired takeoff speed
+			// no ramp, directly go to desired takeoff speed
-			_takeoff_speed = desired_tko_speed;
+			_takeoff_ramp_velocity = takeoff_desired_velocity;
 		}
 
-		_takeoff_speed = math::min(_takeoff_speed, _param_mpc_tko_speed.get());
+		_takeoff_ramp_velocity = math::min(_takeoff_ramp_velocity, _param_mpc_tko_speed.get());
 
-		// Smooth takeoff is achieved once desired altitude/velocity setpoint is reached.
+		// smooth takeoff is achieved once desired altitude/velocity setpoint is reached
-		if (!_smooth_velocity_takeoff) {
+		if (!_velocity_triggered_takeoff) {
-			_in_smooth_takeoff = _states.position(2) - 0.2f > math::max(z_sp, _takeoff_reference_z - _param_mpc_land_alt2.get());
+			_in_takeoff_ramp = _states.position(2) - 0.2f > math::max(z_sp, _takeoff_reference_z - _param_mpc_land_alt2.get());
 
 		} else {
-			// Make sure to stay in smooth takeoff if takeoff has not been detected yet by the land detector
+			// make sure to stay in smooth takeoff if takeoff has not been detected yet by the land detector
-			_in_smooth_takeoff = (_takeoff_speed < -vz_sp) || _vehicle_land_detected.landed;
+			_in_takeoff_ramp = (_takeoff_ramp_velocity < -vz_sp) || _vehicle_land_detected.landed;
 		}
 	}
 }