PositionControl: clear setter interface naming, order, description

src/modules/mc_pos_control/PositionControl/PositionControl.cpp

@@ -63,7 +63,7 @@ void PositionControl::setThrustLimits(const float min, const float max)
 	_lim_thr_max = max;
 }
 
-void PositionControl::updateState(const PositionControlStates &states)
+void PositionControl::setState(const PositionControlStates &states)
 {
 	_pos = states.position;
 	_vel = states.velocity;
@@ -78,7 +78,7 @@ void PositionControl::_setCtrlFlag(bool value)
 	}
 }
 
-bool PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &setpoint)
+bool PositionControl::setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint)
 {
 	// by default we use the entire position-velocity control-loop pipeline (flag only for logging purpose)
 	_setCtrlFlag(true);
@@ -99,7 +99,32 @@ bool PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &se
 	return mapping_succeeded;
 }
 
-void PositionControl::generateThrustYawSetpoint(const float dt)
+void PositionControl::setConstraints(const vehicle_constraints_s &constraints)
+{
+	_constraints = constraints;
+
+	// For safety check if adjustable constraints are below global constraints. If they are not stricter than global
+	// constraints, then just use global constraints for the limits.
+
+	if (!PX4_ISFINITE(constraints.tilt)
+	    || !(constraints.tilt < _lim_tilt)) {
+		_constraints.tilt = _lim_tilt;
+	}
+
+	if (!PX4_ISFINITE(constraints.speed_up) || !(constraints.speed_up < _lim_vel_up)) {
+		_constraints.speed_up = _lim_vel_up;
+	}
+
+	if (!PX4_ISFINITE(constraints.speed_down) || !(constraints.speed_down < _lim_vel_down)) {
+		_constraints.speed_down = _lim_vel_down;
+	}
+
+	if (!PX4_ISFINITE(constraints.speed_xy) || !(constraints.speed_xy < _lim_vel_horizontal)) {
+		_constraints.speed_xy = _lim_vel_horizontal;
+	}
+}
+
+void PositionControl::update(const float dt)
 {
 	if (_skip_controller) {
 
@@ -120,8 +145,8 @@ void PositionControl::generateThrustYawSetpoint(const float dt)
 		_acc_sp = _vel_dot;
 
 	} else {
-		_positionController();
-		_velocityController(dt);
+		_positionControl();
+		_velocityControl(dt);
 	}
 }
 
@@ -178,7 +203,7 @@ bool PositionControl::_interfaceMapping()
 			_ctrl_pos[i] = _ctrl_vel[i] = false; // position/velocity control loop is not used
 
 			// Reset the Integral term.
-			_thr_int(i) = 0.0f;
+			_vel_int(i) = 0.0f;
 			// Don't require velocity derivative.
 			_vel_dot(i) = 0.0f;
 
@@ -228,7 +253,7 @@ bool PositionControl::_interfaceMapping()
 	return !(failsafe);
 }
 
-void PositionControl::_positionController()
+void PositionControl::_positionControl()
 {
 	// P-position controller
 	const Vector3f vel_sp_position = (_pos_sp - _pos).emult(_gain_pos_p);
@@ -244,7 +269,7 @@ void PositionControl::_positionController()
 	_vel_sp(2) = math::constrain(_vel_sp(2), -_constraints.speed_up, _constraints.speed_down);
 }
 
