move takeoff state machine flight_mode_manager -> mc_pos_control

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);
+	PositionControlStates states;
 
-	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);
-			}
-		}
-	}
-}
-
-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;
 	}
 
+	// reschedule backup
+	ScheduleDelayed(100_ms);
+
+	parameters_update(false);
+
 	perf_begin(_cycle_perf);
+	vehicle_local_position_s local_pos;
 
-	if (_local_pos_sub.update(&_local_pos)) {
-
-		poll_subscriptions();
-		parameters_update(false);
-
-		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,26 +277,130 @@ 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);
 
-		// check if any task is active
-		if (_trajectory_setpoint_sub.update(&setpoint)) {
-			_control.setInputSetpoint(setpoint);
+		if (_param_mpc_use_hte.get()) {
+			hover_thrust_estimate_s hte;
 
-			// check if all local states are valid and map accordingly
-			set_vehicle_states(setpoint.vz);
-			_control.setState(_states);
-
-			vehicle_constraints_s constraints;
-
-			if (_vehicle_constraints_sub.update(&constraints)) {
-				_control.setConstraints(constraints);
-				_control.setThrustLimits(constraints.minimum_thrust, _param_mpc_thr_max.get());
-
-				if (constraints.reset_integral) {
-					_control.resetIntegral();
+			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);
+
+			// 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];
+					}
+
+					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;
+					}
+				}
+
+				if (local_pos.xy_reset_counter != _xy_reset_counter) {
+					if (PX4_ISFINITE(_setpoint.x)) {
+						_setpoint.x += local_pos.delta_xy[0];
+					}
+
+					if (PX4_ISFINITE(_setpoint.y)) {
+						_setpoint.y += local_pos.delta_xy[1];
+					}
+				}
+
+				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)) {
@@ -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

@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+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)
+
+px4_add_unit_gtest(SRC TakeoffTest.cpp LINKLIBS Takeoff)

src/modules/mc_pos_control/Takeoff/Takeoff.cpp

@@ -0,0 +1,132 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Takeoff.cpp
+ */
+
+#include "Takeoff.hpp"
+#include <mathlib/mathlib.h>
+#include <lib/ecl/geo/geo.h>
+
+void Takeoff::generateInitialRampValue(float velocity_p_gain)
+{
+	velocity_p_gain = math::max(velocity_p_gain, 0.01f);
+	_takeoff_ramp_vz_init = -CONSTANTS_ONE_G / velocity_p_gain;
+}
+
+void Takeoff::updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff,
+				 const float takeoff_desired_vz, const bool skip_takeoff, const hrt_abstime &now_us)
+{
+	_spoolup_time_hysteresis.set_state_and_update(armed, now_us);
+
+	switch (_takeoff_state) {
+	case TakeoffState::disarmed:
+		if (armed) {
+			_takeoff_state = TakeoffState::spoolup;
+
+		} else {
+			break;
+		}
+
+	// FALLTHROUGH
+	case TakeoffState::spoolup:
+		if (_spoolup_time_hysteresis.get_state()) {
+			_takeoff_state = TakeoffState::ready_for_takeoff;
+
+		} else {
+			break;
+		}
+
+	// FALLTHROUGH
+	case TakeoffState::ready_for_takeoff:
+		if (want_takeoff) {
+			_takeoff_state = TakeoffState::rampup;
+			_takeoff_ramp_vz = _takeoff_ramp_vz_init;
+
+		} else {
+			break;
+		}
+
+	// FALLTHROUGH
+	case TakeoffState::rampup:
+		if (_takeoff_ramp_vz >= takeoff_desired_vz) {
+			_takeoff_state = TakeoffState::flight;
+
+		} else {
+			break;
+		}
+
+	// FALLTHROUGH
+	case TakeoffState::flight:
+		if (landed) {
+			_takeoff_state = TakeoffState::ready_for_takeoff;
+		}
+
+		break;
+
+	default:
+		break;
+	}
+
+	if (armed && skip_takeoff) {
+		_takeoff_state = TakeoffState::flight;
+	}
+
+	// TODO: need to consider free fall here
+	if (!armed) {
+		_takeoff_state = TakeoffState::disarmed;
+	}
+}
+
+float Takeoff::updateRamp(const float dt, const float takeoff_desired_vz)
+{
+	if (_takeoff_state < TakeoffState::rampup) {
+		return _takeoff_ramp_vz_init;
+	}
+
+	if (_takeoff_state == TakeoffState::rampup) {
+		if (_takeoff_ramp_time > dt) {
+			_takeoff_ramp_vz += (takeoff_desired_vz - _takeoff_ramp_vz_init) * dt / _takeoff_ramp_time;
+
+		} else {
+			_takeoff_ramp_vz = takeoff_desired_vz;
+		}
+
+		if (_takeoff_ramp_vz < takeoff_desired_vz) {
+			return _takeoff_ramp_vz;
+		}
+	}
+
+	return takeoff_desired_vz;
+}

