move takeoff state machine flight_mode_manager -> mc_pos_control

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;
-
 		}
 	}
 }