mc_pos_control delete unused velocity feed forwards

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -159,7 +159,6 @@ private:
 		param_t z_vel_d;
 		param_t z_vel_max_up;
 		param_t z_vel_max_down;
-		param_t z_ff;
 		param_t slow_land_alt1;
 		param_t slow_land_alt2;
 		param_t xy_p;
@@ -168,7 +167,6 @@ private:
 		param_t xy_vel_d;
 		param_t xy_vel_max;
 		param_t xy_vel_cruise;
-		param_t xy_ff;
 		param_t tilt_max_air;
 		param_t land_speed;
 		param_t tko_speed;
@@ -215,8 +213,7 @@ private:
 		math::Vector<3> vel_p;
 		math::Vector<3> vel_i;
 		math::Vector<3> vel_d;
-		math::Vector<3> vel_ff;
+	} _params{};
-	}		_params;
 
 	struct map_projection_reference_s _ref_pos;
 	float _ref_alt;
@@ -248,7 +245,6 @@ private:
 	math::Vector<3> _vel;
 	math::Vector<3> _vel_sp;
 	math::Vector<3> _vel_prev;			/**< velocity on previous step */
-	math::Vector<3> _vel_ff;
 	math::Vector<3> _vel_sp_prev;
 	math::Vector<3> _vel_err_d;		/**< derivative of current velocity */
 	math::Vector<3> _curr_pos_sp;  /**< current setpoint of the triplets */
@@ -449,14 +445,13 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_params.vel_p.zero();
 	_params.vel_i.zero();
 	_params.vel_d.zero();
-	_params.vel_ff.zero();
 
 	_pos.zero();
 	_pos_sp.zero();
+
 	_vel.zero();
 	_vel_sp.zero();
 	_vel_prev.zero();
-	_vel_ff.zero();
 	_vel_sp_prev.zero();
 	_vel_err_d.zero();
 	_curr_pos_sp.zero();
@@ -476,7 +471,6 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_params_handles.z_vel_d		= param_find("MPC_Z_VEL_D");
 	_params_handles.z_vel_max_up	= param_find("MPC_Z_VEL_MAX_UP");
 	_params_handles.z_vel_max_down	= param_find("MPC_Z_VEL_MAX_DN");
-	_params_handles.z_ff		= param_find("MPC_Z_FF");
 
 	_params_handles.xy_p		= param_find("MPC_XY_P");
 	_params_handles.xy_vel_p	= param_find("MPC_XY_VEL_P");
@@ -484,7 +478,6 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_params_handles.xy_vel_d	= param_find("MPC_XY_VEL_D");
 	_params_handles.xy_vel_max	= param_find("MPC_XY_VEL_MAX");
 	_params_handles.xy_vel_cruise	= param_find("MPC_XY_CRUISE");
-	_params_handles.xy_ff		= param_find("MPC_XY_FF");
 
 	_params_handles.slow_land_alt1  = param_find("MPC_LAND_ALT1");
 	_params_handles.slow_land_alt2  = param_find("MPC_LAND_ALT2");
@@ -591,13 +584,6 @@ MulticopterPositionControl::parameters_update(bool force)
 		_params.vel_max_xy = v;
 		param_get(_params_handles.xy_vel_cruise, &v);
 		_params.vel_cruise_xy = v;
-		param_get(_params_handles.xy_ff, &v);
-		v = math::constrain(v, 0.0f, 1.0f);
-		_params.vel_ff(0) = v;
-		_params.vel_ff(1) = v;
-		param_get(_params_handles.z_ff, &v);
-		v = math::constrain(v, 0.0f, 1.0f);
-		_params.vel_ff(2) = v;
 		param_get(_params_handles.hold_max_xy, &v);
 		_params.hold_max_xy = math::max(0.f, v);
 		param_get(_params_handles.hold_max_z, &v);
@@ -859,8 +845,7 @@ MulticopterPositionControl::limit_altitude()
 		float delta_t = -_vel(2) / _params.acc_down_max;
 
 		/* predicted position */
-		float pos_z_next = _pos(2) + _vel(2) * delta_t + 0.5f *
+		float pos_z_next = _pos(2) + _vel(2) * delta_t + 0.5f * _params.acc_down_max * delta_t *delta_t;
-				   _params.acc_down_max * delta_t *delta_t;
 
 		if (pos_z_next <= -_vehicle_land_detected.alt_max) {
 			_pos_sp(2) = -_vehicle_land_detected.alt_max;
@@ -981,14 +966,14 @@ MulticopterPositionControl::control_manual(float dt)
 	}
 
 	/* prepare yaw to rotate into NED frame */