src/modules/mc_pos_control/Takeoff/Takeoff.hpp

@@ -0,0 +1,102 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Takeoff.hpp
+ *
+ * A class handling all takeoff states and a smooth ramp up of the motors.
+ */
+
+#pragma once
+
+#include <lib/hysteresis/hysteresis.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/takeoff_status.h>
+
+using namespace time_literals;
+
+enum class TakeoffState {
+	disarmed = takeoff_status_s::TAKEOFF_STATE_DISARMED,
+	spoolup = takeoff_status_s::TAKEOFF_STATE_SPOOLUP,
+	ready_for_takeoff = takeoff_status_s::TAKEOFF_STATE_READY_FOR_TAKEOFF,
+	rampup = takeoff_status_s::TAKEOFF_STATE_RAMPUP,
+	flight = takeoff_status_s::TAKEOFF_STATE_FLIGHT
+};
+
+class Takeoff
+{
+public:
+	Takeoff() = default;
+	~Takeoff() = default;
+
+	// initialize parameters
+	void setSpoolupTime(const float seconds) { _spoolup_time_hysteresis.set_hysteresis_time_from(false, seconds * 1_s); }
+	void setTakeoffRampTime(const float seconds) { _takeoff_ramp_time = seconds; }
+
+	/**
+	 * Calculate a vertical velocity to initialize the takeoff ramp
+	 * that when passed to the velocity controller results in a zero throttle setpoint.
+	 * @param hover_thrust normalized thrsut value with which the vehicle hovers
+	 * @param velocity_p_gain proportional gain of the velocity controller to calculate the thrust
+	 */
+	void generateInitialRampValue(const float velocity_p_gain);
+
+	/**
+	 * Update the state for the takeoff.
+	 * @param setpoint a vehicle_local_position_setpoint_s structure
+	 * @return true if setpoint has updated correctly
+	 */
+	void updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff,
+				const float takeoff_desired_vz, const bool skip_takeoff, const hrt_abstime &now_us);
+
+	/**
+	 * Update and return the velocity constraint ramp value during takeoff.
+	 * By ramping up _takeoff_ramp_vz during the takeoff and using it to constain the maximum climb rate a smooth takeoff behavior is achieved.
+	 * Returns zero on the ground and takeoff_desired_vz in flight.
+	 * @param dt time in seconds since the last call/loop iteration
+	 * @param takeoff_desired_vz end value for the velocity ramp
+	 * @return true if setpoint has updated correctly
+	 */
+	float updateRamp(const float dt, const float takeoff_desired_vz);
+
+	TakeoffState getTakeoffState() { return _takeoff_state; }
+
+private:
+	TakeoffState _takeoff_state = TakeoffState::disarmed;
+
+	systemlib::Hysteresis _spoolup_time_hysteresis{false}; /**< becomes true MPC_SPOOLUP_TIME seconds after the vehicle was armed */
+
+	float _takeoff_ramp_time = 0.f;
+	float _takeoff_ramp_vz_init = 0.f;
+	float _takeoff_ramp_vz = 0.f;
+};

src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp

@@ -0,0 +1,79 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <gtest/gtest.h>
+#include <Takeoff.hpp>
+#include <drivers/drv_hrt.h>
+#include <lib/ecl/geo/geo.h>
+
+TEST(TakeoffTest, Initialization)
+{
+	Takeoff takeoff;
+	EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::disarmed);
+}
+
+TEST(TakeoffTest, RegularTakeoffRamp)
+{
+	Takeoff takeoff;
+	takeoff.setSpoolupTime(1.f);
+	takeoff.setTakeoffRampTime(2.0);
+	takeoff.generateInitialRampValue(CONSTANTS_ONE_G / 0.5f);
+
+	// disarmed, landed, don't want takeoff
+	takeoff.updateTakeoffState(false, true, false, 1.f, false, 0);
+	EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::disarmed);
+
+	// armed, not landed anymore, don't want takeoff
+	takeoff.updateTakeoffState(true, false, false, 1.f, false, 500_ms);
+	EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::spoolup);
+
+	// armed, not landed, don't want takeoff yet, spoolup time passed
+	takeoff.updateTakeoffState(true, false, false, 1.f, false, 2_s);
+	EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::ready_for_takeoff);
+
+	// armed, not landed, want takeoff
+	takeoff.updateTakeoffState(true, false, true, 1.f, false, 3_s);
+	EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::rampup);
+
+	// armed, not landed, want takeoff, ramping up
+	takeoff.updateTakeoffState(true, false, true, 1.f, false, 4_s);
+	EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 0.f);
+	EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), .5f);
+	EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 1.f);
+	EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 1.5f);
+	EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 1.5f);
+
+	// armed, not landed, want takeoff, rampup time passed
+	takeoff.updateTakeoffState(true, false, true, 1.f, false, 6500_ms);
+	EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::flight);
+}