-void PositionControl::_velocityController(const float &dt)
+void PositionControl::_velocityControl(const float dt)
 {
 	// Generate desired thrust setpoint.
 	// PID
@@ -273,7 +298,7 @@ void PositionControl::_velocityController(const float &dt)
 	const Vector3f vel_err = _vel_sp - _vel;
 
 	// Consider thrust in D-direction.
-	float thrust_desired_D = _gain_vel_p(2) * vel_err(2) +  _gain_vel_d(2) * _vel_dot(2) + _thr_int(
+	float thrust_desired_D = _gain_vel_p(2) * vel_err(2) +  _gain_vel_d(2) * _vel_dot(2) + _vel_int(
 					 2) - _hover_thrust;
 
 	// The Thrust limits are negated and swapped due to NED-frame.
@@ -288,10 +313,10 @@ void PositionControl::_velocityController(const float &dt)
 			       (thrust_desired_D <= uMin && vel_err(2) <= 0.0f);
 
 	if (!stop_integral_D) {
-		_thr_int(2) += vel_err(2) * _gain_vel_i(2) * dt;
+		_vel_int(2) += vel_err(2) * _gain_vel_i(2) * dt;
 
 		// limit thrust integral
-		_thr_int(2) = math::min(fabsf(_thr_int(2)), _lim_thr_max) * math::sign(_thr_int(2));
+		_vel_int(2) = math::min(fabsf(_vel_int(2)), _lim_thr_max) * math::sign(_vel_int(2));
 	}
 
 	// Saturate thrust setpoint in D-direction.
@@ -307,8 +332,8 @@ void PositionControl::_velocityController(const float &dt)
 	} else {
 		// PID-velocity controller for NE-direction.
 		Vector2f thrust_desired_NE;
-		thrust_desired_NE(0) = _gain_vel_p(0) * vel_err(0) + _gain_vel_d(0) * _vel_dot(0) + _thr_int(0);
-		thrust_desired_NE(1) = _gain_vel_p(1) * vel_err(1) + _gain_vel_d(1) * _vel_dot(1) + _thr_int(1);
+		thrust_desired_NE(0) = _gain_vel_p(0) * vel_err(0) + _gain_vel_d(0) * _vel_dot(0) + _vel_int(0);
+		thrust_desired_NE(1) = _gain_vel_p(1) * vel_err(1) + _gain_vel_d(1) * _vel_dot(1) + _vel_int(1);
 
 		// Get maximum allowed thrust in NE based on tilt and excess thrust.
 		float thrust_max_NE_tilt = fabsf(_thr_sp(2)) * tanf(_constraints.tilt);
@@ -334,35 +359,11 @@ void PositionControl::_velocityController(const float &dt)
 		vel_err_lim(1) = vel_err(1) - (thrust_desired_NE(1) - _thr_sp(1)) * arw_gain;
 
 		// Update integral
-		_thr_int(0) += _gain_vel_i(0) * vel_err_lim(0) * dt;
-		_thr_int(1) += _gain_vel_i(1) * vel_err_lim(1) * dt;
+		_vel_int(0) += _gain_vel_i(0) * vel_err_lim(0) * dt;
+		_vel_int(1) += _gain_vel_i(1) * vel_err_lim(1) * dt;
 	}
 }
 
-void PositionControl::updateConstraints(const vehicle_constraints_s &constraints)
-{
-	_constraints = constraints;
-
-	// For safety check if adjustable constraints are below global constraints. If they are not stricter than global
-	// constraints, then just use global constraints for the limits.
-
-	if (!PX4_ISFINITE(constraints.tilt)
-	    || !(constraints.tilt < _lim_tilt)) {
-		_constraints.tilt = _lim_tilt;
-	}
-
-	if (!PX4_ISFINITE(constraints.speed_up) || !(constraints.speed_up < _lim_vel_up)) {
-		_constraints.speed_up = _lim_vel_up;
-	}
-
-	if (!PX4_ISFINITE(constraints.speed_down) || !(constraints.speed_down < _lim_vel_down)) {
-		_constraints.speed_down = _lim_vel_down;
-	}
-
-	if (!PX4_ISFINITE(constraints.speed_xy) || !(constraints.speed_xy < _lim_vel_horizontal)) {
-		_constraints.speed_xy = _lim_vel_horizontal;
-	}
-}
 
 void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s &local_position_setpoint) const
 {

src/modules/mc_pos_control/PositionControl/PositionControl.hpp

@@ -121,23 +121,25 @@ public:
 	void setHoverThrust(const float thrust) { _hover_thrust = thrust; }
 
 	/**
-	 * Update the current vehicle state.
+	 * Pass the current vehicle state to the controller
 	 * @param PositionControlStates structure
 	 */
-	void updateState(const PositionControlStates &states);
+	void setState(const PositionControlStates &states);
 
 	/**
-	 * Update the desired setpoints.
+	 * Pass the desired setpoints
+	 * Note: NAN value means no feed forward/leave state uncontrolled if there's no higher order setpoint.
 	 * @param setpoint a vehicle_local_position_setpoint_s structure
-	 * @return true if setpoint has updated correctly
+	 * @return true if a valid setpoint was set
 	 */
-	bool updateSetpoint(const vehicle_local_position_setpoint_s &setpoint);
+	bool setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint);
 
 	/**
-	 * Set constraints that are stricter than the global limits.
+	 * Pass constraints that are stricter than the global limits
+	 * Note: NAN value means no constraint, take maximum limit of controller.
 	 * @param constraints a PositionControl structure with supported constraints
 	 */
-	void updateConstraints(const vehicle_constraints_s &constraints);
+	void setConstraints(const vehicle_constraints_s &constraints);
 
 	/**
 	 * Apply P-position and PID-velocity controller that updates the member
@@ -145,21 +147,16 @@ public:
 	 * @see _thr_sp
 	 * @see _yaw_sp
 	 * @see _yawspeed_sp
-	 * @param dt the delta-time
+	 * @param dt time in seconds since last iteration
+	 * @return true if output setpoint is executable, false if not
 	 */
-	void generateThrustYawSetpoint(const float dt);
+	void update(const float dt);
 
 	/**
-	 * 	Set the integral term in xy to 0.
-	 * 	@see _thr_int
+	 * Set the integral term in xy to 0.
+	 * @see _vel_int
 	 */