-	float yaw_input_fame = _control_mode.flag_control_fixed_hdg_enabled ? _yaw_takeoff : _att_sp.yaw_body;
+	float yaw_input_frame = _control_mode.flag_control_fixed_hdg_enabled ? _yaw_takeoff : _att_sp.yaw_body;
 
 	/* prepare cruise speed (m/s) vector to scale the velocity setpoint */
 	float vel_mag = (_velocity_hor_manual.get() < _vel_max_xy) ? _velocity_hor_manual.get() : _vel_max_xy;
 	matrix::Vector3f vel_cruise_scale(vel_mag, vel_mag, (man_vel_sp(2) > 0.0f) ? _params.vel_max_down : _params.vel_max_up);
 
 	/* setpoint in NED frame and scaled to cruise velocity */
-	man_vel_sp = matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f, yaw_input_fame)) * man_vel_sp.emult(vel_cruise_scale);
+	man_vel_sp = matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f, yaw_input_frame)) * man_vel_sp.emult(vel_cruise_scale);
 
 	/*
 	 * assisted velocity mode: user controls velocity, but if velocity is small enough, position
@@ -1021,12 +1006,11 @@ MulticopterPositionControl::control_manual(float dt)
 			/* time to travel from current velocity to zero velocity */
 			float delta_t = fabsf(_vel(2) / max_acc_z);
 
-			/* set desired position setpoint assuming max acceleraiton */
+			/* set desired position setpoint assuming max acceleration */
 			_pos_sp(2) = _pos(2) + _vel(2) * delta_t + 0.5f * max_acc_z * delta_t *delta_t;
 
 			_alt_hold_engaged = true;
 		}
-
 	}
 
 	/* check horizontal hold engaged flag */
@@ -1056,7 +1040,7 @@ MulticopterPositionControl::control_manual(float dt)
 		}
 	}
 
-	/* set requested velocity setpoitns */
+	/* set requested velocity setpoints */
 	if (!_alt_hold_engaged) {
 		_pos_sp(2) = _pos(2);
 		_run_alt_control = false; /* request velocity setpoint to be used, instead of altitude setpoint */
@@ -1397,7 +1381,7 @@ void MulticopterPositionControl::control_auto(float dt)
 			}
 		}
 
-		//only project setpoints if they are finite, else use current position
+		// only project setpoints if they are finite, else use current position
 		if (PX4_ISFINITE(_pos_sp_triplet.current.alt)) {
 			_curr_pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);
 
@@ -1424,7 +1408,6 @@ void MulticopterPositionControl::control_auto(float dt)
 		}
 	}
 
-
 	if (_pos_sp_triplet.next.valid) {
 		map_projection_project(&_ref_pos,
 				       _pos_sp_triplet.next.lat, _pos_sp_triplet.next.lon,
@@ -1588,17 +1571,14 @@ void MulticopterPositionControl::control_auto(float dt)
 			// During takeoff and landing, we better put it down again.
 			_att_sp.landing_gear = -1.0f;
 
-		} else {
 			// For the rest of the setpoint types, just leave it as is.
 		}
 
 	} else {
-
 		/* idle or triplet not valid, set velocity setpoint to zero */
 		_vel_sp.zero();
 		_run_pos_control = false;
 		_run_alt_control = false;
-
 	}
 }
 
