mc_pos_control: further reshuffling

Should not have any functional change

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -338,6 +338,8 @@ private:
 	 */
 	void		control_manual(float dt);
 
+	void control_non_manual(float dt);
+
 	/**
 	 * Set position setpoint using offboard control
 	 */
@@ -348,6 +350,8 @@ private:
 	 */
 	void		control_auto(float dt);
 
+	void control_position(float dt);
+
 	/**
 	 * Select between barometric and global (AMSL) altitudes
 	 */
@@ -355,7 +359,7 @@ private:
 
 	void update_velocity_derivative();
 
-	void do_position_control(float dt);
+	void do_control(float dt);
 
 	void generate_attitude_setpoint(float dt);
 
@@ -1012,6 +1016,89 @@ MulticopterPositionControl::control_manual(float dt)
 			_vel_sp(2) = req_vel_sp_scaled(2);
 		}
 	}
+
+	if (_vehicle_land_detected.landed) {
+		/* don't run controller when landed */
+		_reset_pos_sp = true;
+		_reset_alt_sp = true;
+		_mode_auto = false;
+		_reset_int_z = true;
+		_reset_int_xy = true;
+
+		_R_setpoint.identity();
+
+		_att_sp.roll_body = 0.0f;
+		_att_sp.pitch_body = 0.0f;
+		_att_sp.yaw_body = _yaw;
+		_att_sp.thrust = 0.0f;
+
+		_att_sp.timestamp = hrt_absolute_time();
+
+		/* publish attitude setpoint */
+		if (_att_sp_pub != nullptr) {
+			orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);
+
+		} else if (_attitude_setpoint_id) {
+			_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
+		}
+
+	} else {
+		control_position(dt);
+	}
+
+}
+
+void
+MulticopterPositionControl::control_non_manual(float dt)
+{
+	/* select control source */
+	if (_control_mode.flag_control_offboard_enabled) {
+		/* offboard control */
+		control_offboard(dt);
+		_mode_auto = false;
+
+	} else {
+		/* AUTO */
+		control_auto(dt);
+	}
+
+	/* weather-vane mode for vtol: disable yaw control */
+	if (_pos_sp_triplet.current.disable_mc_yaw_control == true) {
+		_att_sp.disable_mc_yaw_control = true;
+
+	} else {
+		/* reset in case of setpoint updates */
+		_att_sp.disable_mc_yaw_control = false;
+	}
+
+	if (_pos_sp_triplet.current.valid
+	    && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
+		/* idle state, don't run controller and set zero thrust */
+		_R_setpoint.identity();
+
+
+		matrix::Quatf qd = _R_setpoint;
+		memcpy(&_att_sp.q_d[0], qd.data(), sizeof(_att_sp.q_d));
+		_att_sp.q_d_valid = true;
+
+		_att_sp.roll_body = 0.0f;
+		_att_sp.pitch_body = 0.0f;
+		_att_sp.yaw_body = _yaw;
+		_att_sp.thrust = 0.0f;
+
+		_att_sp.timestamp = hrt_absolute_time();
+
+		/* publish attitude setpoint */
+		if (_att_sp_pub != nullptr) {
+			orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);
+
+		} else if (_attitude_setpoint_id) {
+			_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
+		}
+
+	} else {
+		control_position(dt);
+	}
 }
 
 void
@@ -1362,7 +1449,7 @@ MulticopterPositionControl::update_velocity_derivative()
 }
 
 void
-MulticopterPositionControl::do_position_control(float dt)
+MulticopterPositionControl::do_control(float dt)
 {
 
 	_vel_ff.zero();
@@ -1372,84 +1459,19 @@ MulticopterPositionControl::do_position_control(float dt)
 	_run_pos_control = true;
 	_run_alt_control = true;
 
-	/* select control source */
 	if (_control_mode.flag_control_manual_enabled) {
 		/* manual control */
 		control_manual(dt);
 		_mode_auto = false;
 
-	} else if (_control_mode.flag_control_offboard_enabled) {
-		/* offboard control */
-		control_offboard(dt);
-		_mode_auto = false;
-
 	} else {
-		/* AUTO */
+		control_non_manual(dt);
-		control_auto(dt);
-	}
-
-	/* weather-vane mode for vtol: disable yaw control */
-	if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.disable_mc_yaw_control == true) {
-		_att_sp.disable_mc_yaw_control = true;
-
-	} else {
-		/* reset in case of setpoint updates */
-		_att_sp.disable_mc_yaw_control = false;
-	}
-
-	if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid
-	    && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
-		/* idle state, don't run controller and set zero thrust */
-		_R_setpoint.identity();
-
-
-		matrix::Quatf qd = _R_setpoint;
-		memcpy(&_att_sp.q_d[0], qd.data(), sizeof(_att_sp.q_d));
-		_att_sp.q_d_valid = true;
-
-		_att_sp.roll_body = 0.0f;
-		_att_sp.pitch_body = 0.0f;
-		_att_sp.yaw_body = _yaw;
-		_att_sp.thrust = 0.0f;
-
-		_att_sp.timestamp = hrt_absolute_time();
-
-		/* publish attitude setpoint */
-		if (_att_sp_pub != nullptr) {
-			orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);
-
-		} else if (_attitude_setpoint_id) {
-			_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
-		}
-
-	} else if (_control_mode.flag_control_manual_enabled
-		   && _vehicle_land_detected.landed) {
-		/* don't run controller when landed */
-		_reset_pos_sp = true;
-		_reset_alt_sp = true;
-		_do_reset_alt_pos_flag = true;
-		_mode_auto = false;
-		_reset_int_z = true;
-		_reset_int_xy = true;
-
-		_R_setpoint.identity();
-
-		_att_sp.roll_body = 0.0f;
-		_att_sp.pitch_body = 0.0f;
-		_att_sp.yaw_body = _yaw;
-		_att_sp.thrust = 0.0f;
-
-		_att_sp.timestamp = hrt_absolute_time();
-
-		/* publish attitude setpoint */
-		if (_att_sp_pub != nullptr) {
-			orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);
-
-		} else if (_attitude_setpoint_id) {
-			_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
 	}