-	void resetIntegralXY() { _thr_int(0) = _thr_int(1) = 0.f; }
-
-	/**
-	 * 	Set the integral term in z to 0.
-	 * 	@see _thr_int
-	 */
-	void resetIntegralZ() { _thr_int(2) = 0.f; }
+	void resetIntegral() { _vel_int.setZero(); }
 
 	/**
 	 * 	Get the
@@ -205,8 +202,8 @@ private:
 	 */
 	bool _interfaceMapping();
 
-	void _positionController(); /** applies the P-position-controller */
-	void _velocityController(const float &dt); /** applies the PID-velocity-controller */
+	void _positionControl(); ///< Position proportional control
+	void _velocityControl(const float dt); ///< Velocity PID control
 	void _setCtrlFlag(bool value); /**< set control-loop flags (only required for logging) */
 
 	// Gains
@@ -229,7 +226,7 @@ private:
 	matrix::Vector3f _pos; /**< current position */
 	matrix::Vector3f _vel; /**< current velocity */
 	matrix::Vector3f _vel_dot; /**< velocity derivative (replacement for acceleration estimate) */
-	matrix::Vector3f _thr_int; /**< integral term of the velocity controller */
+	matrix::Vector3f _vel_int; /**< integral term of the velocity controller */
 	float _yaw{}; /**< current heading */
 
 	vehicle_constraints_s _constraints{}; /**< variable constraints */

src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp

@@ -102,9 +102,9 @@ public:
 
 	void runController()
 	{
-		_position_control.updateConstraints(_contraints);
-		_position_control.updateSetpoint(_input_setpoint);
-		_position_control.generateThrustYawSetpoint(.1f);
+		_position_control.setConstraints(_contraints);
+		_position_control.setInputSetpoint(_input_setpoint);
+		_position_control.update(.1f);
 		_position_control.getLocalPositionSetpoint(_output_setpoint);
 		_position_control.getAttitudeSetpoint(_attitude);
 	}

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -609,8 +609,7 @@ MulticopterPositionControl::Run()
 				setpoint.yaw = _states.yaw;
 				setpoint.yawspeed = 0.f;
 				// prevent any integrator windup
-				_control.resetIntegralXY();
-				_control.resetIntegralZ();
+				_control.resetIntegral();
 				// reactivate the task which will reset the setpoint to current state
 				_flight_tasks.reActivate();
 			}
@@ -624,20 +623,18 @@ MulticopterPositionControl::Run()
 				limit_altitude(setpoint);
 			}
 
-			// Update states, setpoints and constraints.
-			_control.updateConstraints(constraints);
-			_control.updateState(_states);
+			// Run position control
+			_control.setState(_states);
+			_control.setConstraints(constraints);
 
-			// update position controller setpoints
-			if (!_control.updateSetpoint(setpoint)) {
-				warn_rate_limited("Position-Control Setpoint-Update failed");
+			if (!_control.setInputSetpoint(setpoint)) {
+				warn_rate_limited("PositionControl: invalid setpoints");
 				failsafe(setpoint, _states, true, !was_in_failsafe);
-				_control.updateSetpoint(setpoint);
+				_control.setInputSetpoint(setpoint);
 				constraints = FlightTask::empty_constraints;
 			}
 
-			// Generate desired thrust and yaw.
-			_control.generateThrustYawSetpoint(_dt);
+			_control.update(_dt);
 
 			// Fill local position, velocity and thrust setpoint.
 			// This message contains setpoints where each type of setpoint is either the input to the PositionController
@@ -660,8 +657,7 @@ MulticopterPositionControl::Run()
 			local_pos_sp.vz = PX4_ISFINITE(_control.getVelSp()(2)) ? _control.getVelSp()(2) : setpoint.vz;
 
 			// Publish local position setpoint
-			// This message will be used by other modules (such as Landdetector) to determine
-			// vehicle intention.
+			// This message will be used by other modules (such as Landdetector) to determine vehicle intention.
 			_local_pos_sp_pub.publish(local_pos_sp);
 
 			// Inform FlightTask about the input and output of the velocity controller
@@ -691,10 +687,8 @@ MulticopterPositionControl::Run()
 			// if there's any change in landing gear setpoint publish it
 			if (gear.landing_gear != _old_landing_gear_position
 			    && gear.landing_gear != landing_gear_s::GEAR_KEEP) {
-
-				_landing_gear.landing_gear = gear.landing_gear;
 				_landing_gear.timestamp = time_stamp_now;
-
+				_landing_gear.landing_gear = gear.landing_gear;
 				_landing_gear_pub.publish(_landing_gear);
 			}
 
@@ -940,8 +934,7 @@ MulticopterPositionControl::limit_thrust_during_landing(vehicle_attitude_setpoin
 		Quatf(AxisAngle<float>(0, 0, _states.yaw)).copyTo(setpoint.q_d);
 		setpoint.yaw_sp_move_rate = 0.f;
 		// prevent any position control integrator windup
-		_control.resetIntegralXY();
-		_control.resetIntegralZ();
+		_control.resetIntegral();
 	}
 }