@@ -1651,8 +1631,6 @@ MulticopterPositionControl::update_velocity_derivative()
 void
 MulticopterPositionControl::do_control(float dt)
 {
-	_vel_ff.zero();
-
 	/* by default, run position/altitude controller. the control_* functions
 	 * can disable this and run velocity controllers directly in this cycle */
 	_run_pos_control = true;
@@ -1668,7 +1646,7 @@ MulticopterPositionControl::do_control(float dt)
 		control_manual(dt);
 		_mode_auto = false;
 
-		/* we set tiplets to false
+		/* we set triplets to false
 		 * this ensures that when switching to auto, the position
 		 * controller will not use the old triplets but waits until triplets
 		 * have been updated */
@@ -1702,7 +1680,6 @@ MulticopterPositionControl::calculate_velocity_setpoint(float dt)
 {
 	/* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */
 	if (_run_pos_control) {
-
 		// If for any reason, we get a NaN position setpoint, we better just stay where we are.
 		if (PX4_ISFINITE(_pos_sp(0)) && PX4_ISFINITE(_pos_sp(1))) {
 			_vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0);
@@ -1754,6 +1731,7 @@ MulticopterPositionControl::calculate_velocity_setpoint(float dt)
 	if (_pos_sp_triplet.current.valid
 	    && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF
 	    && !_control_mode.flag_control_manual_enabled) {
+
 		_vel_sp(2) = math::max(_vel_sp(2), -_params.tko_speed);
 	}
 
@@ -1772,8 +1750,10 @@ MulticopterPositionControl::calculate_velocity_setpoint(float dt)
 	/* special velocity setpoint limitation for smooth takeoff */
 	if (_in_takeoff) {
 		_in_takeoff = _takeoff_vel_limit < -_vel_sp(2);
+
 		/* ramp vertical velocity limit up to takeoff speed */
 		_takeoff_vel_limit += _params.tko_speed * dt / _takeoff_ramp_time.get();
+
 		/* limit vertical velocity to the current ramp value */
 		_vel_sp(2) = math::max(_vel_sp(2), -_takeoff_vel_limit);
 	}
@@ -1864,8 +1844,10 @@ MulticopterPositionControl::calculate_thrust_setpoint(float dt)
 
 	float tilt_max = _params.tilt_max_air;
 	float thr_max = _params.thr_max;
+
 	/* filter vel_z over 1/8sec */
 	_vel_z_lp = _vel_z_lp * (1.0f - dt * 8.0f) + dt * 8.0f * _vel(2);
+
 	/* filter vel_z change over 1/8sec */
 	float vel_z_change = (_vel(2) - _vel_prev(2)) / dt;
 	_acc_z_lp = _acc_z_lp * (1.0f - dt * 8.0f) + dt * 8.0f * vel_z_change;
@@ -1890,8 +1872,9 @@ MulticopterPositionControl::calculate_thrust_setpoint(float dt)
 
 		/* descend stabilized, we're landing */
 		if (!_in_landing && !_lnd_reached_ground
-		    && (float)fabsf(_acc_z_lp) < 0.1f
+		    && (fabsf(_acc_z_lp) < 0.1f)
 		    && _vel_z_lp > 0.6f * _params.land_speed) {
+
 			_in_landing = true;
 		}
 
@@ -1924,9 +1907,9 @@ MulticopterPositionControl::calculate_thrust_setpoint(float dt)
 
 		/* if we suddenly fall, reset landing logic and remove thrust limit */
 		if (_lnd_reached_ground
-		    /* XXX: magic value, assuming free fall above 4m/s2 acceleration */
+		    /* XXX: magic value, assuming free fall above 4 m/s^2 acceleration */
-		    && (_acc_z_lp > 4.0f
+		    && (_acc_z_lp > 4.0f || _vel_z_lp > 2.0f * _params.land_speed)) {
-			|| _vel_z_lp > 2.0f * _params.land_speed)) {
+
 			thr_max = _params.thr_max;
 			_in_landing = true;
 			_lnd_reached_ground = false;
@@ -2303,7 +2286,6 @@ MulticopterPositionControl::task_main()
 			_reset_int_xy = true;
 			_reset_yaw_sp = true;
 			_yaw_takeoff = _yaw;
-
 		}
 
 		was_armed = _control_mode.flag_armed;

src/modules/mc_pos_control/mc_pos_control_params.c

@@ -186,18 +186,6 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_UP, 3.0f);
  */
 PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_DN, 1.0f);
 
-/**
- * Vertical velocity feed forward
- *
- * Feed forward weight for altitude control in stabilized modes (ALTCTRL, POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
- *
- * @min 0.0
- * @max 1.0
- * @decimal 2
- * @group Multicopter Position Control
- */
-PARAM_DEFINE_FLOAT(MPC_Z_FF, 0.5f);
-
 /**
  * Proportional gain for horizontal position error
  *
@@ -299,18 +287,6 @@ PARAM_DEFINE_FLOAT(MPC_TARGET_THRE, 15.0f);
  */
 PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 8.0f);
 
-/**
- * Horizontal velocity feed forward
- *
- * Feed forward weight for position control in position control mode (POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
- *
- * @min 0.0
- * @max 1.0
- * @decimal 2
- * @group Multicopter Position Control
- */
-PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f);
-
 /**
  * Maximum tilt angle in air
  *