struct position_setpoint_triplet_s {
     uint64_t timestamp;
     struct position_setpoint_s previous;
     struct position_setpoint_s current;
     struct position_setpoint_s next;
     //部分略
 };

struct position_setpoint_s {
     uint64_t timestamp;       // 时间戳
     double lat;               // 目标纬度
     double lon;               // 目标经度
     float x;                  // 目标位置 （local坐标系）
     float y;           
     float z;           
     float vx;                 // 目标速度
     float vy;          
     float vz;          
     float loiter_radius;      // 悬停半径
     float a_x;                // 目标加速度
     float a_y;
     float a_z;
     float acceptance_radius;  // 到点判断半径
     float cruising_speed;     // 巡航速度
     // 部分略
 };

uORB::Publication<position_setpoint_triplet_s>	_pos_sp_triplet_pub{ORB_ID(position_setpoint_triplet)};

uORB::SubscriptionData<position_setpoint_triplet_s> _sub_triplet_setpoint{ORB_ID(position_setpoint_triplet)};

void FlightTaskAuto::_updateInternalWaypoints()
{
	// The internal Waypoints might differ from _triplet_prev_wp, _triplet_target and _triplet_next_wp.
	// The cases where it differs:
	// 1. The vehicle already passed the target -> go straight to target
	// 2. The vehicle is more than cruise speed in front of previous waypoint -> go straight to previous waypoint
	// 3. The vehicle is more than cruise speed from track -> go straight to closest point on track
	switch (_current_state) {
	case State::target_behind:
		_target = _triplet_target;
		_prev_wp = _position;
		_next_wp = _triplet_next_wp;
		break;

	case State::previous_infront:
		_next_wp = _triplet_target;
		_target = _triplet_prev_wp;
		_prev_wp = _position;
		break;

	case State::offtrack:
		_next_wp = _triplet_target;
		_target = matrix::Vector3f(_closest_pt(0), _closest_pt(1), _triplet_target(2));
		_prev_wp = _position;
		break;

	case State::none:
		_target = _triplet_target;
		_prev_wp = _triplet_prev_wp;
		_next_wp = _triplet_next_wp;
		break;

	default:
		break;

	}
}

State FlightTaskAuto::_getCurrentState()
{
	// Calculate the vehicle current state based on the Navigator triplets and the current position.
	Vector2f u_prev_to_target = Vector2f(_triplet_target - _triplet_prev_wp).unit_or_zero();
	Vector2f pos_to_target(_triplet_target - _position);
	Vector2f prev_to_pos(_position - _triplet_prev_wp);
	// Calculate the closest point to the vehicle position on the line prev_wp - target
	_closest_pt = Vector2f(_triplet_prev_wp) + u_prev_to_target * (prev_to_pos * u_prev_to_target);

	State return_state = State::none;

	if (u_prev_to_target * pos_to_target < 0.0f) {
		// Target is behind.
		return_state = State::target_behind;

	} else if (u_prev_to_target * prev_to_pos < 0.0f && prev_to_pos.length() > _mc_cruise_speed) {
		// Current position is more than cruise speed in front of previous setpoint.
		return_state = State::previous_infront;

	} else if (Vector2f(Vector2f(_position) - _closest_pt).length() > _mc_cruise_speed) {
		// Vehicle is more than cruise speed off track.
		return_state = State::offtrack;

	}

	return return_state;
}

void FlightTaskAutoMapper::_preparePositionSetpoints()
{
	// Simple waypoint navigation: go to xyz target, with standard limitations
	_position_setpoint = _target;
	_velocity_setpoint.setNaN(); // No special velocity limitations
}

void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
{
	// Interface: A valid position setpoint generates a velocity target using conservative motion constraints.
	// If a velocity is specified, that is used as a feedforward to track the position setpoint
	// (ie. it assumes the position setpoint is moving at the specified velocity)
	// If the position setpoints are set to NAN, the values in the velocity setpoints are used as velocity targets: nothing to do here.

		const bool xy_pos_setpoint_valid = PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1));
		const bool z_pos_setpoint_valid = PX4_ISFINITE(_position_setpoint(2));

	if (xy_pos_setpoint_valid && z_pos_setpoint_valid) {
		// Use 3D position setpoint to generate a 3D velocity setpoint
		// 三维轨迹规划 代码略
	}

	else if (xy_pos_setpoint_valid) {
		// Use 2D position setpoint to generate a 2D velocity setpoint

		// Get various path specific vectors
		Vector2f pos_traj(_trajectory[0].getCurrentPosition(), _trajectory[1].getCurrentPosition());
		Vector2f pos_traj_to_dest_xy = Vector2f(_position_setpoint.xy()) - pos_traj;
		Vector2f u_pos_traj_to_dest_xy(pos_traj_to_dest_xy.unit_or_zero());

		Vector2f vel_sp_constrained_xy = u_pos_traj_to_dest_xy * _getMaxXYSpeed();

		for (int i = 0; i < 2; i++) {
			// If available, use the existing velocity as a feedforward, otherwise replace it
			if (PX4_ISFINITE(_velocity_setpoint(i))) {
				_velocity_setpoint(i) += vel_sp_constrained_xy(i);

			} else {
				_velocity_setpoint(i) = vel_sp_constrained_xy(i);
			}
		}
	}

	else if (z_pos_setpoint_valid) {
		// Use Z position setpoint to generate a Z velocity setpoint
		// 高度轨迹规划 代码略
	}
}