mc_pos_control auto: replace min_cruise_speed with cruise_speed_90; take care...

mc_pos_control auto: replace min_cruise_speed with cruise_speed_90; take care of case when cruise_speed_90 is exactly in the middle of max and min

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -410,8 +410,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_hold_dz(this, "HOLD_DZ"),
 	_acceleration_hor_max(this, "ACC_HOR_MAX", true),
 	_deceleration_hor_max(this, "DEC_HOR_MAX", true),
-	_min_cruise_speed(this, "CRUISE_MIN", true),
 	_velocity_hor_manual(this, "VEL_MAN_MAX", true),
+	_cruise_speed_90(this, "CRUISE_90", true),
 	_takeoff_ramp_time(this, "TKO_RAMP_T", true),
 	_min_cruise_speed(this, "CRUISE_MIN", true),
 	_nav_rad(this, "NAV_ACC_RAD", false),
@@ -890,18 +890,33 @@ MulticopterPositionControl::get_vel_close(const matrix::Vector2f &unit_prev_to_c
 		const matrix::Vector2f &unit_current_to_next)
 {
 
+	/* minimum cruise speed when passing waypoint */
+	float min_cruise_speed = 1.0f;
+
 	/* make sure that cruise speed is larger than minimum*/
-	if ((get_cruising_speed_xy() - _min_cruise_speed.get()) < SIGMA_NORM) {
+	if ((get_cruising_speed_xy() - min_cruise_speed) < SIGMA_NORM) {
 		return get_cruising_speed_xy();
 	}
 
 	/* middle cruise speed is a number between maximum cruising speed and minimum cruising speed and corresponds to speed at angle of 90degrees
-	 * it needs to be alwawys larger than minimum cruise speed */
-	float middle_cruise_speed = 3.0f * (_min_cruise_speed.get() + SIGMA_NORM);
+	 * it needs to be always larger than minimum cruise speed */
+	float middle_cruise_speed = _cruise_speed_90.get();
+
+	if ((middle_cruise_speed - min_cruise_speed) < SIGMA_NORM) {
+		middle_cruise_speed = min_cruise_speed + SIGMA_NORM;
+	}
 
-	/* make sure that cruise speed is larger than middle cruise speed, otherwise set it to half of max/min*/
 	if ((get_cruising_speed_xy() - middle_cruise_speed) < SIGMA_NORM) {
-		middle_cruise_speed = (get_cruising_speed_xy() + _min_cruise_speed.get()) * 0.5f;
+		middle_cruise_speed = (get_cruising_speed_xy() + min_cruise_speed) * 0.5f;
+	}
+
+	/* if middle cruise speed is exactly in the middle, then compute
+	 * vel_close linearly
+	 */
+	bool use_linear_approach = false;
+
+	if (((get_cruising_speed_xy() + min_cruise_speed) * 0.5f) - middle_cruise_speed < SIGMA_NORM) {
+		use_linear_approach = true;
 	}
 
 	/* angle = cos(x) + 1.0
@@ -912,21 +927,35 @@ MulticopterPositionControl::get_vel_close(const matrix::Vector2f &unit_prev_to_c
 		angle = unit_current_to_next * (unit_prev_to_current * -1.0f) + 1.0f;
 	}
 
-	/* velocity close to target adjusted to angle
-	 * vel_close = a *b ^x + c; where at angle = 0 -> vel_close = vel_cruise; angle = 1 -> vel_close = middle_cruise_speed (this means that at 90degrees
-	 * the velocity at target is middle_cruise_speed);
-	 * angle = 2 -> vel_close = min_cruising_speed */
+	/* compute velocity target close to waypoint */
+	float vel_close;
+
+	if (use_linear_approach) {
+
+		/* velocity close to target adjusted to angle
+		 * vel_close =  m*x+q
+		 */
+		float slope = -(get_cruising_speed_xy() - min_cruise_speed) / 2.0f;
+		vel_close = slope * angle + get_cruising_speed_xy();
+
+	} else {
+
+		/* velocity close to target adjusted to angle
+		 * vel_close = a *b ^x + c; where at angle = 0 -> vel_close = vel_cruise; angle = 1 -> vel_close = middle_cruise_speed (this means that at 90degrees
+		 * the velocity at target is middle_cruise_speed);
+		 * angle = 2 -> vel_close = min_cruising_speed */
 
-	/* from maximum cruise speed, minimum cruise speed and middle cruise speed compute constants a, b and c */
-	float a = -((middle_cruise_speed -  get_cruising_speed_xy()) * (middle_cruise_speed -  get_cruising_speed_xy())) /
-		  (2.0f * middle_cruise_speed - get_cruising_speed_xy() - _min_cruise_speed.get());
-	float c =  get_cruising_speed_xy() - a;
-	float b = (middle_cruise_speed - c) / a;
-	float vel_close = a * powf(b, angle) + c;
+		/* from maximum cruise speed, minimum cruise speed and middle cruise speed compute constants a, b and c */
+		float a = -((middle_cruise_speed -  get_cruising_speed_xy()) * (middle_cruise_speed -  get_cruising_speed_xy())) /
+			  (2.0f * middle_cruise_speed - get_cruising_speed_xy() - min_cruise_speed);
+		float c =  get_cruising_speed_xy() - a;
+		float b = (middle_cruise_speed - c) / a;
+		vel_close = a * powf(b, angle) + c;
+	}
 
 	/* sanity check: vel_close needs to be in between max and min */
-	if ((vel_close - _min_cruise_speed.get()) < SIGMA_SINGLE_OP) {
-		vel_close = _min_cruise_speed.get();
+	if ((vel_close - min_cruise_speed) < SIGMA_SINGLE_OP) {
+		vel_close = min_cruise_speed;
 
 	} else if (vel_close > get_cruising_speed_xy()) {
 		vel_close = get_cruising_speed_xy();
@@ -1676,6 +1705,10 @@ void MulticopterPositionControl::control_auto(float dt)
 							target_threshold_xy = _nav_rad.get();
 						}
 
+						if ((target_threshold - _nav_rad.get()) < SIGMA_NORM) {
+							target_threshold = _nav_rad.get();
+						}
+
 						/* velocity close to current setpoint with default zero if no next setpoint is available */
 						float vel_close = 0.0f;
 						float acceptance_radius = 0.0f;
@@ -1696,28 +1729,20 @@ void MulticopterPositionControl::control_auto(float dt)
 							final_cruise_speed = slope  * (target_threshold_xy - acceptance_radius) + vel_close;
 							final_cruise_speed = (final_cruise_speed > vel_close) ? final_cruise_speed : vel_close;
 						}
-					}
 
-					/* make sure final cruise speed is larger than minimum curise speed */
-					final_cruise_speed = (_min_cruise_speed.get() < final_cruise_speed) ? final_cruise_speed : _min_cruise_speed.get();
-
-
-					/* compute the velocity along the track depending on distance close to previous setpoint */
-					if ((target_threshold - _nav_rad.get()) < SIGMA_NORM) {
-						vel_sp_along_track = _min_cruise_speed.get();
-
-					} else {
+					}
 
-						vel_sp_along_track = final_cruise_speed;
+					/* make sure final cruise speed is larger than 0*/
+					final_cruise_speed = (final_cruise_speed > SIGMA_NORM) ? final_cruise_speed : SIGMA_NORM;
+					vel_sp_along_track = final_cruise_speed;
 
-						/* we want to accelerate not too fast
-						 * TODO: change the name acceleration_hor_man to something that can
-						 * be used by auto and manual */
-						float acc_track = (final_cruise_speed - vel_sp_along_track_prev) / dt;
+					/* we want to accelerate not too fast
+					* TODO: change the name acceleration_hor_man to something that can
+					* be used by auto and manual */
+					float acc_track = (final_cruise_speed - vel_sp_along_track_prev) / dt;
 
-						if (acc_track > _acceleration_hor_manual.get()) {
-							vel_sp_along_track = _acceleration_hor_manual.get() * dt + vel_sp_along_track_prev;
-						}
+					if (acc_track > _acceleration_hor_manual.get()) {
+						vel_sp_along_track = _acceleration_hor_manual.get() * dt + vel_sp_along_track_prev;
 					}
 
 					/* enforce minimum cruise speed */
@@ -1831,7 +1856,7 @@ void MulticopterPositionControl::control_auto(float dt)
 						cruise_sp = get_cruising_speed_xy();
 					}
 
