/*! \brief A ray-rod intersection test.
      
      A rod is a cylinder which is not infinite, but of limited
      length. The cylinder is defined using a single base vertex at
      the center of the bottom circular face and an axial vector
      pointing from the base vertex to the top vertex. This test
      ignores the back face of the rod. It is used to detect when a
      ray will enter a rod.
     
      \param T The origin of the ray relative to the base vertex.
      \param D The direction/velocity of the ray.
      \param A The axial vector of the rod.
      \param r Radius of the rod.
      \return The time until the intersection, or HUGE_VAL if no intersection.
    */
    inline double ray_rod(math::Vector T, math::Vector D, const math::Vector& A, const double r)
    {
      double t = ray_cylinder(T, D, A / A.nrm(), r);
      double Tproj = ((T + t * D) | A);
      
      if ((Tproj < 0) || (Tproj > A.nrm2())) return HUGE_VAL;

      return t;
    }

void ALSound::SetListener(const Math::Vector &eye, const Math::Vector &lookat)
{
    m_eye = eye;
    m_lookat = lookat;
    Math::Vector forward = lookat - eye;
    forward.Normalize();
    float orientation[] = {forward.x, forward.y, forward.z, 0.f, -1.0f, 0.0f};

    alListener3f(AL_POSITION, eye.x, eye.y, eye.z);
    alListenerfv(AL_ORIENTATION, orientation);
}

    /*! \brief A ray-inverse_rod intersection test.
    
      A rod is a cylinder which is not infinite, but of limited
      length. An inverse rod is used to test when a ray will exit a
      rod. The cylinder is defined using a single base vertex at the
      center of the bottom circular face and an axial vector
      pointing from the base vertex to the top vertex. This test
      ignores the back face of the rod.
     
      \param T The origin of the ray relative to the base vertex.
      \param D The direction/velocity of the ray.
      \param A The axial vector of the inverse rod.
      \param r Radius of the inverse rod.

      \tparam always_intersect If true, this will ensure that glancing
      ray's never escape the enclosing sphere by returning the time
      when the ray is nearest the sphere if the ray does not intersect
      the sphere.

      \return The time until the intersection, or HUGE_VAL if no intersection.
    */
    inline double ray_inv_rod(math::Vector T, math::Vector D, const math::Vector& A, const double r)
    {
      double t = ray_inv_cylinder(T, D, A / A.nrm(), r);

      M_throw() << "Confirm that this function is correct";

      double Tproj = ((T + t * D) | A);
      
      if ((Tproj < 0) || (Tproj > A.nrm2())) return HUGE_VAL;

      return t;
    }

    //! \brief A ray-sphere intersection test with backface culling.
    //!
    //! \param T The origin of the ray relative to the sphere center.
    //! \param D The direction/velocity of the ray.
    //! \param r The radius of the sphere.
    //! \return The time until the intersection, or HUGE_VAL if no intersection.
    inline double ray_sphere_bfc(const math::Vector& T,
				 const math::Vector& D,
				 const double& r)
    {
      double TD = (T | D);

      if (TD >= 0) return HUGE_VAL;
      
      double c = T.nrm2() - r * r;
      double arg = TD * TD - D.nrm2() * c;
      
      if (arg < 0) return HUGE_VAL;

      return  - c / (TD - std::sqrt(arg));
    }

    //! \brief A ray-inverse_sphere intersection test with backface culling.
    //!
    //! An inverse sphere means an "enclosing" sphere.
    //!
    //! \param T The origin of the ray relative to the inverse sphere
    //! center.
    //! \param D The direction/velocity of the ray.
    //! \param d The diameter of the inverse sphere.
    //! \return The time until the intersection, or HUGE_VAL if no intersection.
    inline double ray_inv_sphere_bfc(const math::Vector& T,
				     const math::Vector& D,
				     const double& r)
    {
      double D2 = D.nrm2();

      if (D2 == 0) return HUGE_VAL;
      
      double TD = T | D;
      double arg = TD * TD - D2 * (T.nrm2() - r * r);

      if (arg < 0) return HUGE_VAL;

      return (std::sqrt(arg) - TD) / D2;
    }

/*
 * Attitude rates controller.
 * Input: '_rates_sp' vector, '_thrust_sp'
 * Output: '_att_control' vector
 */
