move takeoff state machine flight_mode_manager -> mc_pos_control

msg/takeoff_status.msg

@@ -2,10 +2,11 @@
 
 uint64 timestamp # time since system start (microseconds)
 
-uint8 TAKEOFF_STATE_DISARMED = 0
-uint8 TAKEOFF_STATE_SPOOLUP = 1
-uint8 TAKEOFF_STATE_READY_FOR_TAKEOFF = 2
-uint8 TAKEOFF_STATE_RAMPUP = 3
-uint8 TAKEOFF_STATE_FLIGHT = 4
+uint8 TAKEOFF_STATE_UNINITIALIZED     = 0
+uint8 TAKEOFF_STATE_DISARMED          = 1
+uint8 TAKEOFF_STATE_SPOOLUP           = 2
+uint8 TAKEOFF_STATE_READY_FOR_TAKEOFF = 3
+uint8 TAKEOFF_STATE_RAMPUP            = 4
+uint8 TAKEOFF_STATE_FLIGHT            = 5
 
 uint8 takeoff_state

msg/vehicle_constraints.msg

@@ -3,15 +3,8 @@
 
 uint64 timestamp # time since system start (microseconds)
 
-float32 yawspeed # in radians/sec
 float32 speed_xy # in meters/sec
 float32 speed_up # in meters/sec
 float32 speed_down # in meters/sec
-float32 tilt # in radians [0, PI]
-float32 min_distance_to_ground # in meters
-float32 max_distance_to_ground # in meters
-bool want_takeoff # tell the controller to initiate takeoff when idling (ignored during flight)
-bool reset_integral # tells controller to reset integrators e.g. since we know the vehicle is not in air
-float32 minimum_thrust # tell controller what the minimum collective output thrust should be
 
-uint32 flight_task
+bool want_takeoff # tell the controller to initiate takeoff when idling (ignored during flight)

src/modules/flight_mode_manager/CMakeLists.txt

@@ -108,8 +108,6 @@ add_compile_options(
 # add subdirectory containing all tasks
 add_subdirectory(tasks)
 
-add_subdirectory(Takeoff)
-
 px4_add_module(
 	MODULE modules__flight_mode_manager
 	MAIN flight_mode_manager
@@ -124,7 +122,6 @@ px4_add_module(
 		${CMAKE_CURRENT_BINARY_DIR}/FlightTasks_generated.hpp
 		${CMAKE_CURRENT_BINARY_DIR}/FlightTasks_generated.cpp
 	DEPENDS
-		Takeoff
 		px4_work_queue
 		WeatherVane
 )

src/modules/flight_mode_manager/FlightModeManager.cpp

@@ -115,10 +115,6 @@ void FlightModeManager::Run()
 			}
 		}
 
-		// an update is necessary here because otherwise the takeoff state doesn't get skiped with non-altitude-controlled modes
-		_takeoff.updateTakeoffState(_vehicle_control_mode_sub.get().flag_armed, _vehicle_land_detected_sub.get().landed, false,
-					    10.f, !_vehicle_control_mode_sub.get().flag_control_climb_rate_enabled, time_stamp_now);
-
 		// activate the weathervane controller if required. If activated a flighttask can use it to implement a yaw-rate control strategy
 		// that turns the nose of the vehicle into the wind
 		if (_wv_controller != nullptr) {
@@ -162,10 +158,6 @@ void FlightModeManager::updateParams()
 		_current_task.task->handleParameterUpdate();
 	}
 
-	_takeoff.setSpoolupTime(_param_mpc_spoolup_time.get());
-	_takeoff.setTakeoffRampTime(_param_mpc_tko_ramp_t.get());
-	_takeoff.generateInitialRampValue(_param_mpc_z_vel_p_acc.get());
-
 	if (_wv_controller != nullptr) {
 		_wv_controller->update_parameters();
 	}
@@ -517,21 +509,15 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt,
 	// limit altitude according to land detector
 	limitAltitude(setpoint, vehicle_local_position);
 
-	const bool not_taken_off = _takeoff.getTakeoffState() < TakeoffState::rampup;
-	const bool flying = _takeoff.getTakeoffState() >= TakeoffState::flight;
-	const bool flying_but_ground_contact = flying && _vehicle_land_detected_sub.get().ground_contact;
+	if (_takeoff_status_sub.updated()) {
+		takeoff_status_s takeoff_status;
 
-	if (not_taken_off || flying_but_ground_contact) {
-		// we are not flying yet and need to avoid any corrections
-		reset_setpoint_to_nan(setpoint);
-		Vector3f(0.f, 0.f, 100.f).copyTo(setpoint.acceleration); // High downwards acceleration to make sure there's no thrust
-		// set yaw-sp to current yaw
-		setpoint.yawspeed = 0.f;
-		// prevent any integrator windup
-		constraints.reset_integral = true;
+		if (_takeoff_status_sub.copy(&takeoff_status)) {
+			_takeoff_state = takeoff_status.takeoff_state;
+		}
 	}
 
-	if (not_taken_off) {
+	if (_takeoff_state < takeoff_status_s::TAKEOFF_STATE_RAMPUP) {
 		// reactivate the task which will reset the setpoint to current state
 		_current_task.task->reActivate();
 	}
@@ -540,38 +526,9 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt,
 	setpoint.timestamp = hrt_absolute_time();
 	_trajectory_setpoint_pub.publish(setpoint);
 
-
-	// Allow ramping from zero thrust on takeoff
-	if (flying) {
-		constraints.minimum_thrust = _param_mpc_thr_min.get();
-
-	} else {
-		// allow zero thrust when taking off and landing
-		constraints.minimum_thrust = 0.f;
-	}
-
-	// fix to prevent the takeoff ramp to ramp to a too high value or get stuck because of NAN
-	// TODO: this should get obsolete once the takeoff limiting moves into the flight tasks
-	if (!PX4_ISFINITE(constraints.speed_up) || (constraints.speed_up > _param_mpc_z_vel_max_up.get())) {
-		constraints.speed_up = _param_mpc_z_vel_max_up.get();
-	}
-
-	// limit tilt during takeoff ramupup
-	if (_takeoff.getTakeoffState() < TakeoffState::flight) {
-		constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get());
-	}
-
-	// handle smooth takeoff
-	_takeoff.updateTakeoffState(_vehicle_control_mode_sub.get().flag_armed, _vehicle_land_detected_sub.get().landed,
-				    constraints.want_takeoff, constraints.speed_up, !_vehicle_control_mode_sub.get().flag_control_climb_rate_enabled,
-				    _time_stamp_last_loop);
-	constraints.speed_up = _takeoff.updateRamp(dt, constraints.speed_up);
-
-	constraints.flight_task = static_cast<uint32_t>(_current_task.index);
 	constraints.timestamp = hrt_absolute_time();
 	_vehicle_constraints_pub.publish(constraints);
 
-
 	// if there's any change in landing gear setpoint publish it
 	landing_gear_s landing_gear = _current_task.task->getGear();
 
@@ -583,16 +540,6 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt,
 	}
 
 	_old_landing_gear_position = landing_gear.landing_gear;
