TranslationControl: update comments

src/modules/mc_pos_control/TranslationControl.cpp

@@ -73,7 +73,7 @@ TranslationControl::TranslationControl()
 	_YawRateMax_h = param_find("MPC_MAN_Y_MAX");
 	_Pyaw_h = param_find("MC_YAW_P");
 
-	/* Set parameter the very first time */
+	/* Set parameter the very first time. */
 	_setParams();
 };
 
@@ -100,37 +100,42 @@ void TranslationControl::updateSetpoint(struct vehicle_local_position_setpoint_s
 void TranslationControl::generateThrustYawSetpoint(const float &dt)
 {
 	_updateParams();
-
 	_positionController();
-
 	_velocityController(dt);
-
 	_yawController(dt);
-
 }
 
 void TranslationControl::_interfaceMapping()
 {
-	/* loop through x,y and z component */
+	/* Respects FlightTask interface, where
+	 * NAN-setpoints are of no interest and
+	 * do not require control.
+	 */
+
+	/* Loop through x,y and z components of all setpoints. */
 	for (int i = 0; i <= 2; i++) {
 
 		if (PX4_ISFINITE(_pos_sp(i))) {
 
-			/* Check if velocity should be used as feedforward */
+			/* Position control is required */
+
 			if (!PX4_ISFINITE(_vel_sp(i))) {
+				/* Velocity is not used as feedforward term. */
 				_vel_sp(i) = 0.0f;
 			}
 
 		} else {
 
-			/* Position controller is not active
+			/* Velocity controller is active without
-			 * -> Set setpoint just to current position since */
+			 * position control.
+			 */
 			_pos_sp(i) = _pos(i);
 
-			/* check if velocity setpoint should be used */
 			if (!PX4_ISFINITE(_vel_sp(i)))  {
 				/* No position/velocity controller active.
 				 * Attitude will be generated from sticks directly
+				 * TODO: Adjust to the new FlightTask interface
+				 * that also sends thrust setpoints.
 				 */
 				_vel_sp(i) = _vel(i);
 			}
@@ -138,25 +143,34 @@ void TranslationControl::_interfaceMapping()
 	}
 
 	if (!PX4_ISFINITE(_yawspeed_sp)) {
+
+		/* Targe yaw is yaw setpoint. No need for yawspped */
 		_yawspeed_sp = 0.0f;
 
 		if (!PX4_ISFINITE(_yaw_sp)) {
+
+			/* There is no finite setpoint. The best
+			 * we can do is to just re-use old setpoint */
 			_yaw_sp = _yaw_sp_int;
 		}
 
 	} else if (!PX4_ISFINITE(_yaw_sp)) {
+		/* Nothing is finite: Best we can do is to just
+		 * reuse old setpoint.
+		 */
 		_yaw_sp = _yaw_sp_int;
 	}
 }
 
-/* generate desired velocity */
 void TranslationControl::_positionController()
 {
+	/* Generate desired velocity setpoint */
+
+	/* P-controller */
 	_vel_sp = (_pos_sp - _pos).emult(Pp) + _vel_sp;
 
-	/* make sure velocity setpoint is constrained in all directions (xyz) */
+	/* Make sure velocity setpoint is constrained in all directions (xyz). */
-	float vel_norm_xy = sqrtf(
+	float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) + _vel_sp(1) * _vel_sp(1));
-				    _vel_sp(0) * _vel_sp(0) + _vel_sp(1) * _vel_sp(1));
 
 	if (vel_norm_xy > _VelMaxXY) {
 		_vel_sp(0) = _vel_sp(0) * _VelMaxXY / vel_norm_xy;
@@ -166,9 +180,9 @@ void TranslationControl::_positionController()
 	_vel_sp(2) = math::constrain(_vel_sp(2), -_VelMaxZ[0], _VelMaxZ[1]);
 }
 
-/* generates desired thrust vector */
 void TranslationControl::_velocityController(const float &dt)
 {
+	/* Generate desired thrust setpoint */
 	/* PID
 	 * u_des = P(vel_err) + D(vel_err_dot) + I(vel_integral)
 	 * Umin <= u_des <= Umax
@@ -184,13 +198,15 @@ void TranslationControl::_velocityController(const float &dt)
 
 	Data vel_err = _vel_sp - _vel;
 
-	/* TODO: add offboard acceleration mode */
+	/* TODO: add offboard acceleration mode
+	 * PID-controller */
 	Data offset(0.0f, 0.0f, _ThrHover);
 	_thr_sp = Pv.emult(vel_err) + Dv.emult(_vel_dot) + _thr_int - offset;
 
 	/* Limit tilt with priority on z
 	 * For manual controlled mode excluding pure manual and rate control, maximum tilt is 90;
-	 * It is to note that pure manual and rate control will never enter _velocityController method*/
+	 * It is to note that pure manual and rate control will never enter _velocityController method.
+	 * TODO: This needs to be revisited. */
 	float tilt_max = PX4_ISFINITE(_constraints.tilt_max) ? _constraints.tilt_max : M_PI_2_F;
 	tilt_max = math::min(tilt_max, M_PI_2_F);
 	_thr_sp = ControlMath::constrainTilt(_thr_sp, tilt_max);
@@ -222,10 +238,10 @@ void TranslationControl::_velocityController(const float &dt)
 	bool stop_I[2] = {false, false}; // stop integration for xy and z
 	ControlMath::constrainPIDu(_thr_sp, stop_I, _ThrLimit, direction);
 
-	/* throttle is just thrust length */
+	/* Throttle is just thrust length. */
 	_throttle = _thr_sp.length();
 
-	/* update integrals */
+	/* Update integrals */
 	if (!stop_I[0]) {
 		_thr_int(0) += vel_err(0) * Iv(0) * dt;
 		_thr_int(1) += vel_err(1) * Iv(1) * dt;
@@ -251,7 +267,7 @@ void TranslationControl::_yawController(const float &dt)
 		_yaw_sp = yaw_target;
 	}
 
-	/* update yaw setpoint integral */
+	/* Update yaw setpoint integral */
 	_yaw_sp_int = _yaw_sp;
 
 }

src/modules/mc_pos_control/TranslationControl.hpp

@@ -34,7 +34,7 @@
 /**
  * @file TranslationControl.hpp
  *
- * @inputs: position-, velocity-, acceleration-, thrust- setpoints
+ * @inputs: position-, velocity-, acceleration-, thrust-setpoints
  * @outputs: thrust vector
  *
  */
@@ -47,7 +47,7 @@
 
 #pragma once
 
-/* Variable constraints based on mode:
+/* Constraints based on mode:
  * Eventually this structure should be part of local position message
  */
 namespace Controller
@@ -79,7 +79,7 @@ public:
 
 private:
 
-	/* states */
+	/* States */
 	matrix::Vector3f _pos{};
 	matrix::Vector3f _vel{};
 	matrix::Vector3f _vel_dot{};
@@ -88,7 +88,7 @@ private:
 	float _yaw{0.0f};
 	matrix::Matrix<float, 3, 3> _R{};
 
-	/* setpoints */
+	/* Setpoints */
 	matrix::Vector3f _pos_sp{};
 	matrix::Vector3f _vel_sp{};
 	matrix::Vector3f _acc_sp{};
@@ -97,12 +97,12 @@ private:
 	float _yawspeed_sp{};
 	float _throttle{};
 
-	/* other variables */
+	/* Other variables */
 	matrix::Vector3f _thr_int{};
 	float _yaw_sp_int{};
 	Controller::Constraints _constraints{};
 
-	/* params handles */
+	/* Parameter handles */
 	int _parameter_sub{-1};
 	param_t _Pz_h{PARAM_INVALID};
 	param_t _Pvz_h{PARAM_INVALID};
@@ -121,7 +121,7 @@ private:
 	param_t _YawRateMax_h{PARAM_INVALID};
 	param_t _Pyaw_h{PARAM_INVALID}; //only temporary: this will be moved into attitude controller
 
-	/* params */
+	/* Parameters */
 	matrix::Vector3f Pp, Pv, Iv, Dv = matrix::Vector3f{0.0f, 0.0f, 0.0f};
 	float _VelMaxXY{};
 	float _VelMaxZ[2]; //index 0: index up; 1: down
@@ -130,7 +130,7 @@ private:
 	float _Pyaw{};
 	float _YawRateMax{};
 
-	/* helper methods */
+	/* Helper methods */
 	void _interfaceMapping();
 	void _positionController();
 	void _velocityController(const float &dt);