void
MulticopterAttitudeControl::control_attitude_rates(float dt)
{
	/* reset integral if disarmed */
	if (!_armed.armed) {
		_rates_int.zero();
	}

	/* current body angular rates */
	math::Vector<3> rates;
	rates(0) = _v_att.rollspeed;
	rates(1) = _v_att.pitchspeed;
	rates(2) = _v_att.yawspeed;

	/* angular rates error */
	math::Vector<3> rates_err = _rates_sp - rates;
	_att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int;
	_rates_prev = rates;

	/* update integral only if not saturated on low limit */
	if (_thrust_sp > MIN_TAKEOFF_THRUST) {
		for (int i = 0; i < 3; i++) {
			if (fabsf(_att_control(i)) < _thrust_sp) {
				float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;

				if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
				    _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
					_rates_int(i) = rate_i;
				}
			}
		}
	}
}

/*
 * Attitude rates controller.
 * Input: '_rates_sp' vector, '_thrust_sp'
 * Output: '_att_control' vector
 */
void
MulticopterAttitudeControl::control_attitude_rates(float dt)
{
	/* reset integral if disarmed */
	if (!_armed.armed || !_vehicle_status.is_rotary_wing) {
		_rates_int.zero();
	}

	/* current body angular rates */
	math::Vector<3> rates;
	rates(0) = _ctrl_state.roll_rate;
	rates(1) = _ctrl_state.pitch_rate;
	rates(2) = _ctrl_state.yaw_rate;

	/* angular rates error */
	math::Vector<3> rates_err = _rates_sp - rates;
	_att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int +
		       _params.rate_ff.emult(_rates_sp - _rates_sp_prev) / dt;
	_rates_sp_prev = _rates_sp;
	_rates_prev = rates;

	/* update integral only if not saturated on low limit and if motor commands are not saturated */
	if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit) {
		for (int i = 0; i < 3; i++) {
			if (fabsf(_att_control(i)) < _thrust_sp) {
				float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;

				if (PX4_ISFINITE(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
				    _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
					_rates_int(i) = rate_i;
				}
			}
		}
	}
}

void math::Matrix::assignToBucket(math::Vector &vector) {
	math::Space *space = vector.getSpace();

	int column = std::floor(vector.get(math::Dimension::X) / bucketSize[math::Dimension::X]);
	int row = std::floor(vector.get(math::Dimension::Y) / bucketSize[math::Dimension::Y]);
	int dimension = std::floor(std::min<double>(vector.get(math::Dimension::Z), Vector::getSpace()->getMax(math::Dimension::Z)) /
		bucketSize[math::Dimension::Z]);

	if (column == space->getMax(math::Dimension::X) / bucketSize[math::Dimension::X])
		column--;
	if (row == space->getMax(math::Dimension::Y) / bucketSize[math::Dimension::Y])
		row--;
	if (dimension == space->getMax(math::Dimension::Z) / bucketSize[math::Dimension::Z])
		dimension--;

	values[row][column][dimension]++;
}

