text: /****************************************************************************
   *
   *   Copyright (c) 2018 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 FlightAutoLine.cpp
   */
  
  #include "FlightTaskAutoLine.hpp"
  #include <mathlib/mathlib.h>
  
  using namespace matrix;
  
  static constexpr float SIGMA_NORM		=	0.001f;
  
  void FlightTaskAutoLine::_generateSetpoints()
  {
  	if (!PX4_ISFINITE(_yaw_setpoint)) {
  		// no valid heading -> set heading along track
  		_generateHeadingAlongTrack();
  	}
  
  	if (_param_mpc_yaw_mode.get() == 4 && !_yaw_sp_aligned) {
  		// Wait for the yaw setpoint to be aligned
  		if (!_position_locked) {
  			_velocity_setpoint.setAll(0.f);
  			_position_setpoint = _position;
  			_position_locked = true;
  		}
  
  	} else {
  		_position_locked = false;
  		_generateAltitudeSetpoints();
  		_generateXYsetpoints();
  	}
  }
  
  void FlightTaskAutoLine::_setSpeedAtTarget()
  {
  	// angle goes from 0 to 2 with 0 = large angle, 2 = small angle:   0 = PI ; 2 = PI*0
  	float angle = 2.0f; // minimum angle
  	_speed_at_target = 0.0f;
  
  	if (Vector2f(&(_target - _next_wp)(0)).length() > 0.001f &&
  	    (Vector2f(&(_target - _prev_wp)(0)).length() > _target_acceptance_radius)) {
  		// angle = cosf(x) + 1.0f
  		angle =
  			Vector2f(&(_target - _prev_wp)(0)).unit_or_zero()
  			* Vector2f(&(_target - _next_wp)(0)).unit_or_zero()
  			+ 1.0f;
  		_speed_at_target = math::expontialFromLimits(angle, 0.0f, _param_mpc_cruise_90.get(), _mc_cruise_speed);
  	}
  }
  
  
  void FlightTaskAutoLine::_generateHeadingAlongTrack()
  {
  	Vector2f prev_to_dest(_target - _prev_wp);
  	_compute_heading_from_2D_vector(_yaw_setpoint, prev_to_dest);
  
  }
  
  void FlightTaskAutoLine::_generateXYsetpoints()
  {
  	_setSpeedAtTarget();
  	Vector2f pos_sp_to_dest(_target - _position_setpoint);
  	const bool has_reached_altitude = fabsf(_target(2) - _position(2)) < _param_nav_mc_alt_rad.get();
  
  	if ((_speed_at_target < 0.001f && pos_sp_to_dest.length() < _target_acceptance_radius) ||
  	    (!has_reached_altitude && pos_sp_to_dest.length() < _target_acceptance_radius)) {
  
  		// Vehicle reached target in xy and no passing required. Lock position
  		_position_setpoint(0) = _target(0);
  		_position_setpoint(1) = _target(1);
  		_velocity_setpoint(0) = _velocity_setpoint(1) = 0.0f;
  
  	} else {
  		// Vehicle needs to pass waypoint
  		// Ensure that vehicle never gets stuck because of too low target-speed
  		_speed_at_target = math::max(_speed_at_target, 0.5f);
  
  		// Get various path specific vectors. */
  		Vector2f u_prev_to_dest = Vector2f(_target - _prev_wp).unit_or_zero();
  		Vector2f prev_to_pos(_position - _prev_wp);
  		Vector2f closest_pt = Vector2f(_prev_wp) + u_prev_to_dest * (prev_to_pos * u_prev_to_dest);
  		Vector2f closest_to_dest(_target - _position);
  		Vector2f prev_to_dest(_target - _prev_wp);
  		float speed_sp_track = _mc_cruise_speed;
  		float speed_sp_prev_track = math::max(Vector2f(_velocity_setpoint) * u_prev_to_dest, 0.0f);
  
  		// Distance to target when brake should occur. The assumption is made that
  		// 1.5 * cruising speed is enough to break.
  		float target_threshold = 1.5f * _mc_cruise_speed;
  		float speed_threshold = _mc_cruise_speed;
  		const float threshold_max = target_threshold;
  
  		if (target_threshold > 0.5f * prev_to_dest.length()) {
  			// Target threshold cannot be more than distance from previous to target
  			target_threshold = 0.5f * prev_to_dest.length();
  		}
  
  		// Compute maximum speed at target threshold */
  		if (threshold_max > _target_acceptance_radius) {
  			float m = (_mc_cruise_speed - _speed_at_target) / (threshold_max - _target_acceptance_radius);
  			speed_threshold = m * (target_threshold - _target_acceptance_radius) + _speed_at_target; // speed at transition
  		}
  
  		// Either accelerate or decelerate
  		if (closest_to_dest.length() < target_threshold) {
  
  			// Vehicle is close to destination. Start to decelerate
  
  			if (!has_reached_altitude) {
  				// Altitude is not reached yet. Vehicle has to stop first before proceeding
  				_speed_at_target = 0.0f;
  			}
  
  			float acceptance_radius = _target_acceptance_radius;
  
  			if (_speed_at_target < 0.01f) {
  				// If vehicle wants to stop at the target, then set acceptance radius to zero as well.
  				acceptance_radius = 0.0f;
  			}
  
  			if ((target_threshold - acceptance_radius) >= SIGMA_NORM) {
  
  				// Slow down depending on distance to target minus acceptance radius.
  				float m = (speed_threshold - _speed_at_target) / (target_threshold - acceptance_radius);
  				speed_sp_track = m * (closest_to_dest.length() - acceptance_radius) + _speed_at_target; // speed at transition
  
  			} else {
  				speed_sp_track = _speed_at_target;
  			}
  
  			// If we are close to target and the previous speed setpoint along track was smaller than
  			// current speed setpoint along track, then take over the previous one.
  			// This ensures smoothness since we anyway want to slow down.
  			if ((speed_sp_prev_track < speed_sp_track)
  			    && (speed_sp_track * speed_sp_prev_track > 0.0f)
  			    && (speed_sp_prev_track > _speed_at_target)) {
  				speed_sp_track = speed_sp_prev_track;
  			}
  
  		} else {
  
  			// Vehicle is still far from destination. Accelerate or keep maximum target speed.
  			float acc_track = (speed_sp_track - speed_sp_prev_track) / _deltatime;
  
  			// If yaw offset is large, only accelerate with 0.5 m/s^2.
  			float acc_max = (_yaw_sp_aligned) ? _param_mpc_acc_hor.get() : 0.5f;
  
  			if (acc_track > acc_max) {
  				// accelerate towards target
  				speed_sp_track = acc_max * _deltatime + speed_sp_prev_track;
  			}
  
  			// ensure that desired speed does not exceed speed at threshold
  			speed_sp_track = math::min(speed_threshold, speed_sp_track);
  		}
  
  		speed_sp_track = math::constrain(speed_sp_track, 0.0f, _mc_cruise_speed);
  
  		_position_setpoint(0) = closest_pt(0);
  		_position_setpoint(1) = closest_pt(1);
  		Vector2f velocity_sp_xy = u_prev_to_dest * speed_sp_track;
  		_velocity_setpoint(0) =  velocity_sp_xy(0);
  		_velocity_setpoint(1) =  velocity_sp_xy(1);
  	}
  }
  
  void FlightTaskAutoLine::_generateAltitudeSetpoints()
  {
  	// Total distance between previous and target set-point.
  	const float dist = fabsf(_target(2) - _prev_wp(2));
  
  	// If target has not been reached, then compute set-point depending on maximum velocity.
  	if ((dist > SIGMA_NORM) && (fabsf(_position(2) - _target(2)) > 0.1f)) {
  
  		// get various distances */
  		const float dist_to_prev = fabsf(_position(2) - _prev_wp(2));
  		const float dist_to_target = fabsf(_target(2) - _position(2));
  
  		// check sign
  		const bool flying_upward = _target(2) < _position(2);
  
  		// limit vertical downwards speed (positive z) close to ground
  		// for now we use the altitude above home and assume that we want to land at same height as we took off
  		float vel_limit = math::gradual(_dist_to_ground,
  						_param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
  						_param_mpc_land_speed.get(), _constraints.speed_down);
  
  		// Speed at threshold is by default maximum speed. Threshold defines
  		// the point in z at which vehicle slows down to reach target altitude.
  		float speed_sp = (flying_upward) ? _constraints.speed_up : vel_limit;
  
  		// Target threshold defines the distance to target(2) at which
  		// the vehicle starts to slow down to approach the target smoothly.
  		float target_threshold = speed_sp * 1.5f;
  
  		// If the total distance in z is NOT 2x distance of target_threshold, we
  		// will need to adjust the final_velocity in z.
  		const bool is_2_target_threshold = dist >= 2.0f * target_threshold;
  		const float min_vel = 0.2f; // minimum velocity: this is needed since estimation is not perfect
  		const float slope = (speed_sp - min_vel) / (target_threshold); // defines the the acceleration when slowing down */
  
  		if (!is_2_target_threshold) {
  			// Adjust final_velocity since we are already are close to target and therefore it is not necessary to accelerate
  			// upwards with full speed.
  			target_threshold = dist * 0.5f;
  			// get the velocity at target_threshold
  			speed_sp = slope * (target_threshold) + min_vel;
  		}
  
  		// we want to slow down
  		if (dist_to_target < target_threshold) {
  
  			speed_sp = slope * dist_to_target + min_vel;
  
  		} else if (dist_to_prev < target_threshold) {
  			// we want to accelerate
  
  			const float acc = (speed_sp - fabsf(_velocity_setpoint(2))) / _deltatime;
  			const float acc_max = (flying_upward) ? (_param_mpc_acc_up_max.get() * 0.5f) : (_param_mpc_acc_down_max.get() * 0.5f);
  
  			if (acc > acc_max) {
  				speed_sp = acc_max * _deltatime + fabsf(_velocity_setpoint(2));
  			}
  		}
  
  		// make sure vel_sp_z is always positive
  		if (speed_sp < 0.0f) {
  			PX4_WARN("speed cannot be smaller than 0");
  			speed_sp = 0.0f;
  		}
  
  		// get the sign of vel_sp_z
  		_velocity_setpoint(2) = (flying_upward) ? -speed_sp : speed_sp;
  		_position_setpoint(2) = NAN; // We don't care about position setpoint
  
  	} else {
  
  		// vehicle reached desired target altitude
  		_velocity_setpoint(2) = 0.0f;
  		_position_setpoint(2) = _target(2);
  	}
  }
  