+}
 
-	} else {
+void
+MulticopterPositionControl::control_position(float dt)
+{
 	/* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */
 	if (_run_pos_control) {
 		_vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0);
@@ -1588,7 +1610,7 @@ MulticopterPositionControl::do_position_control(float dt)
 	acc_hor(0) = (_vel_sp(0) - _vel_sp_prev(0)) / dt;
 	acc_hor(1) = (_vel_sp(1) - _vel_sp_prev(1)) / dt;
 
-		if ((acc_hor.length() > _params.acc_hor_max) & !_reset_pos_sp) {
+	if (acc_hor.length() > _params.acc_hor_max) {
 		acc_hor.normalize();
 		acc_hor *= _params.acc_hor_max;
 		math::Vector<2> vel_sp_hor_prev(_vel_sp_prev(0), _vel_sp_prev(1));
@@ -1600,7 +1622,7 @@ MulticopterPositionControl::do_position_control(float dt)
 	// limit vertical acceleration
 	float acc_v = (_vel_sp(2) - _vel_sp_prev(2)) / dt;
 
-		if ((fabsf(acc_v) > 2 * _params.acc_hor_max) & !_reset_alt_sp) {
+	if (fabsf(acc_v) > 2 * _params.acc_hor_max) {
 		acc_v /= fabsf(acc_v);
 		_vel_sp(2) = acc_v * 2 * _params.acc_hor_max * dt + _vel_sp_prev(2);
 	}
@@ -1804,7 +1826,7 @@ MulticopterPositionControl::do_position_control(float dt)
 		}
 
 		if (_control_mode.flag_control_climb_rate_enabled && !_control_mode.flag_control_velocity_enabled) {
-				/* thrust compensation for altitude only control modes */
+			/* thrust compensation when vertical velocity but not horizontal velocity is controlled */
 			float att_comp;
 
 			if (_R(2, 2) > TILT_COS_MAX) {
@@ -1956,25 +1978,6 @@ MulticopterPositionControl::do_position_control(float dt)
 	} else {
 		_reset_int_z = true;
 	}
-	}
-
-	/* fill local position, velocity and thrust setpoint */
-	_local_pos_sp.timestamp = hrt_absolute_time();
-	_local_pos_sp.x = _pos_sp(0);
-	_local_pos_sp.y = _pos_sp(1);
-	_local_pos_sp.z = _pos_sp(2);
-	_local_pos_sp.yaw = _att_sp.yaw_body;
-	_local_pos_sp.vx = _vel_sp(0);
-	_local_pos_sp.vy = _vel_sp(1);
-	_local_pos_sp.vz = _vel_sp(2);
-
-	/* publish local position setpoint */
-	if (_local_pos_sp_pub != nullptr) {
-		orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp);
-
-	} else {
-		_local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp);
-	}
 }
 
 void
@@ -2192,7 +2195,25 @@ MulticopterPositionControl::task_main()
 		    _control_mode.flag_control_velocity_enabled ||
 		    _control_mode.flag_control_acceleration_enabled) {
 
-			do_position_control(dt);
+			do_control(dt);
+
+			/* fill local position, velocity and thrust setpoint */
+			_local_pos_sp.timestamp = hrt_absolute_time();
+			_local_pos_sp.x = _pos_sp(0);
+			_local_pos_sp.y = _pos_sp(1);
+			_local_pos_sp.z = _pos_sp(2);
+			_local_pos_sp.yaw = _att_sp.yaw_body;
+			_local_pos_sp.vx = _vel_sp(0);
+			_local_pos_sp.vy = _vel_sp(1);
+			_local_pos_sp.vz = _vel_sp(2);
+
+			/* publish local position setpoint */
+			if (_local_pos_sp_pub != nullptr) {
+				orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp);
+
+			} else {
+				_local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp);
+			}
 
 		} else {
 			/* position controller disabled, reset setpoints */