template<UnsignedInt dimensions> std::size_t AbstractImage::dataSize(Math::Vector<dimensions, Int> size) const {
    /** @todo Code this properly when all @fn_gl{PixelStore} parameters are implemented */
    /* Row size, rounded to multiple of 4 bytes */
    const std::size_t rowSize = ((size[0]*pixelSize() + 3)/4)*4;

    /** @todo Can't this be done somewhat nicer? */
    size[0] = 1;
    return rowSize*size.product();
}

	OffcentreSpheresOverlapFunction(const math::Vector& rij, const math::Vector& vij, const math::Vector& omegai, const math::Vector& omegaj,
					const math::Vector& nu1, const math::Vector& nu2, const double diameter1, const double diameter2, 
					const double maxdist, const double t, const double invgamma, const double t_min, const double t_max):
	  w1(omegai), w2(omegaj), u1(nu1), u2(nu2), r12(rij), v12(vij), _diameter1(diameter1), _diameter2(diameter2), _invgamma(invgamma), _t(t), _t_min(t_min), _t_max(t_max)
	{
	  double Gmax = std::max(1 + t * invgamma, 1 + (t + t_max) * invgamma);
	  const double sigmaij = 0.5 * (_diameter1 + _diameter2);
	  const double sigmaij2 = sigmaij * sigmaij;
	  double magw1 = w1.nrm(), magw2 = w2.nrm();
	  double rijmax = Gmax * std::max(maxdist, rij.nrm());
#ifdef MAGNET_DEBUG
	  if (rij.nrm() > 1.0001 * maxdist)
	    std::cout << "WARNING!: Particle separation is larger than the maximum specified. " <<rij.nrm() << ">" << maxdist << "\n";
#endif
	  double magu1 = u1.nrm(), magu2 = u2.nrm();
	  double vijmax = v12.nrm() + Gmax * (magu1 * magw1 + magu2 * magw2) + std::abs(invgamma) * (magu1 + magu2);
	  double aijmax = Gmax * (magu1 * magw1 * magw1 + magu2 * magw2 * magw2) + 2 * std::abs(invgamma) * (magu1 * magw1 + magu2 * magw2);
	  double dotaijmax = Gmax * (magu1 * magw1 * magw1 * magw1 + magu2 * magw2 * magw2 * magw2) + 3 * std::abs(invgamma) * (magu1 * magw1 * magw1 + magu2 * magw2 * magw2);

	  _f1max = 2 * rijmax * vijmax + 2 * Gmax * std::abs(invgamma) * sigmaij2;
	  _f2max = 2 * vijmax * vijmax + 2 * rijmax * aijmax + 2 * invgamma * invgamma * sigmaij2;
	  _f3max = 6 * vijmax * aijmax + 2 * rijmax * dotaijmax;
	}

	std::array<double, nderivs> eval(const double dt = 0) const
	{
	  math::Vector u1new = Rodrigues(w1 * dt) * math::Vector(u1);
	  math::Vector u2new = Rodrigues(w2 * dt) * math::Vector(u2);

	  const double colldiam = 0.5 * (_diameter1 + _diameter2);
	  const double growthfactor = 1 + _invgamma * (_t + dt);
	  const math::Vector rij = r12 + dt * v12 + growthfactor * (u1new - u2new);
	  const math::Vector vij = v12 + growthfactor * ((w1 ^ u1new) - (w2 ^ u2new)) + _invgamma * (u1new - u2new);
	  const math::Vector aij = growthfactor * (-w1.nrm2() * u1new + w2.nrm2() * u2new) + 2 * _invgamma * ((w1 ^ u1new) - (w2 ^ u2new));
	  const math::Vector dotaij = growthfactor * (-w1.nrm2() * (w1 ^ u1new) + w2.nrm2() * (w2 ^ u2new)) + 3 * _invgamma * (-w1.nrm2() * u1new + w2.nrm2() * u2new);

	  std::array<double, nderivs> retval;
	  for (size_t i(0); i < nderivs; ++i)
	    switch (first_deriv + i) {
	    case 0: retval[i] = (rij | rij) - growthfactor * growthfactor * colldiam * colldiam; break;
	    case 1: retval[i] = 2 * (rij | vij) - 2 * _invgamma * growthfactor * colldiam * colldiam; break;
	    case 2: retval[i] = 2 * vij.nrm2() + 2 * (rij | aij) - 2 * _invgamma * _invgamma * colldiam * colldiam; break;
	    case 3: retval[i] = 6 * (vij | aij) + 2 * (rij | dotaij); break;
	    default:
	      M_throw() << "Invalid access";
	    }
	  return retval;
	}

/*
 * Attitude rates controller.
 * Input: '_rates_sp' vector, '_thrust_sp'
 * Output: '_att_control' vector
 */
void
MulticopterAttitudeControl::control_attitude_rates(float dt)
{
	/* reset integral if disarmed */
	if (!_armed.armed || !_vehicle_status.is_rotary_wing) {
		_rates_int.zero();
	}

	/* current body angular rates */
	math::Vector<3> rates;
	rates(0) = _ctrl_state.roll_rate;
	rates(1) = _ctrl_state.pitch_rate;
	rates(2) = _ctrl_state.yaw_rate;

	/* throttle pid attenuation factor */
	float tpa =  fmaxf(0.0f, fminf(1.0f, 1.0f - _params.tpa_slope * (fabsf(_v_rates_sp.thrust) - _params.tpa_breakpoint)));

	/* angular rates error */
	math::Vector<3> rates_err = _rates_sp - rates;

	_att_control = _params.rate_p.emult(rates_err * tpa) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int +
		       _params.rate_ff.emult(_rates_sp);

	_rates_sp_prev = _rates_sp;
	_rates_prev = rates;

	/* update integral only if not saturated on low limit and if motor commands are not saturated */
	if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit) {
		for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) {
			if (fabsf(_att_control(i)) < _thrust_sp) {
				float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;

				if (PX4_ISFINITE(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
				    _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT &&
				    /* if the axis is the yaw axis, do not update the integral if the limit is hit */
				    !((i == AXIS_INDEX_YAW) && _motor_limits.yaw)) {
					_rates_int(i) = rate_i;
				}
			}
		}
	}
}

