头文件"FlightTaskAuto.hpp"中订阅了position_setpoint_triplet的主题,表示该文件调用了三重目标位置的航点信息。其中,整个_evaluateTriplets()函数都在评估航点信息的有效性、有效性、有效性....以及是否通过和上一帧对比,判读外部传入的航点信息是否被更新。如果航点更新,那么就调用_updateInternalWaypoints()函数规划真正用于位置控制的目标航点信息。其实就是根据当前位置和三个目标航点位置对比,重新排列。贴代码,上图。
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;
函数根据_current_state状态分为了四个case,重点在_current_state如果计算的,该变量是在_getCurrentState()函数中定义的,变量名起得简单粗暴:
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;
在第四节中,更新了当前航点_target的信息,在void FlightTaskAutoMapper类的成员函数_preparePositionSetpoints()中,更新了_position_setpoint。函数中速度设定值_velocity_setpoint为空,因为AUTO模式的目标速度是根据目标航点位置计算的,所以不需要外部输入指令:
void FlightTaskAutoMapper::_preparePositionSetpoints()
{
// Simple waypoint navigation: go to xyz target, with standard limitations
_position_setpoint = _target;
_velocity_setpoint.setNaN(); // No special velocity limitations
}
在“FlightTaskAutoLineSmoothVel”文件中调用_position_setpoint,根据S曲线实时规划的目标位置(每帧都在更新)和航点目标位置的误差更新目标速度_velocity_setpoint,个人认为使用当前的位置反馈求取位置误差也可以:
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.
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 if (z_pos_setpoint_valid) {
// Use Z position setpoint to generate a Z velocity setpoint
// 高度轨迹规划 代码略
}
}
FlightTaskAutoLineSmoothVel的类函数_generateTrajectory()中,调用_velocity_setpoint进行S曲线规划。
六、S曲线在AUTO模式中的应用