mc_pos_control: refactoring only in manual velocity setpoint generation

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -922,74 +922,69 @@ MulticopterPositionControl::control_manual(float dt)
 
 
 	/*
-	* Map from stick input to velocity setpoint
-	* xy in local frame, z in global frame
-	*/
+	 * Map from stick input to velocity setpoint
+	 */
 
-	/*  setpoint in normalized range [-1,1]  */
-	math::Vector<2> req_vel_sp_xy;
-	req_vel_sp_xy.zero();
-	float req_vel_sp_z = 0.0f;
+	/* velocity setpoint commanded by user stick input */
+	matrix::Vector3f man_vel_sp;
 
 	if (_control_mode.flag_control_altitude_enabled) {
 		/* set vertical velocity setpoint with throttle stick */
-		req_vel_sp_z = -math::expo_deadzone((_manual.z - 0.5f) * 2.f, _z_vel_man_expo.get(), _params.hold_dz);
+		man_vel_sp(2) = -math::expo_deadzone((_manual.z - 0.5f) * 2.f, _z_vel_man_expo.get(), _params.hold_dz);
 
 		/* reset alt setpoint to current altitude if needed */
 		reset_alt_sp();
 	}
 
 	if (_control_mode.flag_control_position_enabled) {
 		/* set horizontal velocity setpoint with roll/pitch stick */
-		req_vel_sp_xy(0) = math::expo_deadzone(_manual.x, _params.xy_vel_man_expo, _params.hold_dz);
-		req_vel_sp_xy(1) = math::expo_deadzone(_manual.y, _params.xy_vel_man_expo, _params.hold_dz);
+		man_vel_sp(0) = math::expo_deadzone(_manual.x, _params.xy_vel_man_expo, _params.hold_dz);
+		man_vel_sp(1) = math::expo_deadzone(_manual.y, _params.xy_vel_man_expo, _params.hold_dz);
+
+		const float man_vel_hor_length = ((matrix::Vector2f)man_vel_sp.slice<2, 1>(0, 0)).length();
 
-		/* saturarate such that magnitude is never larger than 1 */
-		if (req_vel_sp_xy.length() > 1.0f) {
-			req_vel_sp_xy = req_vel_sp_xy.normalized();
+		/* saturate such that magnitude is never larger than 1 */
+		if (man_vel_hor_length > 1.0f) {
+			man_vel_sp(0) /= man_vel_hor_length;
+			man_vel_sp(1) /= man_vel_hor_length;
 		}
 
 		/* reset position setpoint to current position if needed */
 		reset_pos_sp();
 	}
 
-	/* scale requested velocity setpoint to cruisespeed and rotate around yaw */
-	math::Vector<3> cruising_scale(_params.vel_cruise_xy,
-				       _params.vel_cruise_xy,
-				       (req_vel_sp_z > 0.0f) ? _params.vel_max_down : _params.vel_max_up);
-	math::Vector<3> req_vel_sp_scaled(req_vel_sp_xy(0), req_vel_sp_xy(1), req_vel_sp_z);
+	/* prepare yaw to rotate into NED frame */
+	float yaw_input_fame = _control_mode.flag_control_fixed_hdg_enabled ? _yaw_takeoff : _local_pos.yaw;
 
-	/* scale velocity setpoint to cruise speed (m/s) and rotate around yaw to NED frame */
-	math::Matrix<3, 3> R_input_fame;
+	/* prepare cruise speed (m/s) vector to scale the velocity setpoint */
+	matrix::Vector3f vel_cruise_scale(_params.vel_cruise_xy,
+					  _params.vel_cruise_xy,
+					  (man_vel_sp(2) > 0.0f) ? _params.vel_max_down : _params.vel_max_up);
 
-	if (_control_mode.flag_control_fixed_hdg_enabled) {
-		R_input_fame.from_euler(0.0f, 0.0f, _yaw_takeoff);
+	/* 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);
 
-	} else {
-		R_input_fame.from_euler(0.0f, 0.0f, _local_pos.yaw);
-
-	}
-
-	req_vel_sp_scaled = R_input_fame * req_vel_sp_scaled.emult(
-				    cruising_scale); // in NED and scaled to actual velocity;
 
 	/*
-	 * assisted velocity mode: user controls velocity, but if	velocity is small enough, position
+	 * assisted velocity mode: user controls velocity, but if velocity is small enough, position
 	 * hold is activated for the corresponding axis
 	 */
 
+	/* want to get/stay in altitude hold if user has z stick in the middle (accounted for deadzone already) */
+	const bool alt_hold_desired = _control_mode.flag_control_altitude_enabled && fabsf(man_vel_sp(2)) < FLT_EPSILON;
+
+	/* want to get/stay in position hold if user has xy stick in the middle (accounted for deadzone already) */
+	const bool pos_hold_desired = _control_mode.flag_control_position_enabled &&
+				      fabsf(man_vel_sp(0)) < FLT_EPSILON && fabsf(man_vel_sp(1)) < FLT_EPSILON;
 
 	/* check vertical hold engaged flag */
 	if (_alt_hold_engaged) {
-
-		/* only switch back to velocity control if user moves stick */
-		_alt_hold_engaged = _control_mode.flag_control_altitude_enabled && (fabsf(req_vel_sp_z) < FLT_EPSILON);
+		_alt_hold_engaged = alt_hold_desired;
 
 	} else {
 
 		/* check if we switch to alt_hold_engaged */
-		bool smooth_alt_transition = _control_mode.flag_control_altitude_enabled &&
-					     fabsf(req_vel_sp_z) < FLT_EPSILON &&
+		bool smooth_alt_transition = alt_hold_desired &&
 					     (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z);
 
 		/* during transition predict setpoint forward */
@@ -1011,17 +1006,13 @@ MulticopterPositionControl::control_manual(float dt)
 
 	/* check horizontal hold engaged flag */
 	if (_pos_hold_engaged) {
-
-		/* only switch back to velocity control if user moves stick */
-		_pos_hold_engaged = _control_mode.flag_control_position_enabled && (fabsf(req_vel_sp_xy(0)) < FLT_EPSILON)
-				    && (fabsf(req_vel_sp_xy(1)) < FLT_EPSILON);
+		_pos_hold_engaged = pos_hold_desired;
 
 	} else {
 
 		/* check if we switch to pos_hold_engaged */
 		float vel_xy_mag = sqrtf(_vel(0) * _vel(0) + _vel(1) * _vel(1));
-		bool smooth_pos_transition = _control_mode.flag_control_position_enabled &&
-					     (fabsf(req_vel_sp_xy(0)) < FLT_EPSILON && fabsf(req_vel_sp_xy(1)) < FLT_EPSILON) &&
+		bool smooth_pos_transition = pos_hold_desired &&
 					     (_params.hold_max_xy < FLT_EPSILON || vel_xy_mag < _params.hold_max_xy);
 
 		/* during transition predict setpoint forward */
@@ -1046,15 +1037,15 @@ MulticopterPositionControl::control_manual(float dt)
 	if (!_alt_hold_engaged) {
 		_pos_sp(2) = _pos(2);
 		_run_alt_control = false; /* request velocity setpoint to be used, instead of altitude setpoint */
-		_vel_sp(2) = req_vel_sp_scaled(2);
+		_vel_sp(2) = man_vel_sp(2);
 	}
 
 	if (!_pos_hold_engaged) {
 		_pos_sp(0) = _pos(0);
 		_pos_sp(1) = _pos(1);
 		_run_pos_control = false; /* request velocity setpoint to be used, instead of position setpoint */
-		_vel_sp(0) = req_vel_sp_scaled(0);
-		_vel_sp(1) = req_vel_sp_scaled(1);
+		_vel_sp(0) = man_vel_sp(0);
+		_vel_sp(1) = man_vel_sp(1);
 	}
 
 	if (_vehicle_land_detected.landed) {