void
MulticopterPositionControl::control_manual(float dt)
{
	_sp_move_rate.zero();

	if (_control_mode.flag_control_altitude_enabled) {
		if(_reset_mission)
		{
			_reset_mission = false;
			_mode_mission = 1 ;
			_hover_time = 0.0 ;
		}
		float height_hover_constant=-1.0;
		float hover_time_constant = 20.0;
		switch(_mode_mission)
		{	
			case 1:
				_sp_move_rate(2) = -0.8;
				if(_pos_sp(2)<=height_hover_constant)
					_mode_mission=2;
				break;
			case 2:
				_hover_time += dt;
				if(_hover_time>hover_time_constant)
				{
					_hover_time=0.0;
					_mode_mission=3;
				}
				break;
			case 3:
				_pos_sp_triplet.current.type =position_setpoint_s::SETPOINT_TYPE_LAND;
				break;
			default:
				/* move altitude setpoint with throttle stick */
				_sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz);
				break;
		}
	}

	if (_control_mode.flag_control_position_enabled) {
		/* move position setpoint with roll/pitch stick */
		_sp_move_rate(0) = _manual.x;
		_sp_move_rate(1) = _manual.y;
	}

	/* limit setpoint move rate */
	float sp_move_norm = _sp_move_rate.length();

	if (sp_move_norm > 1.0f) {
		_sp_move_rate /= sp_move_norm;
	}

	/* _sp_move_rate scaled to 0..1, scale it to max speed and rotate around yaw */
	math::Matrix<3, 3> R_yaw_sp;
	R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
	_sp_move_rate = R_yaw_sp * _sp_move_rate.emult(_params.vel_max);

	if (_control_mode.flag_control_altitude_enabled) {
		/* reset alt setpoint to current altitude if needed */
		reset_alt_sp();
	}

	if (_control_mode.flag_control_position_enabled) {
		/* reset position setpoint to current position if needed */
		reset_pos_sp();
	}

	/* feed forward setpoint move rate with weight vel_ff */
	_vel_ff = _sp_move_rate.emult(_params.vel_ff);

	/* move position setpoint */
	_pos_sp += _sp_move_rate * dt;

	/* check if position setpoint is too far from actual position */
	math::Vector<3> pos_sp_offs;
	pos_sp_offs.zero();

	if (_control_mode.flag_control_position_enabled) {
		pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
		pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
	}

	if (_control_mode.flag_control_altitude_enabled) {
		pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
	}

	float pos_sp_offs_norm = pos_sp_offs.length();

	if (pos_sp_offs_norm > 1.0f) {
		pos_sp_offs /= pos_sp_offs_norm;
		_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
	}
}