-
-	// Publish takeoff status
-	takeoff_status_s takeoff_status;
-	takeoff_status.takeoff_state = static_cast<uint8_t>(_takeoff.getTakeoffState());
-
-	if (takeoff_status.takeoff_state != _old_takeoff_state) {
-		takeoff_status.timestamp = hrt_absolute_time();
-		_takeoff_status_pub.publish(takeoff_status);
-		_old_takeoff_state = takeoff_status.takeoff_state;
-	}
 }
 
 void FlightModeManager::limitAltitude(vehicle_local_position_setpoint_s &setpoint,
@@ -614,15 +561,6 @@ void FlightModeManager::limitAltitude(vehicle_local_position_setpoint_s &setpoin
 	}
 }
 
-void FlightModeManager::reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint)
-{
-	setpoint.x = setpoint.y = setpoint.z = NAN;
-	setpoint.vx = setpoint.vy = setpoint.vz = NAN;
-	setpoint.yaw = setpoint.yawspeed = NAN;
-	setpoint.acceleration[0] = setpoint.acceleration[1] = setpoint.acceleration[2] = NAN;
-	setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = NAN;
-}
-
 FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index)
 {
 	// switch to the running task, nothing to do

src/modules/flight_mode_manager/FlightModeManager.hpp

@@ -56,8 +56,6 @@
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
 
-#include "Takeoff/Takeoff.hpp"
-
 #include <new>
 
 enum class FlightTaskError : int {
@@ -95,7 +93,6 @@ private:
 	void handleCommand();
 	void generateTrajectorySetpoint(const float dt, const vehicle_local_position_s &vehicle_local_position);
 	void limitAltitude(vehicle_local_position_setpoint_s &setpoint, const vehicle_local_position_s &vehicle_local_position);
-	void reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint);
 
 	/**
 	 * Switch to a specific task (for normal usage)
@@ -133,10 +130,9 @@ private:
 		FlightTaskIndex index{FlightTaskIndex::None};
 	} _current_task{};
 
-	Takeoff _takeoff; ///< state machine and ramp to bring the vehicle off the ground without a jump
 	WeatherVane *_wv_controller{nullptr};
 	int8_t _old_landing_gear_position{landing_gear_s::GEAR_KEEP};
-	uint8_t _old_takeoff_state{0};
+	uint8_t _takeoff_state{takeoff_status_s::TAKEOFF_STATE_DISARMED};
 	int _task_failure_count{0};
 	uint8_t _last_vehicle_nav_state{0};
 
@@ -145,6 +141,7 @@ private:
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
+	uORB::Subscription _takeoff_status_sub{ORB_ID(takeoff_status)};
 	uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)};
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 	uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)};
@@ -156,19 +153,12 @@ private:
 	uORB::SubscriptionData<vehicle_status_s> _vehicle_status_sub{ORB_ID(vehicle_status)};
 
 	uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)};
-	uORB::Publication<takeoff_status_s> _takeoff_status_pub{ORB_ID(takeoff_status)};
 	uORB::Publication<vehicle_local_position_setpoint_s> _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};
 	uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
 	uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
 	uORB::Publication<vehicle_constraints_s> _vehicle_constraints_pub{ORB_ID(vehicle_constraints)};
 
 	DEFINE_PARAMETERS(
-		(ParamInt<px4::params::MPC_POS_MODE>) _param_mpc_pos_mode,
-		(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd,
-		(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up,
-		(ParamFloat<px4::params::MPC_SPOOLUP_TIME>) _param_mpc_spoolup_time,
-		(ParamFloat<px4::params::MPC_TKO_RAMP_T>) _param_mpc_tko_ramp_t,
-		(ParamFloat<px4::params::MPC_Z_VEL_P_ACC>) _param_mpc_z_vel_p_acc,
-		(ParamFloat<px4::params::MPC_THR_MIN>) _param_mpc_thr_min
+		(ParamInt<px4::params::MPC_POS_MODE>) _param_mpc_pos_mode
 	);
 };

src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.hpp

@@ -65,7 +65,6 @@ protected:
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto,
 					(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
-					(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd,
 					(ParamInt<px4::params::MPC_LAND_RC_HELP>) _param_mpc_land_rc_help,
 					(ParamFloat<px4::params::MPC_LAND_ALT1>)
 					_param_mpc_land_alt1, // altitude at which speed limit downwards reaches maximum speed

src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp

@@ -6,7 +6,7 @@ constexpr uint64_t FlightTask::_timeout;
 // First index of empty_setpoint corresponds to time-stamp and requires a finite number.
 const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {}};
 
-const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, false, {}};
+const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, {}};
 const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}};
 
 bool FlightTask::activate(const vehicle_local_position_setpoint_s &last_setpoint)
@@ -172,9 +172,6 @@ void FlightTask::_setDefaultConstraints()
 	_constraints.speed_xy = _param_mpc_xy_vel_max.get();
 	_constraints.speed_up = _param_mpc_z_vel_max_up.get();
 	_constraints.speed_down = _param_mpc_z_vel_max_dn.get();
-	_constraints.tilt = NAN;
-	_constraints.min_distance_to_ground = NAN;
-	_constraints.max_distance_to_ground = NAN;
 	_constraints.want_takeoff = false;
 }
 

src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp

@@ -227,6 +227,9 @@ protected:
 	float _dist_to_bottom{}; /**< current height above ground level */
 	float _dist_to_ground{}; /**< equals _dist_to_bottom if valid, height above home otherwise */
 
+	float _min_distance_to_ground{-INFINITY}; /**< min distance to ground constraint */
+	float _max_distance_to_ground{INFINITY};  /**< max distance to ground constraint */
+
 	/**
 	 * Setpoints which the position controller has to execute.
 	 * Setpoints that are set to NAN are not controlled. Not all setpoints can be set at the same time.

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

@@ -81,17 +81,17 @@ bool FlightTaskManualAltitude::activate(const vehicle_local_position_setpoint_s
 void FlightTaskManualAltitude::_updateConstraintsFromEstimator()
 {
 	if (PX4_ISFINITE(_sub_vehicle_local_position.get().hagl_min)) {
-		_constraints.min_distance_to_ground = _sub_vehicle_local_position.get().hagl_min;
+		_min_distance_to_ground = _sub_vehicle_local_position.get().hagl_min;
 
 	} else {
-		_constraints.min_distance_to_ground = -INFINITY;
+		_min_distance_to_ground = -INFINITY;
 	}
 
 	if (PX4_ISFINITE(_sub_vehicle_local_position.get().hagl_max)) {
-		_constraints.max_distance_to_ground = _sub_vehicle_local_position.get().hagl_max;
+		_max_distance_to_ground = _sub_vehicle_local_position.get().hagl_max;
 
 	} else {
-		_constraints.max_distance_to_ground = INFINITY;
+		_max_distance_to_ground = INFINITY;
 	}
 }
 
@@ -184,7 +184,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
 
 			// Ensure that minimum altitude is respected if
 			// there is a distance sensor and distance to bottom is below minimum.
-			if (PX4_ISFINITE(_dist_to_bottom) && _dist_to_bottom < _constraints.min_distance_to_ground) {
+			if (PX4_ISFINITE(_dist_to_bottom) && _dist_to_bottom < _min_distance_to_ground) {
 				_terrainFollowing(apply_brake, stopped);
 
 			} else {
@@ -210,14 +210,10 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
 
 void FlightTaskManualAltitude::_respectMinAltitude()
 {
-	const bool respectAlt = PX4_ISFINITE(_dist_to_bottom)
-				&& _dist_to_bottom < _constraints.min_distance_to_ground;
-
 	// Height above ground needs to be limited (flow / range-finder)
-	if (respectAlt) {
+	if (PX4_ISFINITE(_dist_to_bottom) && (_dist_to_bottom < _min_distance_to_ground)) {
 		// increase altitude to minimum flow distance
-		_position_setpoint(2) = _position(2)
-					- (_constraints.min_distance_to_ground - _dist_to_bottom);
+		_position_setpoint(2) = _position(2) - (_min_distance_to_ground - _dist_to_bottom);
 	}
 }
 
@@ -253,8 +249,8 @@ void FlightTaskManualAltitude::_respectMaxAltitude()
 
 		// if there is a valid maximum distance to ground, linearly increase speed limit with distance
 		// below the maximum, preserving control loop vertical position error gain.
-		if (PX4_ISFINITE(_constraints.max_distance_to_ground)) {
-			_constraints.speed_up = math::constrain(_param_mpc_z_p.get() * (_constraints.max_distance_to_ground - _dist_to_bottom),
+		if (PX4_ISFINITE(_max_distance_to_ground)) {
+			_constraints.speed_up = math::constrain(_param_mpc_z_p.get() * (_max_distance_to_ground - _dist_to_bottom),
 								-_max_speed_down, _max_speed_up);
 
 		} else {
@@ -262,17 +258,16 @@ void FlightTaskManualAltitude::_respectMaxAltitude()
 		}
 
 		// if distance to bottom exceeded maximum distance, slowly approach maximum distance
-		if (_dist_to_bottom >  _constraints.max_distance_to_ground) {
+		if (_dist_to_bottom > _max_distance_to_ground) {
 			// difference between current distance to ground and maximum distance to ground
-			const float delta_distance_to_max = _dist_to_bottom - _constraints.max_distance_to_ground;
+			const float delta_distance_to_max = _dist_to_bottom - _max_distance_to_ground;
 			// set position setpoint to maximum distance to ground
-			_position_setpoint(2) = _position(2) +  delta_distance_to_max;
+			_position_setpoint(2) = _position(2) + delta_distance_to_max;
 			// limit speed downwards to 0.7m/s
 			_constraints.speed_down = math::min(_max_speed_down, 0.7f);
 
 		} else {
 			_constraints.speed_down = _max_speed_down;
-
 		}
 	}
 }

src/modules/mc_pos_control/CMakeLists.txt

@@ -32,6 +32,7 @@
 ############################################################################
 
 add_subdirectory(PositionControl)
+add_subdirectory(Takeoff)
 
 px4_add_module(
 	MODULE modules__mc_pos_control
@@ -42,6 +43,7 @@ px4_add_module(
 		MulticopterPositionControl.hpp
 	DEPENDS
 		PositionControl
+		Takeoff
 		controllib
 		git_ecl
 		ecl_geo

src/modules/mc_pos_control/MulticopterPositionControl.cpp

@@ -36,24 +36,26 @@
 #include <float.h>
 #include <lib/mathlib/mathlib.h>
 #include <lib/matrix/matrix/math.hpp>
+#include "PositionControl/ControlMath.hpp"
 
 using namespace matrix;
 
 MulticopterPositionControl::MulticopterPositionControl(bool vtol) :
 	SuperBlock(nullptr, "MPC"),
 	ModuleParams(nullptr),
-	WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
 	_vehicle_attitude_setpoint_pub(vtol ? ORB_ID(mc_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),
 	_vel_x_deriv(this, "VELD"),
 	_vel_y_deriv(this, "VELD"),
-	_vel_z_deriv(this, "VELD"),
-	_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time"))
+	_vel_z_deriv(this, "VELD")
 {
 	// fetch initial parameter values
 	parameters_update(true);
 
 	// set failsafe hysteresis
 	_failsafe_land_hysteresis.set_hysteresis_time_from(false, LOITER_TIME_BEFORE_DESCEND);
+
+	reset_setpoint_to_nan(_setpoint);
 }
 
 MulticopterPositionControl::~MulticopterPositionControl()
@@ -68,10 +70,8 @@ bool MulticopterPositionControl::init()
 		return false;
 	}
 
-	// limit to every other vehicle_local_position update (50 Hz)
-	_local_pos_sub.set_interval_us(20_ms);
-
 	_time_stamp_last_loop = hrt_absolute_time();
+	ScheduleNow();
 
 	return true;
 }
@@ -156,9 +156,6 @@ int MulticopterPositionControl::parameters_update(bool force)
 			Vector3f(_param_mpc_xy_vel_p_acc.get(), _param_mpc_xy_vel_p_acc.get(), _param_mpc_z_vel_p_acc.get()),
 			Vector3f(_param_mpc_xy_vel_i_acc.get(), _param_mpc_xy_vel_i_acc.get(), _param_mpc_z_vel_i_acc.get()),
 			Vector3f(_param_mpc_xy_vel_d_acc.get(), _param_mpc_xy_vel_d_acc.get(), _param_mpc_z_vel_d_acc.get()));
-		_control.setVelocityLimits(_param_mpc_xy_vel_max.get(), _param_mpc_z_vel_max_up.get(), _param_mpc_z_vel_max_dn.get());
-		_control.setThrustLimits(_param_mpc_thr_min.get(), _param_mpc_thr_max.get());
-		_control.setTiltLimit(M_DEG_TO_RAD_F * _param_mpc_tiltmax_air.get()); // convert to radians!
 
 		// Check that the design parameters are inside the absolute maximum constraints
 		if (_param_mpc_xy_cruise.get() > _param_mpc_xy_vel_max.get()) {
@@ -189,79 +186,68 @@ int MulticopterPositionControl::parameters_update(bool force)
 		// initialize vectors from params and enforce constraints
 		_param_mpc_tko_speed.set(math::min(_param_mpc_tko_speed.get(), _param_mpc_z_vel_max_up.get()));
 		_param_mpc_land_speed.set(math::min(_param_mpc_land_speed.get(), _param_mpc_z_vel_max_dn.get()));
+
+		_takeoff.setSpoolupTime(_param_mpc_spoolup_time.get());
+		_takeoff.setTakeoffRampTime(_param_mpc_tko_ramp_t.get());
+		_takeoff.generateInitialRampValue(_param_mpc_z_vel_p_acc.get());
 	}
 
 	return OK;
 }
 
-void MulticopterPositionControl::poll_subscriptions()
+PositionControlStates MulticopterPositionControl::set_vehicle_states(const vehicle_local_position_s &local_pos)
 {
-	_control_mode_sub.update(&_control_mode);
-
-	if (_param_mpc_use_hte.get()) {
-		hover_thrust_estimate_s hte;
-
-		if (_hover_thrust_estimate_sub.update(&hte)) {
-			if (hte.valid) {
-				_control.updateHoverThrust(hte.hover_thrust);
-			}
-		}
-	}
-}
+	PositionControlStates states;
 
-void MulticopterPositionControl::set_vehicle_states(const float &vel_sp_z)
-{
 	// only set position states if valid and finite
-	if (PX4_ISFINITE(_local_pos.x) && PX4_ISFINITE(_local_pos.y) && _local_pos.xy_valid) {
-		_states.position(0) = _local_pos.x;
-		_states.position(1) = _local_pos.y;
+	if (PX4_ISFINITE(local_pos.x) && PX4_ISFINITE(local_pos.y) && local_pos.xy_valid) {
+		states.position(0) = local_pos.x;
+		states.position(1) = local_pos.y;
 
 	} else {
-		_states.position(0) = _states.position(1) = NAN;
+		states.position(0) = NAN;
+		states.position(1) = NAN;
 	}
 
-	if (PX4_ISFINITE(_local_pos.z) && _local_pos.z_valid) {
-		_states.position(2) = _local_pos.z;
+	if (PX4_ISFINITE(local_pos.z) && local_pos.z_valid) {
+		states.position(2) = local_pos.z;
 
 	} else {
-		_states.position(2) = NAN;
+		states.position(2) = NAN;
 	}
 
-	if (PX4_ISFINITE(_local_pos.vx) && PX4_ISFINITE(_local_pos.vy) && _local_pos.v_xy_valid) {
-		_states.velocity(0) = _local_pos.vx;
-		_states.velocity(1) = _local_pos.vy;
-		_states.acceleration(0) = _vel_x_deriv.update(_states.velocity(0));
-		_states.acceleration(1) = _vel_y_deriv.update(_states.velocity(1));
+	if (PX4_ISFINITE(local_pos.vx) && PX4_ISFINITE(local_pos.vy) && local_pos.v_xy_valid) {
+		states.velocity(0) = local_pos.vx;
+		states.velocity(1) = local_pos.vy;
+		states.acceleration(0) = _vel_x_deriv.update(local_pos.vx);
+		states.acceleration(1) = _vel_y_deriv.update(local_pos.vy);
 
 	} else {
-		_states.velocity(0) = _states.velocity(1) = NAN;
-		_states.acceleration(0) = _states.acceleration(1) = NAN;
+		states.velocity(0) = NAN;
+		states.velocity(1) = NAN;
+		states.acceleration(0) = NAN;
+		states.acceleration(1) = NAN;
+
 		// reset derivatives to prevent acceleration spikes when regaining velocity
 		_vel_x_deriv.reset();
 		_vel_y_deriv.reset();
 	}
 
-	if (PX4_ISFINITE(_local_pos.vz) && _local_pos.v_z_valid) {
-		_states.velocity(2) = _local_pos.vz;
-
-		if (PX4_ISFINITE(vel_sp_z) && fabsf(vel_sp_z) > FLT_EPSILON && PX4_ISFINITE(_local_pos.z_deriv)) {
-			// A change in velocity is demanded. Set velocity to the derivative of position
-			// because it has less bias but blend it in across the landing speed range
-			float weighting = fminf(fabsf(vel_sp_z) / _param_mpc_land_speed.get(), 1.0f);
-			_states.velocity(2) = _local_pos.z_deriv * weighting + _local_pos.vz * (1.0f - weighting);
-		}
-
-		_states.acceleration(2) = _vel_z_deriv.update(_states.velocity(2));
+	if (PX4_ISFINITE(local_pos.vz) && local_pos.v_z_valid) {
+		states.velocity(2) = local_pos.vz;
+		states.acceleration(2) = _vel_z_deriv.update(states.velocity(2));
 
 	} else {
-		_states.velocity(2) = _states.acceleration(2) = NAN;
+		states.velocity(2) = NAN;
+		states.acceleration(2) = NAN;
+
 		// reset derivative to prevent acceleration spikes when regaining velocity
 		_vel_z_deriv.reset();
 	}
 
-	if (PX4_ISFINITE(_local_pos.heading)) {
-		_states.yaw = _local_pos.heading;
-	}
+	states.yaw = local_pos.heading;
+
+	return states;
 }
 
 void MulticopterPositionControl::Run()
@@ -272,14 +258,16 @@ void MulticopterPositionControl::Run()
 		return;
 	}
 
-	perf_begin(_cycle_perf);
+	// reschedule backup
+	ScheduleDelayed(100_ms);
 
-	if (_local_pos_sub.update(&_local_pos)) {
+	parameters_update(false);
 
-		poll_subscriptions();
-		parameters_update(false);
+	perf_begin(_cycle_perf);
+	vehicle_local_position_s local_pos;
 
-		const hrt_abstime time_stamp_now = _local_pos.timestamp;
+	if (_local_pos_sub.update(&local_pos)) {
+		const hrt_abstime time_stamp_now = local_pos.timestamp;
 		const float dt = math::constrain(((time_stamp_now - _time_stamp_last_loop) * 1e-6f), 0.002f, 0.04f);
 		_time_stamp_last_loop = time_stamp_now;
 
@@ -289,27 +277,131 @@ void MulticopterPositionControl::Run()
 		const bool was_in_failsafe = _in_failsafe;
 		_in_failsafe = false;
 
-		vehicle_local_position_setpoint_s setpoint;
+		_control_mode_sub.update(&_control_mode);
+		_vehicle_land_detected_sub.update(&_vehicle_land_detected);
+
+		if (_param_mpc_use_hte.get()) {
+			hover_thrust_estimate_s hte;
+
+			if (_hover_thrust_estimate_sub.update(&hte)) {
+				if (hte.valid) {
+					_control.updateHoverThrust(hte.hover_thrust);
+				}
+			}
+		}
+
+		PositionControlStates states{set_vehicle_states(local_pos)};
+
+		if (_control_mode.flag_control_climb_rate_enabled) {
+
+			_trajectory_setpoint_sub.update(&_setpoint);
 
-		// check if any task is active
-		if (_trajectory_setpoint_sub.update(&setpoint)) {
-			_control.setInputSetpoint(setpoint);
+			// adjust existing (or older) setpoint with any EKF reset deltas
+			if (_setpoint.timestamp < local_pos.timestamp) {
+				if (local_pos.vxy_reset_counter != _vxy_reset_counter) {
+					if (PX4_ISFINITE(_setpoint.vx)) {
+						_setpoint.vx += local_pos.delta_vxy[0];
+					}
 
-			// check if all local states are valid and map accordingly
-			set_vehicle_states(setpoint.vz);
-			_control.setState(_states);
+					if (PX4_ISFINITE(_setpoint.vy)) {
+						_setpoint.vy += local_pos.delta_vxy[1];
+					}
+				}
+
+				if (local_pos.vz_reset_counter != _vz_reset_counter) {
+					if (PX4_ISFINITE(_setpoint.vz)) {
+						_setpoint.vz += local_pos.delta_vz;
+					}
+				}
 
-			vehicle_constraints_s constraints;
+				if (local_pos.xy_reset_counter != _xy_reset_counter) {
+					if (PX4_ISFINITE(_setpoint.x)) {
+						_setpoint.x += local_pos.delta_xy[0];
+					}
 
-			if (_vehicle_constraints_sub.update(&constraints)) {
-				_control.setConstraints(constraints);
-				_control.setThrustLimits(constraints.minimum_thrust, _param_mpc_thr_max.get());
+					if (PX4_ISFINITE(_setpoint.y)) {
+						_setpoint.y += local_pos.delta_xy[1];
+					}
+				}
 
-				if (constraints.reset_integral) {
-					_control.resetIntegral();
+				if (local_pos.z_reset_counter != _z_reset_counter) {
+					if (PX4_ISFINITE(_setpoint.z)) {
+						_setpoint.z += local_pos.delta_z;
+					}
 				}
+
+				if (local_pos.heading_reset_counter != _heading_reset_counter) {
+					if (PX4_ISFINITE(_setpoint.yaw)) {
+						_setpoint.yaw += local_pos.delta_heading;
+					}
+				}
+			}
+
+			// update vehicle constraints and handle smooth takeoff
+			_vehicle_constraints_sub.update(&_vehicle_constraints);
+
+			// fix to prevent the takeoff ramp to ramp to a too high value or get stuck because of NAN
+			// TODO: this should get obsolete once the takeoff limiting moves into the flight tasks
+			if (!PX4_ISFINITE(_vehicle_constraints.speed_up) || (_vehicle_constraints.speed_up > _param_mpc_z_vel_max_up.get())) {
+				_vehicle_constraints.speed_up = _param_mpc_z_vel_max_up.get();
+			}
+
+			// handle smooth takeoff
+			_takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, _vehicle_constraints.want_takeoff,
+						    _vehicle_constraints.speed_up, false, time_stamp_now);
+
+			const bool not_taken_off = (_takeoff.getTakeoffState() < TakeoffState::rampup);
+			const bool flying = (_takeoff.getTakeoffState() >= TakeoffState::flight);
+			const bool flying_but_ground_contact = (flying && _vehicle_land_detected.ground_contact);
+
+			if (not_taken_off || flying_but_ground_contact) {
+				// we are not flying yet and need to avoid any corrections
+				reset_setpoint_to_nan(_setpoint);
+				Vector3f(0.f, 0.f, 100.f).copyTo(_setpoint.acceleration); // High downwards acceleration to make sure there's no thrust
+
+				// prevent any integrator windup
+				_control.resetIntegral();
 			}
 
+			// limit tilt during takeoff ramupup
+			if (_takeoff.getTakeoffState() < TakeoffState::flight) {
+				_control.setTiltLimit(math::radians(_param_mpc_tiltmax_lnd.get()));
+
+			} else {
+				_control.setTiltLimit(math::radians(_param_mpc_tiltmax_air.get()));
+			}
+
+			const float speed_up = _takeoff.updateRamp(dt, _vehicle_constraints.speed_up);
+			const float speed_down = PX4_ISFINITE(_vehicle_constraints.speed_down) ? _vehicle_constraints.speed_down :
+						 _param_mpc_z_vel_max_dn.get();
+			const float speed_horizontal = PX4_ISFINITE(_vehicle_constraints.speed_xy) ? _vehicle_constraints.speed_xy :
+						       _param_mpc_xy_vel_max.get();
+
+			// Allow ramping from zero thrust on takeoff
+			const float minimum_thrust = flying ? _param_mpc_thr_min.get() : 0.f;
+
+			_control.setThrustLimits(minimum_thrust, _param_mpc_thr_max.get());
+
+			_control.setVelocityLimits(
+				math::constrain(speed_horizontal, 0.f, _param_mpc_xy_vel_max.get()),
+				math::constrain(speed_up, 0.f, _param_mpc_z_vel_max_up.get()),
+				math::constrain(speed_down, 0.f, _param_mpc_z_vel_max_dn.get()));
+
+			_control.setInputSetpoint(_setpoint);
+
+			// update states
+			if (PX4_ISFINITE(_setpoint.vz) && (fabsf(_setpoint.vz) > FLT_EPSILON)
+			    && PX4_ISFINITE(local_pos.z_deriv) && local_pos.z_valid && local_pos.v_z_valid) {
+				// A change in velocity is demanded. Set velocity to the derivative of position
+				// because it has less bias but blend it in across the landing speed range
+				//  <  MPC_LAND_SPEED: ramp up using altitude derivative without a step
+				//  >= MPC_LAND_SPEED: use altitude derivative
+				float weighting = fminf(fabsf(_setpoint.vz) / _param_mpc_land_speed.get(), 1.f);
+				states.velocity(2) = local_pos.z_deriv * weighting + local_pos.vz * (1.f - weighting);
+			}
+
+			_control.setState(states);
+
 			// Run position control
 			if (_control.update(dt)) {
 				_failsafe_land_hysteresis.set_state_and_update(false, time_stamp_now);
@@ -321,13 +413,13 @@ void MulticopterPositionControl::Run()
 					_last_warn = time_stamp_now;
 				}
 
-				failsafe(time_stamp_now, setpoint, _states, !was_in_failsafe);
+				failsafe(time_stamp_now, _setpoint, states, !was_in_failsafe);
 
-				_control.setInputSetpoint(setpoint);
-
-				constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, false, {}};
-				_control.setConstraints(constraints);
+				// reset constraints
+				_vehicle_constraints = {0, NAN, NAN, NAN, false, {}};
 
+				_control.setInputSetpoint(_setpoint);
+				_control.setVelocityLimits(_param_mpc_xy_vel_max.get(), _param_mpc_z_vel_max_up.get(), _param_mpc_z_vel_max_dn.get());
 				_control.update(dt);
 			}
 
@@ -335,26 +427,39 @@ void MulticopterPositionControl::Run()
 			// on top of the input/feed-forward setpoints these containt the PID corrections
 			// This message is used by other modules (such as Landdetector) to determine vehicle intention.
 			vehicle_local_position_setpoint_s local_pos_sp{};
-			local_pos_sp.timestamp = time_stamp_now;
 			_control.getLocalPositionSetpoint(local_pos_sp);
+			local_pos_sp.timestamp = hrt_absolute_time();
 			_local_pos_sp_pub.publish(local_pos_sp);
 
 			// Publish attitude setpoint output
-			// It's important to publish also when disarmed otheriwse the attitude setpoint stays uninitialized.
-			// Not publishing when not running a flight task
-			// in stabilized mode attitude setpoints get ignored
-			// in offboard with attitude setpoints they come from MAVLink directly
 			vehicle_attitude_setpoint_s attitude_setpoint{};
-			attitude_setpoint.timestamp = time_stamp_now;
 			_control.getAttitudeSetpoint(attitude_setpoint);
+			attitude_setpoint.timestamp = hrt_absolute_time();
 			_vehicle_attitude_setpoint_pub.publish(attitude_setpoint);
 
 		} else {
-			// reset the numerical derivatives to not generate d term spikes when coming from non-position controlled operation
-			_vel_x_deriv.reset();
-			_vel_y_deriv.reset();
-			_vel_z_deriv.reset();
+			// 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, 10.f, true, time_stamp_now);
+		}
+
+		// Publish takeoff status
+		const uint8_t takeoff_state = static_cast<uint8_t>(_takeoff.getTakeoffState());
+
+		if (takeoff_state != _old_takeoff_state) {
+			takeoff_status_s takeoff_status{};
+			takeoff_status.takeoff_state = takeoff_state;
+			takeoff_status.timestamp = hrt_absolute_time();
+			_takeoff_status_pub.publish(takeoff_status);
+
+			_old_takeoff_state = takeoff_state;
 		}
+
+		// save latest reset counters
+		_vxy_reset_counter = local_pos.vxy_reset_counter;
+		_vz_reset_counter = local_pos.vz_reset_counter;
+		_xy_reset_counter = local_pos.xy_reset_counter;
+		_z_reset_counter = local_pos.z_reset_counter;
+		_heading_reset_counter = local_pos.heading_reset_counter;
 	}
 
 	perf_end(_cycle_perf);
@@ -374,7 +479,7 @@ void MulticopterPositionControl::failsafe(const hrt_abstime &now, vehicle_local_
 	if (_failsafe_land_hysteresis.get_state()) {
 		reset_setpoint_to_nan(setpoint);
 
-		if (PX4_ISFINITE(_states.velocity(0)) && PX4_ISFINITE(_states.velocity(1))) {
+		if (PX4_ISFINITE(states.velocity(0)) && PX4_ISFINITE(states.velocity(1))) {
 			// don't move along xy
 			setpoint.vx = setpoint.vy = 0.f;
 
@@ -392,7 +497,7 @@ void MulticopterPositionControl::failsafe(const hrt_abstime &now, vehicle_local_
 			}
 		}
 
-		if (PX4_ISFINITE(_states.velocity(2))) {
+		if (PX4_ISFINITE(states.velocity(2))) {
 			// don't move along z if we can stop in all dimensions
 			if (!PX4_ISFINITE(setpoint.vz)) {
 				setpoint.vz = 0.f;

src/modules/mc_pos_control/MulticopterPositionControl.hpp

@@ -37,7 +37,9 @@
 
 #pragma once
 
-#include <commander/px4_custom_mode.h>
+#include "PositionControl/PositionControl.hpp"
+#include "Takeoff/Takeoff.hpp"
+
 #include <drivers/drv_hrt.h>
 #include <lib/controllib/blocks.hpp>
 #include <lib/hysteresis/hysteresis.h>
@@ -50,23 +52,22 @@
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <px4_platform_common/posix.h>
 #include <px4_platform_common/tasks.h>
-
+#include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
-#include <uORB/Publication.hpp>
 #include <uORB/topics/hover_thrust_estimate.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_constraints.h>
 #include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 
-#include "PositionControl/PositionControl.hpp"
-
 using namespace time_literals;
 
 class MulticopterPositionControl : public ModuleBase<MulticopterPositionControl>, public control::SuperBlock,
-	public ModuleParams, public px4::WorkItem
+	public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
 	MulticopterPositionControl(bool vtol = false);
@@ -86,26 +87,46 @@ public:
 private:
 	void Run() override;
 
-	uORB::Publication<vehicle_attitude_setpoint_s>	_vehicle_attitude_setpoint_pub;
+	Takeoff _takeoff; /**< state machine and ramp to bring the vehicle off the ground without jumps */
+
 	orb_advert_t _mavlink_log_pub{nullptr};
 
-	uORB::Publication<vehicle_local_position_setpoint_s>	_local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)};	/**< vehicle local position setpoint publication */
+	uORB::Publication<takeoff_status_s> _takeoff_status_pub{ORB_ID(takeoff_status)};
+	uORB::Publication<vehicle_attitude_setpoint_s>	_vehicle_attitude_setpoint_pub;
+	uORB::Publication<vehicle_local_position_setpoint_s> _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)};	/**< vehicle local position setpoint publication */
 
 	uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)};	/**< vehicle local position */
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
-	uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)};		/**< vehicle control mode subscription */
+	uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
 	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