-					/* sanity check: done divide by zero */
+					/* sanity check: don't divide by zero */
 					if (vec_pos_to_closest.length() > SIGMA_NORM) {
 						pos_sp(0) = _pos(0) + vec_pos_to_closest(0) / vec_pos_to_closest.length() * cruise_sp / _params.pos_p(0);
 						pos_sp(1) = _pos(1) + vec_pos_to_closest(1) / vec_pos_to_closest.length() * cruise_sp / _params.pos_p(1);

src/modules/mc_pos_control/mc_pos_control_params.c

@@ -245,20 +245,20 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f);
 PARAM_DEFINE_FLOAT(MPC_XY_CRUISE, 5.0f);
 
 /**
- * Minimum horizontal velocity in mission
+ * Cruise speed when angle prev-current/current-next setpoint
+ * is 90 degrees. It should be lower than MPC_XY_CRUISE.
  *
- * Normal horizontal velocity in AUTO modes (includes
- * also RTL / hold / etc.) and endpoint for
- * position stabilized mode (POSCTRL).
+ * Applies only in AUTO modes (includes
+ * also RTL / hold / etc.)
  *
  * @unit m/s
- * @min 0.05
- * @max 1.0
+ * @min 1.0
+ * @max
  * @increment 1
  * @decimal 2
  * @group Multicopter Position Control
  */
-PARAM_DEFINE_FLOAT(MPC_CRUISE_MIN, 0.5f);
+PARAM_DEFINE_FLOAT(MPC_CRUISE_90, 3.0f);
 
 /**
  * Maximum horizontal velocity setpoint for manual controlled mode