void
MulticopterPositionControl::task_main()
{
	warnx("started");

	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
	mavlink_log_info(_mavlink_fd, "[mpc] started");

	/*
	 * do subscriptions
	 */
	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
	_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
	_params_sub = orb_subscribe(ORB_ID(parameter_update));
	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
	_arming_sub = orb_subscribe(ORB_ID(actuator_armed));
	_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
	_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));

	parameters_update(true);

	/* initialize values of critical structs until first regular update */
	_arming.armed = false;

	/* get an initial update for all sensor and status data */
	poll_subscriptions();

	bool reset_int_z = true;
	bool reset_int_z_manual = false;
	bool reset_int_xy = true;
	bool was_armed = false;

	hrt_abstime t_prev = 0;

	const float alt_ctl_dz = 0.2f;
	const float pos_ctl_dz = 0.05f;

	math::Vector<3> sp_move_rate;
	sp_move_rate.zero();
	math::Vector<3> thrust_int;
	thrust_int.zero();
	math::Matrix<3, 3> R;
	R.identity();

	/* wakeup source */
	struct pollfd fds[1];

	fds[0].fd = _local_pos_sub;
	fds[0].events = POLLIN;

	while (!_task_should_exit) {
		/* wait for up to 500ms for data */
		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);

		/* timed out - periodic check for _task_should_exit */
		if (pret == 0) {
			continue;
		}

		/* this is undesirable but not much we can do */
		if (pret < 0) {
			warn("poll error %d, %d", pret, errno);
			continue;
		}

		poll_subscriptions();
		parameters_update(false);

		hrt_abstime t = hrt_absolute_time();
		float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f;
		t_prev = t;

		if (_control_mode.flag_armed && !was_armed) {
			/* reset setpoints and integrals on arming */
			_reset_pos_sp = true;
			_reset_alt_sp = true;
			reset_int_z = true;
			reset_int_xy = true;
		}

		was_armed = _control_mode.flag_armed;

		update_ref();

		if (_control_mode.flag_control_altitude_enabled ||
		    _control_mode.flag_control_position_enabled ||
		    _control_mode.flag_control_climb_rate_enabled ||
		    _control_mode.flag_control_velocity_enabled) {

			_pos(0) = _local_pos.x;
			_pos(1) = _local_pos.y;
			_pos(2) = _local_pos.z;

			_vel(0) = _local_pos.vx;
			_vel(1) = _local_pos.vy;
			_vel(2) = _local_pos.vz;

			sp_move_rate.zero();

//.........这里部分代码省略.........

//==================================================
//! Dot Product
//==================================================
float Math::Vector::operator*( const Math::Vector& rhs ) const {
	return x*rhs.X() + y*rhs.Y() + z*rhs.Z();
}

//.........这里部分代码省略.........
					vehicle_attitude_setpoint_poll();
					_thrust_sp = _v_att_sp.thrust;
					math::Quaternion q(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
					math::Quaternion q_sp(&_v_att_sp.q_d[0]);
					_ts_opt_recovery->setAttGains(_params.att_p, _params.yaw_ff);
					_ts_opt_recovery->calcOptimalRates(q, q_sp, _v_att_sp.yaw_sp_move_rate, _rates_sp);

					/* limit rates */
					for (int i = 0; i < 3; i++) {
						_rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i));
					}
				}

				/* publish attitude rates setpoint */
				_v_rates_sp.roll = _rates_sp(0);
				_v_rates_sp.pitch = _rates_sp(1);
				_v_rates_sp.yaw = _rates_sp(2);
				_v_rates_sp.thrust = _thrust_sp;
				_v_rates_sp.timestamp = hrt_absolute_time();

				if (_v_rates_sp_pub != nullptr) {
					orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);

				} else if (_rates_sp_id) {
					_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
				}

				//}

			} else {
				/* attitude controller disabled, poll rates setpoint topic */
				if (_v_control_mode.flag_control_manual_enabled) {
					/* manual rates control - ACRO mode */
					_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x,
								    _manual_control_sp.r).emult(_params.acro_rate_max);
					_thrust_sp = math::min(_manual_control_sp.z, MANUAL_THROTTLE_MAX_MULTICOPTER);

					/* publish attitude rates setpoint */
					_v_rates_sp.roll = _rates_sp(0);
					_v_rates_sp.pitch = _rates_sp(1);
					_v_rates_sp.yaw = _rates_sp(2);
					_v_rates_sp.thrust = _thrust_sp;
					_v_rates_sp.timestamp = hrt_absolute_time();

					if (_v_rates_sp_pub != nullptr) {
						orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);

					} else if (_rates_sp_id) {
						_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
					}

				} else {
					/* attitude controller disabled, poll rates setpoint topic */
					vehicle_rates_setpoint_poll();
					_rates_sp(0) = _v_rates_sp.roll;
					_rates_sp(1) = _v_rates_sp.pitch;
					_rates_sp(2) = _v_rates_sp.yaw;
					_thrust_sp = _v_rates_sp.thrust;
				}
			}

			if (_v_control_mode.flag_control_rates_enabled) {
				control_attitude_rates(dt);

				/* publish actuator controls */
				_actuators.control[0] = (PX4_ISFINITE(_att_control(0))) ? _att_control(0) : 0.0f;

void MulticopterPositionControl::control_auto(float dt)
{
	if (!_mode_auto) {
		_mode_auto = true;
		/* reset position setpoint on AUTO mode activation */
		reset_pos_sp();
		reset_alt_sp();
	}

	//Poll position setpoint
	bool updated;
	orb_check(_pos_sp_triplet_sub, &updated);
	if (updated) {
		orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);

		//Make sure that the position setpoint is valid
		if (!isfinite(_pos_sp_triplet.current.lat) ||
			!isfinite(_pos_sp_triplet.current.lon) ||
			!isfinite(_pos_sp_triplet.current.alt)) {
			_pos_sp_triplet.current.valid = false;
		}
	}

	if (_pos_sp_triplet.current.valid) {
		/* in case of interrupted mission don't go to waypoint but stay at current position */
		_reset_pos_sp = true;
		_reset_alt_sp = true;

		/* project setpoint to local frame */
		math::Vector<3> curr_sp;
		map_projection_project(&_ref_pos,
				       _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
				       &curr_sp.data[0], &curr_sp.data[1]);
		curr_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);

		/* scaled space: 1 == position error resulting max allowed speed, L1 = 1 in this space */
		math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max);	// TODO add mult param here

		/* convert current setpoint to scaled space */
		math::Vector<3> curr_sp_s = curr_sp.emult(scale);

		/* by default use current setpoint as is */
		math::Vector<3> pos_sp_s = curr_sp_s;

		if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) {
			/* follow "previous - current" line */
			math::Vector<3> prev_sp;
			map_projection_project(&_ref_pos,
						   _pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon,
						   &prev_sp.data[0], &prev_sp.data[1]);
			prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt);

			if ((curr_sp - prev_sp).length() > MIN_DIST) {

				/* find X - cross point of L1 sphere and trajectory */
				math::Vector<3> pos_s = _pos.emult(scale);
				math::Vector<3> prev_sp_s = prev_sp.emult(scale);
				math::Vector<3> prev_curr_s = curr_sp_s - prev_sp_s;
				math::Vector<3> curr_pos_s = pos_s - curr_sp_s;
				float curr_pos_s_len = curr_pos_s.length();
				if (curr_pos_s_len < 1.0f) {
					/* copter is closer to waypoint than L1 radius */
					/* check next waypoint and use it to avoid slowing down when passing via waypoint */
					if (_pos_sp_triplet.next.valid) {
						math::Vector<3> next_sp;
						map_projection_project(&_ref_pos,
									   _pos_sp_triplet.next.lat, _pos_sp_triplet.next.lon,
									   &next_sp.data[0], &next_sp.data[1]);
						next_sp(2) = -(_pos_sp_triplet.next.alt - _ref_alt);

						if ((next_sp - curr_sp).length() > MIN_DIST) {
							math::Vector<3> next_sp_s = next_sp.emult(scale);

							/* calculate angle prev - curr - next */
							math::Vector<3> curr_next_s = next_sp_s - curr_sp_s;
							math::Vector<3> prev_curr_s_norm = prev_curr_s.normalized();

							/* cos(a) * curr_next, a = angle between current and next trajectory segments */
							float cos_a_curr_next = prev_curr_s_norm * curr_next_s;

							/* cos(b), b = angle pos - curr_sp - prev_sp */
							float cos_b = -curr_pos_s * prev_curr_s_norm / curr_pos_s_len;

							if (cos_a_curr_next > 0.0f && cos_b > 0.0f) {
								float curr_next_s_len = curr_next_s.length();
								/* if curr - next distance is larger than L1 radius, limit it */
								if (curr_next_s_len > 1.0f) {
									cos_a_curr_next /= curr_next_s_len;
								}

								/* feed forward position setpoint offset */
								math::Vector<3> pos_ff = prev_curr_s_norm *
										cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) *
										(1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f));
								pos_sp_s += pos_ff;
							}
						}
					}

				} else {
//.........这里部分代码省略.........

void
MulticopterPositionControl::task_main()
{

	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);

	/*
	 * do subscriptions
	 */
	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
	_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
	_params_sub = orb_subscribe(ORB_ID(parameter_update));
	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
	_arming_sub = orb_subscribe(ORB_ID(actuator_armed));
	_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
	_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
	_local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
	_global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));


	parameters_update(true);

	/* initialize values of critical structs until first regular update */
	_arming.armed = false;

	/* get an initial update for all sensor and status data */
	poll_subscriptions();

	bool reset_int_z = true;
	bool reset_int_z_manual = false;
	bool reset_int_xy = true;
	bool reset_yaw_sp = true;
	bool was_armed = false;

	hrt_abstime t_prev = 0;
	_hover_time = 0.0; // miao:
	_mode_mission = 1;
	math::Vector<3> thrust_int;
	thrust_int.zero();
	math::Matrix<3, 3> R;
	R.identity();

	/* wakeup source */
	struct pollfd fds[1];

	fds[0].fd = _local_pos_sub;
	fds[0].events = POLLIN;

	while (!_task_should_exit) {
		/* wait for up to 500ms for data */
		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);

		/* timed out - periodic check for _task_should_exit */
		if (pret == 0) {
			continue;
		}

		/* this is undesirable but not much we can do */
		if (pret < 0) {
			warn("poll error %d, %d", pret, errno);
			continue;
		}

		poll_subscriptions();
		parameters_update(false);

		hrt_abstime t = hrt_absolute_time();
		float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f;
		t_prev = t;

		if (_control_mode.flag_armed && !was_armed) {
			/* reset setpoints and integrals on arming */
			_reset_pos_sp = true;
			_reset_alt_sp = true;
			reset_int_z = true;
			reset_int_xy = true;
			reset_yaw_sp = true;
			_reset_mission = true;//miao:
		}

		//Update previous arming state
		was_armed = _control_mode.flag_armed;

		update_ref();

		if (_control_mode.flag_control_altitude_enabled ||
		    _control_mode.flag_control_position_enabled ||
		    _control_mode.flag_control_climb_rate_enabled ||
		    _control_mode.flag_control_velocity_enabled) {

			_pos(0) = _local_pos.x;
			_pos(1) = _local_pos.y;
			_pos(2) = _local_pos.z;

			_vel(0) = _local_pos.vx;
			_vel(1) = _local_pos.vy;
			_vel(2) = _local_pos.vz;

			_vel_ff.zero();
//.........这里部分代码省略.........

void run () 
{

  Image::Header H (argument[0]);

  Image::Info info (H);
  info.set_ndim (3);
  Image::BufferScratch<bool> mask (info);
  auto v_mask = mask.voxel();

  std::string mask_path;
  Options opt = get_options ("mask");
  if (opt.size()) {
    mask_path = std::string(opt[0][0]);
    Image::Buffer<bool> in (mask_path);
    if (!Image::dimensions_match (H, in, 0, 3))
      throw Exception ("Input mask image does not match DWI");
    if (!(in.ndim() == 3 || (in.ndim() == 4 && in.dim(3) == 1)))
      throw Exception ("Input mask image must be a 3D image");
    auto v_in = in.voxel();
    Image::copy (v_in, v_mask, 0, 3);
  } else {
    for (auto l = Image::LoopInOrder (v_mask) (v_mask); l; ++l) 
      v_mask.value() = true;
  }

  DWI::CSDeconv<float>::Shared shared (H);

  const size_t lmax = DWI::lmax_for_directions (shared.DW_dirs);
  if (lmax < 4)
    throw Exception ("Cannot run dwi2response with lmax less than 4");
  shared.lmax = lmax;

  Image::BufferPreload<float> dwi (H, Image::Stride::contiguous_along_axis (3));
  DWI::Directions::Set directions (1281);

  Math::Vector<float> response (lmax/2+1);
  response.zero();

  {
    // Initialise response function
    // Use lmax = 2, get the DWI intensity mean and standard deviation within the mask and
    //   use these as the first two coefficients
    auto v_dwi = dwi.voxel();
    double sum = 0.0, sq_sum = 0.0;
    size_t count = 0;
    Image::LoopInOrder loop (dwi, "initialising response function... ", 0, 3);
    for (auto l = loop (v_dwi, v_mask); l; ++l) {
      if (v_mask.value()) {
        for (size_t volume_index = 0; volume_index != shared.dwis.size(); ++volume_index) {
          v_dwi[3] = shared.dwis[volume_index];
          const float value = v_dwi.value();
          sum += value;
          sq_sum += Math::pow2 (value);
          ++count;
        }
      }
    }
    response[0] = sum / double (count);
    response[1] = - 0.5 * std::sqrt ((sq_sum / double(count)) - Math::pow2 (response[0]));
    // Account for scaling in SH basis
    response *= std::sqrt (4.0 * Math::pi);
  }
  INFO ("Initial response function is [" + str(response, 2) + "]");

  // Algorithm termination options
  opt = get_options ("max_iters");
  const size_t max_iters = opt.size() ? int(opt[0][0]) : DWI2RESPONSE_DEFAULT_MAX_ITERS;
  opt = get_options ("max_change");
  const float max_change = 0.01 * (opt.size() ? float(opt[0][0]) : DWI2RESPONSE_DEFAULT_MAX_CHANGE);

  // Should all voxels (potentially within a user-specified mask) be tested at every iteration?
  opt = get_options ("test_all");
  const bool reset_mask = opt.size();

  // Single-fibre voxel selection options
  opt = get_options ("volume_ratio");
  const float volume_ratio = opt.size() ? float(opt[0][0]) : DWI2RESPONSE_DEFAULT_VOLUME_RATIO;
  opt = get_options ("dispersion_multiplier");
  const float dispersion_multiplier = opt.size() ? float(opt[0][0]) : DWI2RESPONSE_DEFAULT_DISPERSION_MULTIPLIER;
  opt = get_options ("integral_multiplier");
  const float integral_multiplier = opt.size() ? float(opt[0][0]) : DWI2RESPONSE_DEFAULT_INTEGRAL_STDEV_MULTIPLIER;

  SFThresholds thresholds (volume_ratio); // Only threshold the lobe volume ratio for now; other two are not yet used

  size_t total_iter = 0;
  bool first_pass = true;
  size_t prev_sf_count = 0;
  {
    bool iterate = true;
    size_t iter = 0;
    ProgressBar progress ("optimising response function... ");
    do {

      ++iter;

      {
        MR::LogLevelLatch latch (0);
        shared.set_response (response);
        shared.init();
//.........这里部分代码省略.........

/*
 * Attitude rates controller.
 * Input: '_rates_sp' vector, '_thrust_sp'
 * Output: '_att_control' vector
 */
void
MulticopterAttitudeControl::control_attitude_rates(float dt)
{
	/* reset integral if disarmed */
	if (!_armed.armed || !_vehicle_status.is_rotary_wing) {
		_rates_int.zero();
	}

	/* get transformation matrix from sensor/board to body frame */
	get_rot_matrix((enum Rotation)_params.board_rotation, &_board_rotation);

	/* fine tune the rotation */
	math::Matrix<3, 3> board_rotation_offset;
	board_rotation_offset.from_euler(M_DEG_TO_RAD_F * _params.board_offset[0],
					 M_DEG_TO_RAD_F * _params.board_offset[1],
					 M_DEG_TO_RAD_F * _params.board_offset[2]);
	_board_rotation = board_rotation_offset * _board_rotation;

	// get the raw gyro data and correct for thermal errors
	math::Vector<3> rates(_sensor_gyro.x * _sensor_correction.gyro_scale[0] + _sensor_correction.gyro_offset[0],
			      _sensor_gyro.y * _sensor_correction.gyro_scale[1] + _sensor_correction.gyro_offset[1],
			      _sensor_gyro.z * _sensor_correction.gyro_scale[2] + _sensor_correction.gyro_offset[2]);

	// rotate corrected measurements from sensor to body frame
	rates = _board_rotation * rates;

	// correct for in-run bias errors
	rates(0) -= _ctrl_state.roll_rate_bias;
	rates(1) -= _ctrl_state.pitch_rate_bias;
	rates(2) -= _ctrl_state.yaw_rate_bias;

	math::Vector<3> rates_p_scaled = _params.rate_p.emult(pid_attenuations(_params.tpa_breakpoint_p, _params.tpa_rate_p));
	math::Vector<3> rates_i_scaled = _params.rate_i.emult(pid_attenuations(_params.tpa_breakpoint_i, _params.tpa_rate_i));
	math::Vector<3> rates_d_scaled = _params.rate_d.emult(pid_attenuations(_params.tpa_breakpoint_d, _params.tpa_rate_d));

	/* angular rates error */
	math::Vector<3> rates_err = _rates_sp - rates;

	_att_control = rates_p_scaled.emult(rates_err) +
		       _rates_int +
		       rates_d_scaled.emult(_rates_prev - rates) / dt +
		       _params.rate_ff.emult(_rates_sp);

	_rates_sp_prev = _rates_sp;
	_rates_prev = rates;

	/* update integral only if motors are providing enough thrust to be effective */
	if (_thrust_sp > MIN_TAKEOFF_THRUST) {
		for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) {
			// Check for positive control saturation
			bool positive_saturation =
				((i == AXIS_INDEX_ROLL) && _saturation_status.flags.roll_pos) ||
				((i == AXIS_INDEX_PITCH) && _saturation_status.flags.pitch_pos) ||
				((i == AXIS_INDEX_YAW) && _saturation_status.flags.yaw_pos);

			// Check for negative control saturation
			bool negative_saturation =
				((i == AXIS_INDEX_ROLL) && _saturation_status.flags.roll_neg) ||
				((i == AXIS_INDEX_PITCH) && _saturation_status.flags.pitch_neg) ||
				((i == AXIS_INDEX_YAW) && _saturation_status.flags.yaw_neg);

			// prevent further positive control saturation
			if (positive_saturation) {
				rates_err(i) = math::min(rates_err(i), 0.0f);

			}

			// prevent further negative control saturation
			if (negative_saturation) {
				rates_err(i) = math::max(rates_err(i), 0.0f);

			}

			// Perform the integration using a first order method and do not propaate the result if out of range or invalid
			float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;

			if (PX4_ISFINITE(rate_i) && rate_i > -_params.rate_int_lim(i) && rate_i < _params.rate_int_lim(i)) {
				_rates_int(i) = rate_i;

			}
		}
	}

	/* explicitly limit the integrator state */
	for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) {
		_rates_int(i) = math::constrain(_rates_int(i), -_params.rate_int_lim(i), _params.rate_int_lim(i));

	}
}