+	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 	uORB::Subscription _vehicle_constraints_sub{ORB_ID(vehicle_constraints)};
 
 	hrt_abstime	_time_stamp_last_loop{0};		/**< time stamp of last loop iteration */
 
 	int _task_failure_count{0};         /**< counter for task failures */
 
-	vehicle_control_mode_s	_control_mode{};		/**< vehicle control mode */
-	vehicle_local_position_s _local_pos{};			/**< vehicle local position */
+	vehicle_control_mode_s _control_mode{};
+	vehicle_local_position_setpoint_s _setpoint{};
+	vehicle_constraints_s _vehicle_constraints{
+		.timestamp = 0,
+		.speed_xy = NAN,
+		.speed_up = NAN,
+		.speed_down = NAN,
+		.want_takeoff = false,
+	};
+
+	vehicle_land_detected_s _vehicle_land_detected {
+		.timestamp = 0,
+		.alt_max = -1.0f,
+		.freefall = false,
+		.ground_contact = true,
+		.maybe_landed = true,
+		.landed = true,
+	};
 
 	DEFINE_PARAMETERS(
 		// Position Control
@@ -125,6 +146,8 @@ private:
 		(ParamBool<px4::params::MPC_USE_HTE>) _param_mpc_use_hte,
 
 		// Takeoff / Land
+		(ParamFloat<px4::params::MPC_SPOOLUP_TIME>) _param_mpc_spoolup_time, /**< time to let motors spool up after arming */
+		(ParamFloat<px4::params::MPC_TKO_RAMP_T>) _param_mpc_tko_ramp_t, /**< time constant for smooth takeoff ramp */
 		(ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed,
 		(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
 
@@ -157,11 +180,10 @@ private:
 	control::BlockDerivative _vel_z_deriv; /**< velocity derivative in z */
 
 	PositionControl _control; /**< class for core PID position control */
-	PositionControlStates _states{}; /**< structure containing vehicle state information for position control */
 
-	hrt_abstime _last_warn = 0; /**< timer when the last warn message was sent out */
+	hrt_abstime _last_warn{0}; /**< timer when the last warn message was sent out */
 
-	bool _in_failsafe = false; /**< true if failsafe was entered within current cycle */
+	bool _in_failsafe{false}; /**< true if failsafe was entered within current cycle */
 
 	bool _hover_thrust_initialized{false};
 
@@ -176,7 +198,15 @@ private:
 
 	systemlib::Hysteresis _failsafe_land_hysteresis{false}; /**< becomes true if task did not update correctly for LOITER_TIME_BEFORE_DESCEND */
 
-	perf_counter_t _cycle_perf;
+	uint8_t _old_takeoff_state{};
+
+	uint8_t _vxy_reset_counter{0};
+	uint8_t _vz_reset_counter{0};
+	uint8_t _xy_reset_counter{0};
+	uint8_t _z_reset_counter{0};
+	uint8_t _heading_reset_counter{0};
+
+	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")};
 
 	/**
 	 * Update our local parameter cache.
@@ -185,16 +215,10 @@ private:
 	 */
 	int parameters_update(bool force);
 
-	/**
-	 * Check for changes in subscribed topics.
-	 */
-	void poll_subscriptions();
-
 	/**
 	 * Check for validity of positon/velocity states.
-	 * @param vel_sp_z velocity setpoint in z-direction
 	 */
-	void set_vehicle_states(const float &vel_sp_z);
+	PositionControlStates set_vehicle_states(const vehicle_local_position_s &local_pos);
 
 	/**
 	 * Adjust the setpoint during landing.

src/modules/mc_pos_control/PositionControl/CMakeLists.txt

@@ -32,13 +32,12 @@
 ############################################################################
 
 px4_add_library(PositionControl
-	PositionControl.cpp
 	ControlMath.cpp
+	ControlMath.hpp
+	PositionControl.cpp
+	PositionControl.hpp
 )
-target_include_directories(PositionControl
-	PUBLIC
-	${CMAKE_CURRENT_SOURCE_DIR}
-)
+target_include_directories(PositionControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} )
 
 px4_add_unit_gtest(SRC ControlMathTest.cpp LINKLIBS PositionControl)
 px4_add_unit_gtest(SRC PositionControlTest.cpp LINKLIBS PositionControl)

src/modules/mc_pos_control/PositionControl/PositionControl.cpp

@@ -88,27 +88,6 @@ void PositionControl::setInputSetpoint(const vehicle_local_position_setpoint_s &
 	_yawspeed_sp = setpoint.yawspeed;
 }
 
-void PositionControl::setConstraints(const vehicle_constraints_s &constraints)
-{
-	_constraints = constraints;
-
-	// For safety check if adjustable constraints are below global constraints. If they are not stricter than global
-	// constraints, then just use global constraints for the limits.
-	if (!PX4_ISFINITE(constraints.tilt) || (constraints.tilt > _lim_tilt)) {
-		_constraints.tilt = _lim_tilt;
-	}
-
-	if (!PX4_ISFINITE(constraints.speed_up) || (constraints.speed_up > _lim_vel_up)) {
-		_constraints.speed_up = _lim_vel_up;
-	}
-
-	if (!PX4_ISFINITE(constraints.speed_down) || (constraints.speed_down > _lim_vel_down)) {
-		_constraints.speed_down = _lim_vel_down;
-	}
-
-	// ignore _constraints.speed_xy TODO: remove it completely as soon as no task uses it anymore to avoid confusion
-}
-
 bool PositionControl::update(const float dt)
 {
 	// x and y input setpoints always have to come in pairs
@@ -138,7 +117,7 @@ void PositionControl::_positionControl()
 	// the desired position setpoint over the feed-forward term.
 	_vel_sp.xy() = ControlMath::constrainXY(vel_sp_position.xy(), (_vel_sp - vel_sp_position).xy(), _lim_vel_horizontal);
 	// Constrain velocity in z-direction.
-	_vel_sp(2) = math::constrain(_vel_sp(2), -_constraints.speed_up, _constraints.speed_down);
+	_vel_sp(2) = math::constrain(_vel_sp(2), -_lim_vel_up, _lim_vel_down);
 }
 
 void PositionControl::_velocityControl(const float dt)
@@ -198,7 +177,7 @@ void PositionControl::_accelerationControl()
 {
 	// Assume standard acceleration due to gravity in vertical direction for attitude generation
 	Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), CONSTANTS_ONE_G).normalized();
-	ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _constraints.tilt);
+	ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _lim_tilt);
 	// Scale thrust assuming hover thrust produces standard gravity
 	float collective_thrust = _acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust;
 	// Project thrust to planned body attitude

src/modules/mc_pos_control/PositionControl/PositionControl.hpp

@@ -41,7 +41,6 @@
 
 #include <matrix/matrix/math.hpp>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/vehicle_constraints.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 
 struct PositionControlStates {
@@ -139,13 +138,6 @@ public:
 	 */
 	void setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint);
 
-	/**
-	 * Pass constraints that are stricter than the global limits
-	 * Note: NAN value means no constraint, take maximum limit of controller.
-	 * @param constraints a PositionControl structure with supported constraints
-	 */
-	void setConstraints(const vehicle_constraints_s &constraints);
-
 	/**
 	 * Apply P-position and PID-velocity controller that updates the member
 	 * thrust, yaw- and yawspeed-setpoints.
@@ -209,8 +201,6 @@ private:
 	matrix::Vector3f _vel_int; /**< integral term of the velocity controller */
 	float _yaw{}; /**< current heading */
 
-	vehicle_constraints_s _constraints{}; /**< variable constraints */
-
 	// Setpoints
 	matrix::Vector3f _pos_sp; /**< desired position */
 	matrix::Vector3f _vel_sp; /**< desired velocity */

src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp

@@ -82,11 +82,6 @@ public:
 		_position_control.setTiltLimit(1.f);
 		_position_control.setHoverThrust(.5f);
 
-		_contraints.tilt = 1.f;
-		_contraints.speed_xy = NAN;
-		_contraints.speed_up = NAN;
-		_contraints.speed_down = NAN;
-
 		resetInputSetpoint();
 	}
 
@@ -106,7 +101,6 @@ public:
 
 	bool runController()
 	{
-		_position_control.setConstraints(_contraints);
 		_position_control.setInputSetpoint(_input_setpoint);
 		const bool ret = _position_control.update(.1f);
 		_position_control.getLocalPositionSetpoint(_output_setpoint);
@@ -115,7 +109,6 @@ public:
 	}
 
 	PositionControl _position_control;
-	vehicle_constraints_s _contraints{};
 	vehicle_local_position_setpoint_s _input_setpoint{};
 	vehicle_local_position_setpoint_s _output_setpoint{};
 	vehicle_attitude_setpoint_s _attitude{};
@@ -168,12 +161,14 @@ TEST_F(PositionControlBasicTest, TiltLimit)
 	EXPECT_GT(angle, 0.f);
 	EXPECT_LE(angle, 1.f);
 
-	_contraints.tilt = .5f;
+	_position_control.setTiltLimit(0.5f);
 	EXPECT_TRUE(runController());
 	body_z = Quatf(_attitude.q_d).dcm_z();
 	angle = acosf(body_z.dot(Vector3f(0.f, 0.f, 1.f)));
 	EXPECT_GT(angle, 0.f);
 	EXPECT_LE(angle, .50001f);
+
+	_position_control.setTiltLimit(1.f);  // restore original
 }
 
 TEST_F(PositionControlBasicTest, VelocityLimit)

src/modules/mc_pos_control/Takeoff/CMakeLists.txt

@@ -33,7 +33,9 @@
 
 px4_add_library(Takeoff
 	Takeoff.cpp
+	Takeoff.hpp
 )
+#target_compile_definitions(Takeoff PRIVATE DEBUG_BUILD)
 target_include_directories(Takeoff PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 target_link_libraries(Takeoff PUBLIC hysteresis)