<commit_before>/****************************************************************************
 *
 *   Copyright (c) 2017 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file TranslationControl.cpp
 *
 */

#include "TranslationControl.hpp"
#include <float.h>
#include <mathlib/mathlib.h>
#include "uORB/topics/parameter_update.h"
#include "Utility/ControlMath.hpp"
#include <lib/ecl/geo/geo.h>  //TODO: only used for wrap_pi -> move this to mathlib since
// it makes more sense

using namespace matrix;

using Data = matrix::Vector3f;

TranslationControl::TranslationControl()
{
	_Pz_h   = param_find("MPC_Z_P");
	_Pvz_h  = param_find("MPC_Z_VEL_P");
	_Ivz_h  = param_find("MPC_Z_VEL_I");
	_Dvz_h  = param_find("MPC_Z_VEL_D");
	_Pxy_h  = param_find("MPC_XY_P");
	_Pvxy_h = param_find("MPC_XY_VEL_P");
	_Ivxy_h = param_find("MPC_XY_VEL_I");
	_Dvxy_h = param_find("MPC_XY_VEL_D");
	_VelMaxXY = param_find("MPC_XY_VEL_MAX");
	_VelMaxZdown_h = param_find("MPC_Z_VEL_MAX_DN");
	_VelMaxZup_h = param_find("MPC_Z_VEL_MAX_UP");
	_ThrHover_h = param_find("MPC_THR_HOVER");
	_ThrMax_h = param_find("MPC_THR_MAX");
	_ThrMin_h = param_find("MPC_THR_MIN");
	_YawRateMax_h = param_find("MPC_MAN_Y_MAX");
	_Pyaw_h = param_find("MC_YAW_P");

	/* Set parameter the very first time */
	_setParams();
};

void TranslationControl::updateState(const struct vehicle_local_position_s state, const matrix::Vector3f &vel_dot,
				     const matrix::Matrix<float, 3, 3> &R)
{
	_pos = Data(&state.x);
	_vel = Data(&state.vx);
	_yaw = state.yaw;
	_vel_dot = vel_dot;
	_R = R;
}

void TranslationControl::updateSetpoint(struct vehicle_local_position_setpoint_s setpoint)
{
	_pos_sp = Data(&setpoint.x);
	_vel_sp = Data(&setpoint.vx);
	_acc_sp = Data(&setpoint.acc_x);
	_yaw_sp = setpoint.yaw; //integrate
	_yawspeed_sp = setpoint.yawspeed;
	_interfaceMapping();
}

void TranslationControl::generateThrustYawSetpoint(const float &dt)
{
	_updateParams();

	_positionController();

	_velocityController(dt);

	_yawController(dt);

}

void TranslationControl::_interfaceMapping()
{
	/* loop through x,y and z component */
	for (int i = 0; i <= 2; i++) {

		if (PX4_ISFINITE(_pos_sp(i))) {

			/* Check if velocity should be used as feedforward */
			if (!PX4_ISFINITE(_vel_sp(i))) {
				_vel_sp(i) = 0.0f;
			}

		} else {

			/* Position controller is not active
			 * -> Set setpoint just to current position since */
			_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
				 */
				_vel_sp(i) = _vel(i);
			}
		}
	}

	if (!PX4_ISFINITE(_yawspeed_sp)) {
		_yawspeed_sp = 0.0f;

		if (!PX4_ISFINITE(_yaw_sp)) {
			_yaw_sp = _yaw_sp_int;
		}

	} else if (!PX4_ISFINITE(_yaw_sp)) {
		_yaw_sp = _yaw_sp_int;
	}

}

/* generate desired velocity */
void TranslationControl::_positionController()
{
	_vel_sp = (_pos_sp - _pos).emult(Pp) + _vel_sp;

	/* make sure velocity setpoint is constrained in all directions (xyz) */
	float vel_norm_xy = sqrtf(
				    _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;
		_vel_sp(1) = _vel_sp(1) * _VelMaxXY / vel_norm_xy;
	}

	_vel_sp(2) = math::constrain(_vel_sp(2), -_VelMaxZ[0], _VelMaxZ[1]);
}

/* generates desired thrust vector */
void TranslationControl::_velocityController(const float &dt)
{
	/* PID
	 * u_des = P(vel_err) + D(vel_err_dot) + I(vel_integral)
	 * Umin <= u_des <= Umax
	 *
	 * Saturation for vel_integral;
	 * u_des = _thr_sp; r = _vel_sp; y = _vel
	 * u_des >= Umax and r - y >= 0 => Saturation = true
	 * u_des >= Umax and r - y <= 0 => Saturation = false
	 * u_des <= Umin and r - y <= 0 => Saturation = true
	 * u_des <= Umin and r - y >= 0 => Saturation = false
	 *
	 */

	Data vel_err = _vel_sp - _vel;

	/* TODO: add offboard acceleration mode */
	matrix::Vector3f 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*/
	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);

	/*TODO: Check if it is beneficial to project thrust onto body z axis  */
<commit_msg>TranslationControl: replace matrix::Vector3f with Data<commit_after>/****************************************************************************
 *
 *   Copyright (c) 2017 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file TranslationControl.cpp
 *
 * This file implements a P-position-control cascaded with a
 * PID-velocity-controller.
 *
 * Inputs: vehicle states (pos, vel, q)
 *         desired setpoints (pos, vel, thrust, yaw)
 * Outputs: thrust and yaw setpoint
 */

