mc_pos_control: limit altitude

mc_pos_control: delete spaces

mc_pos_control: cleanup and threshold for velocity control to altitude control when close to max altitude

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -208,7 +208,7 @@ private:
 		param_t alt_mode;
 		param_t opt_recover;
 		param_t xy_vel_man_expo;
-
+		param_t altitude_max;
 	}		_params_handles;		/**< handles for interesting parameters */
 
 	struct {
@@ -236,6 +236,7 @@ private:
 		float vel_max_up;
 		float vel_max_down;
 		float xy_vel_man_expo;
+		float altitude_max;
 		uint32_t alt_mode;
 
 		int opt_recover;
@@ -290,6 +291,7 @@ private:
 	float _vel_z_lp;
 	float _acc_z_lp;
 	float _takeoff_thrust_sp;
+	float _valid_altitude_max; /**< maximum altitude that can be reached based on several conditions */
 
 	// counters for reset events on position and velocity states
 	// they are used to identify a reset event
@@ -374,6 +376,11 @@ private:
 
 	void generate_attitude_setpoint(float dt);
 
+	/**
+	 * limit altitude based on several conditions
+	 */
+	void limit_altitude();
+
 	/**
 	 * Shim for calling task_main from task_create.
 	 */
@@ -532,6 +539,9 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_params_handles.alt_mode = param_find("MPC_ALT_MODE");
 	_params_handles.opt_recover = param_find("VT_OPT_RECOV_EN");
 	_params_handles.xy_vel_man_expo = param_find("MPC_XY_MAN_EXPO");
+	_params_handles.altitude_max = param_find("MPC_ALTITUDE_MAX");
+
+	_valid_altitude_max = _params_handles.altitude_max;
 
 	/* fetch initial parameter values */
 	parameters_update(true);
@@ -649,6 +659,11 @@ MulticopterPositionControl::parameters_update(bool force)
 		_params.acc_down_max = v;
 		param_get(_params_handles.xy_vel_man_expo, &v);
 		_params.xy_vel_man_expo = v;
+		param_get(_params_handles.altitude_max, &v);
+		_params.altitude_max = v;
+
+		/* ToDo: the max altitude will be sent from navigator */
+		_valid_altitude_max = _params.altitude_max;
 
 		/*
 		 * increase the maximum horizontal acceleration such that stopping
@@ -912,6 +927,25 @@ MulticopterPositionControl::reset_alt_sp()
 	}
 }
 
+void
+MulticopterPositionControl::limit_altitude()
+{
+	float alt = _pos(2);
+
+	/* in altitude control, limit setpoint */
+	if (-alt >= _valid_altitude_max && _run_alt_control && _pos_sp(2) <= alt) {
+		_pos_sp(2) = -_valid_altitude_max;
+		return;
+	}
+
+	/* in velocity control in z, prevent vehicle from flying upwards if 0.5m close to altitude max*/
+	if (-alt + 0.5f >= _valid_altitude_max && !_run_alt_control && _vel_sp(2) <= 0.0f) {
+		_pos_sp(2) = -_valid_altitude_max;
+		_run_alt_control = true;
+		return;
+	}
+}
+
 
 
 void
@@ -1667,11 +1701,12 @@ MulticopterPositionControl::control_position(float dt)
 		_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);
 	}
 
+	limit_altitude();
+
 	if (_run_alt_control) {
 		_vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2);
 	}
 
-
 	/* make sure velocity setpoint is saturated in xy*/
 	float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) +
 				  _vel_sp(1) * _vel_sp(1));

src/modules/mc_pos_control/mc_pos_control_params.c

@@ -424,7 +424,6 @@ PARAM_DEFINE_FLOAT(MPC_HOLD_XY_DZ, 0.1f);
  */
 PARAM_DEFINE_FLOAT(MPC_HOLD_Z_DZ, 0.1f);
 
-
 /**
  * Maximum horizontal velocity for which position hold is enabled (use 0 to disable check)
  *
@@ -519,3 +518,15 @@ PARAM_DEFINE_INT32(MPC_ALT_MODE, 0);
  * @group Multicopter Position Control
  */
 PARAM_DEFINE_FLOAT(MPC_XY_MAN_EXPO, 0.0f);
+
+/**
+ * Maximum altitude that can be reached
+ *
+ * @unit m
+ * @min 5
+ * @max 150
+ * @increment 1
+ * @decimal 2
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_ALTITUDE_MAX, 10.0f);