#include "TranslationControl.hpp"
#include <float.h>
#include <mathlib/mathlib.h>
#include "uORB/topics/parameter_update.h"
#include "Utility/ControlMath.hpp"
#include <lib/ecl/geo/geo.h>  //TODO: only used for wrap_pi -> move this to mathlib since
// it makes more sense

using namespace matrix;

using Data = matrix::Vector3f;

TranslationControl::TranslationControl()
{
	_Pz_h   = param_find("MPC_Z_P");
	_Pvz_h  = param_find("MPC_Z_VEL_P");
	_Ivz_h  = param_find("MPC_Z_VEL_I");
	_Dvz_h  = param_find("MPC_Z_VEL_D");
	_Pxy_h  = param_find("MPC_XY_P");
	_Pvxy_h = param_find("MPC_XY_VEL_P");
	_Ivxy_h = param_find("MPC_XY_VEL_I");
	_Dvxy_h = param_find("MPC_XY_VEL_D");
	_VelMaxXY = param_find("MPC_XY_VEL_MAX");
	_VelMaxZdown_h = param_find("MPC_Z_VEL_MAX_DN");
	_VelMaxZup_h = param_find("MPC_Z_VEL_MAX_UP");
	_ThrHover_h = param_find("MPC_THR_HOVER");
	_ThrMax_h = param_find("MPC_THR_MAX");
	_ThrMin_h = param_find("MPC_THR_MIN");
	_YawRateMax_h = param_find("MPC_MAN_Y_MAX");
	_Pyaw_h = param_find("MC_YAW_P");

	/* Set parameter the very first time */
	_setParams();
};

void TranslationControl::updateState(const struct vehicle_local_position_s state, const Data &vel_dot,
				     const matrix::Matrix<float, 3, 3> &R)
{
	_pos = Data(&state.x);
	_vel = Data(&state.vx);
	_yaw = state.yaw;
	_vel_dot = vel_dot;
	_R = R;
}

void TranslationControl::updateSetpoint(struct vehicle_local_position_setpoint_s setpoint)
{
	_pos_sp = Data(&setpoint.x);
	_vel_sp = Data(&setpoint.vx);
	_acc_sp = Data(&setpoint.acc_x);
	_yaw_sp = setpoint.yaw; //integrate
	_yawspeed_sp = setpoint.yawspeed;
	_interfaceMapping();
}

void TranslationControl::generateThrustYawSetpoint(const float &dt)
{
	_updateParams();

	_positionController();

	_velocityController(dt);

	_yawController(dt);

}

void TranslationControl::_interfaceMapping()
{
	/* loop through x,y and z component */
	for (int i = 0; i <= 2; i++) {

		if (PX4_ISFINITE(_pos_sp(i))) {

			/* Check if velocity should be used as feedforward */
			if (!PX4_ISFINITE(_vel_sp(i))) {
				_vel_sp(i) = 0.0f;
			}

		} else {

			/* Position controller is not active
			 * -> Set setpoint just to current position since */
			_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
				 */
				_vel_sp(i) = _vel(i);
			}
		}
	}

	if (!PX4_ISFINITE(_yawspeed_sp)) {
		_yawspeed_sp = 0.0f;

		if (!PX4_ISFINITE(_yaw_sp)) {
			_yaw_sp = _yaw_sp_int;
		}

	} else if (!PX4_ISFINITE(_yaw_sp)) {
		_yaw_sp = _yaw_sp_int;
	}
}

/* generate desired velocity */
void TranslationControl::_positionController()
{
	_vel_sp = (_pos_sp - _pos).emult(Pp) + _vel_sp;

	/* make sure velocity setpoint is constrained in all directions (xyz) */
	float vel_norm_xy = sqrtf(
				    _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;
		_vel_sp(1) = _vel_sp(1) * _VelMaxXY / vel_norm_xy;
	}

	_vel_sp(2) = math::constrain(_vel_sp(2), -_VelMaxZ[0], _VelMaxZ[1]);
}

/* generates desired thrust vector */
void TranslationControl::_velocityController(const float &dt)
{
	/* PID
	 * u_des = P(vel_err) + D(vel_err_dot) + I(vel_integral)
	 * Umin <= u_des <= Umax
	 *
	 * Saturation for vel_integral;
	 * u_des = _thr_sp; r = _vel_sp; y = _vel
	 * u_des >= Umax and r - y >= 0 => Saturation = true
	 * u_des >= Umax and r - y <= 0 => Saturation = false
	 * u_des <= Umin and r - y <= 0 => Saturation = true
	 * u_des <= Umin and r - y >= 0 => Saturation = false
	 *
	 */

	Data vel_err = _vel_sp - _vel;

	/* TODO: add offboard acceleration mode */
	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*/
	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);

	/*TODO: Check if it is beneficial to project thrust onto body z axis  */

{'fraction_non_alphanumeric': 0.13192169396723932, 'fraction_numerical': 0.008070315621254494, 'mean_word_length': 4.314649681528662, 'pattern_counts': {'":': 0, '<': 25, '<?xml version=': 0, '>': 33, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 2, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '7143998', 'n_tokens_mistral': 4695, 'n_tokens_neox': 4343, 'n_words': 1226}