APM程序分析-AC_WPNav.cpp

APM程序分析

1. 主程序在ArduCopter.cpp的loop()函数。

/// advance_wp_target_along_track - move target location along track from origin to destination

设起始点O,目标点D，飞机所在当前点C，OC在OD上的投影为OH。该函数完成沿航迹更新中间目标点。航迹指的是OD。切记，中间目标点都是在OD线段上的。函数0.01s执行一次，即M点0.01s更新一次，两个中间目标点的距离是_limited_speed_cms *0.01，是非常小的。

bool AC_WPNav::advance_wp_target_along_track(float dt)

_track_leash_length  _track_speed  _track_accel是沿航迹OD的最大限制距离、速度、加速度。是该函数上一次更新时计算出来的量。

{

float track_covered;        // distance (in cm) along the track that the vehicle has traveled.  Measured by drawing a perpendicular line from the track to the vehicle.OC在OD的投影OH

Vector3f track_error;       // distance error (in cm) from the track_covered position (i.e. closest point on the line to the vehicle) and the vehicle 向量OC减去向量OD,即CH

float track_desired_max;    // the farthest distance (in cm) along the track that the leash will allow   WP控制器本次更新飞机能在OD上跑与O的最远距离哦,即OMAX

float track_leash_slack;    // additional distance (in cm) along the track from our track_covered position that our leash will allow  WP控制器本次更新飞机能在能在OD上跑的最远距离，注意是飞机本次跑的距离。即HMAX

bool reached_leash_limit = false;   // true when track has reached leash limit and we need to slow down the target point

// get current location

Vector3f curr_pos = _inav.get_position();

// calculate terrain adjustments

float terr_offset = 0.0f;

if (_terrain_alt && !get_terrain_offset(terr_offset)) {_terrain_alt=true目标点和起始点的高度是高于地形高度，false是高于起始点高度。get_terrain_offset(terr_offset) terr_offset等于海拔高度减去测距仪测量高度，如果有则返回true

return false;

}

// calculate 3d vector from segment's origin

Vector3f curr_delta = (curr_pos - Vector3f(0,0,terr_offset)) - _origin;我们求得curr_pos是基于地理坐标系,Z是海拔高度，这里我们应该减去地形高度，得到距离当地地面的高度

// calculate how far along the track we are

track_covered = curr_delta.x * _pos_delta_unit.x + curr_delta.y * _pos_delta_unit.y + curr_delta.z * _pos_delta_unit.z; 线段OH

Vector3f track_covered_pos = _pos_delta_unit * track_covered; 向量OH

track_error = curr_delta - track_covered_pos; 向量CH

// calculate the horizontal error

float track_error_xy = norm(track_error.x, track_error.y);向量CH在oxy水平面的投影

// calculate the vertical error

float track_error_z = fabsf(track_error.z);

// get position control leash lengths

float leash_xy = _pos_control.get_leash_xy();位置控制器在水平方向最大限制距离leash_xy=_speed_cms/kp 或者=0.5_accel_cmss/kp/kp +0.5_speed_cms^2/_accel_cmss

_speed_cms是水平方向最大速度5m/s，accel_cmss是水平方向最大加速度1m/s。我们可以设置他们的大小。Kp是位置控制环的P。

float leash_z;

if (track_error.z >= 0) {

leash_z = _pos_control.get_leash_up_z();

}else{

leash_z = _pos_control.get_leash_down_z();

}飞机向上和向下的加速度、速度不一样，所以位置控制器向上和向下的最大限制距离也不一样，要分开讨论。

// calculate how far along the track we could move the intermediate target before reaching the end of the leash

track_leash_slack = MIN(_track_leash_length*(leash_z-track_error_z)/leash_z, _track_leash_length*(leash_xy-track_error_xy)/leash_xy);如图，track_error是向量CH，track_error越大，说明飞机离航迹OD越远，比如图中蓝色的C,这时我们应该让飞机少飞一点，所以用leash- track_error。_track_leash_length=MIN(leash_z/pos_delta_unit_z, _pos_control.get_leash_xy()/pos_delta_unit_xy)是沿航迹的最大限制距离。这里相当于是根据飞机离航迹的偏差error对_track_leash_length求了一个百分比。比如水平方向上的百分比，（水平方向上最大限制距离-水平方向上的误差）/水平方向上最大限制距离

if (track_leash_slack < 0) {说明偏差比最大限制距离还大，即飞机离航迹很远

track_desired_max = track_covered; 飞机不跑

}else{

track_desired_max = track_covered + track_leash_slack;飞机能飞的最大距离是：沿航迹OD的最大限制距离的百分比。即OMAX

}

// check if target is already beyond the leash

if (_track_desired > track_desired_max) {_track_desired是函数上一次执行计算的飞机应该沿OD飞机距离，函数0.01s执行一次，中间目标点0.01s更新一次。_track_desired += _limited_speed_xy_cms * dt;理论上两次中间目标点的距离是_limited_speed_xy_cms * 0.01s，是非常小的，所以我们可以用上一次的目标点来做这样的判断。所以如果_track_desired > track_desired_max说明本次函数算出来飞行的最大距离还没有上一次算出来应该飞行距离的大。

reached_leash_limit = true;

}

// get current velocity

const Vector3f &curr_vel = _inav.get_velocity();

// get speed along track

float speed_along_track = curr_vel.x * _pos_delta_unit.x + curr_vel.y * _pos_delta_unit.y + curr_vel.z * _pos_delta_unit.z;飞机当前速度在OD上的投影

// calculate point at which velocity switches from linear to sqrt

float linear_velocity = _wp_speed_cms;

float kP = _pos_control.get_pos_xy_kP();

if (kP >= 0.0f) {   // avoid divide by zero

linear_velocity = _track_accel/kP; 沿航迹OD的最大速度

}

// let the limited_speed_xy_cms be some range above or below current velocity along track

if (speed_along_track < -linear_velocity) {飞机当前速度在OD上的投影是负的，说明飞机正沿着OD相反的方向飞行，而且该速度还大于沿航迹的最大速度。

// we are traveling fast in the opposite direction of travel to the waypoint so do not move the intermediate point

_limited_speed_xy_cms = 0;

}else{

// increase intermediate target point's velocity if not yet at the leash limit

if(dt > 0 && !reached_leash_limit) {

_limited_speed_xy_cms += 2.0f * _track_accel * dt; _limited_speed_xy_cms非常重要，是我们认为的飞机沿航迹OD的理论速度。如果飞机还没有到达最终目的地，把_limited_speed_xy_cms增加2at，a是沿航迹的最大加速度。

}

// do not allow speed to be below zero or over top speed

_limited_speed_xy_cms = constrain_float(_limited_speed_xy_cms, 0.0f, _track_speed);不能大于沿航迹OD的最大速度

// check if we should begin slowing down

if (!_flags.fast_waypoint) {

float dist_to_dest = _track_length - _track_desired; _track_length即OD，_track_desired是函数上一次执行计算的飞机应该沿OD飞行距离，即OM^’。两个中间目标点的距离是非常小的，因此可以用上一次的中间目标点做判断。

if (!_flags.slowing_down && dist_to_dest <= _slow_down_dist) {

_flags.slowing_down = true;

}_slow_down_dist = _track_speed * _track_speed / (4.0f*_track_accel) 路程s=v^2/2a，也就是当飞机飞了0.5s时，就该减速了。是上一次计算的。

// if target is slowing down, limit the speed

if (_flags.slowing_down) {

_limited_speed_xy_cms = MIN(_limited_speed_xy_cms, get_slow_down_speed(dist_to_dest, _track_accel));等于4 dist_to_dest* _track_accel开根号

}

}

// if our current velocity is within the linear velocity range limit the intermediate point's velocity to be no more than the linear_velocity above or below our current velocity

if (fabsf(speed_along_track) < linear_velocity) {

_limited_speed_xy_cms = constrain_float(_limited_speed_xy_cms,speed_along_track-linear_velocity,speed_along_track+linear_velocity);再次限幅

}

}

// advance the current target

if (!reached_leash_limit) {

_track_desired += _limited_speed_xy_cms * dt;算OM

// reduce speed if we reach end of leash

if (_track_desired > track_desired_max) { 如果此次算出来的OM大于了OMAX

_track_desired = track_desired_max;

_limited_speed_xy_cms -= 2.0f * _track_accel * dt; 减速

if (_limited_speed_xy_cms < 0.0f) {

_limited_speed_xy_cms = 0.0f;

}

}

}

// do not let desired point go past the end of the track unless it's a fast waypoint

if (!_flags.fast_waypoint) {

_track_desired = constrain_float(_track_desired, 0, _track_length);不要超过OD

} else {

_track_desired = constrain_float(_track_desired, 0, _track_length + WPNAV_WP_FAST_OVERSHOOT_MAX); 可以超过OD 2m

}

// recalculate the desired position

Vector3f final_target = _origin + _pos_delta_unit * _track_desired; 这时候的_track_desired才是我们算出来的真正的OM啦，乘以_pos_delta_unit把它变成向量。注意final_target是一个坐标哦。

// convert final_target.z to altitude above the ekf origin

final_target.z += terr_offset; 加上地面的海拔高度，才是中间目标点真正的地理坐标。

_pos_control.set_pos_target(final_target); 作为期望给位置控制器

// check if we've reached the waypoint

if( !_flags.reached_destination ) {

if( _track_desired >= _track_length ) { OM大于等于OD

// "fast" waypoints are complete once the intermediate point reaches the destination

if (_flags.fast_waypoint) {

_flags.reached_destination = true; 如果是fast_waypoint则认为到达最终目标点

}else{

// regular waypoints also require the copter to be within the waypoint radius

Vector3f dist_to_dest = (curr_pos - Vector3f(0,0,terr_offset)) - _destination;

if( dist_to_dest.length() <= _wp_radius_cm ) {

_flags.reached_destination = true;

}算当前点到目标点的距离，如果在半径内，则认为到达目标点。

}

}

}

// successfully advanced along track

return true;

}

bool AC_WPNav::update_wpnav()

{

// calculate dt

float dt = _pos_control.time_since_last_xy_update();上一次更新XY到现在的时间间隔

bool ret = true;

// update at poscontrol update rate

if (dt >= _pos_control.get_dt_xy()) { dt_xy可以设置，是固定值，如0.02s

// allow the accel and speed values to be set without changing

// out of auto mode. This makes it easier to tune auto flight

_pos_control.set_accel_xy(_wp_accel_cms); wpnav任务期间的水平加速度

_pos_control.set_jerk_xy_to_default();jeck加速度等于17平方米每秒

_pos_control.set_accel_z(_wp_accel_z_cms); wpnav任务期间的垂直加速度

// sanity check dt

if (dt >= 0.2f) {

dt = 0.0f;

}时间间隔不能太长

// advance the target if necessary

if (!advance_wp_target_along_track(dt)) {更新中间点，注意，中间点始终在起始点和目标点的直线上。它调用了函数_pos_control.set_pos_target(final_target);也就是把中间点的坐标给了位置控制器作为位置控制器的期望值。

// To-Do: handle inability to advance along track (probably because of missing terrain data)

ret = false;

}

// freeze feedforwards during known discontinuities

// TODO: why always consider Z axis discontinuous?

if (_flags.new_wp_destination) {位置控制器收到一个新的目标点（期望值）

_flags.new_wp_destination = false;

_pos_control.freeze_ff_xy();有新的目标点则freeze前馈控制

}

_pos_control.freeze_ff_z();freeze速度到加速度环的前馈控制

_pos_control.update_xy_controller(AC_PosControl::XY_MODE_POS_ONLY, 1.0f, false);更新水平控制器，包括位置到速度环pos_to_rate_xy()，速度到加速度环rate_to_accel_xy(),再将加速度转化为期望的欧拉角度accel_to_lean_angles().飞机就会飞向我们设置的中间目标点啦，不断更新中间目标点，这样慢慢我们就会飞到最终目标点咯。

check_wp_leash_length();检查wp_leash_length，_pos_control.update_xy_controller更新后，position controller 的leash lengths可能已经改变了，所以必须调用该函数，来重新调整计算wp_leash_length

_wp_last_update = AP_HAL::millis();

}

return ret;

}

计算waypoint controller 在航迹方向上的leash_length。该leash_length就是pos controller水平方向上的leash_length在航迹方向上的投影和垂直方向上的leash_length在航迹方向上的投影取较小值。同时，该函数还更新了沿航迹的_track_accle _track_speed

void AC_WPNav::calculate_wp_leash_length()

{

// length of the unit direction vector in the horizontal

float pos_delta_unit_xy = norm(_pos_delta_unit.x, _pos_delta_unit.y);

float pos_delta_unit_z = fabsf(_pos_delta_unit.z);

_pos_delta_unit是起点到目标点的方向向量，始终都是固定的。

float speed_z;

float leash_z;

if (_pos_delta_unit.z >= 0.0f) {目标点在当前点上方

speed_z = _wp_speed_up_cms; _wp_speed_up_cms是一个固定值，2.5m/s

leash_z = _pos_control.get_leash_up_z();获取pos controller 的leash z

}else{

speed_z = _wp_speed_down_cms;1.5m/s

leash_z = _pos_control.get_leash_down_z();

}

// calculate the maximum acceleration, maximum velocity, and leash length in the direction of travel计算航迹方向上最大的加速度，速度，leash length

if(is_zero(pos_delta_unit_z) && is_zero(pos_delta_unit_xy)){ 方向向量为0，说明没有目标点

_track_accel = 0;

_track_speed = 0;

_track_leash_length = WPNAV_LEASH_LENGTH_MIN;

}else if(is_zero(_pos_delta_unit.z)){在Z方向为零，说明目标点和当前点在同一高度，飞机只需要水平飞行

_track_accel = _wp_accel_cms/pos_delta_unit_xy; _wp_accel_cms也是固定值，1m/s2,

_track_speed = _wp_speed_cms/pos_delta_unit_xy; _wp_speed_cms是固定值，5m/s

这里说一下，以_wp_开头的加速度，速度都是wp controller控制器的固定值参数。_wp_accel_cms/pos_delta_unit_xy表示把水平方向的加速度投影的航迹上面来。_track_accel是沿航迹的最大加速度。

_track_leash_length = _pos_control.get_leash_xy()/pos_delta_unit_xy;把pos controller获得的水平方向上的leash_length投影的航迹上面来，作为航迹上的最大限制距离leash_length。也就是_track_leash_length是pos controller的leash_length在航迹上的投影。

}else if(is_zero(pos_delta_unit_xy)){目标点恰好在当前点的正上方或者正下方

_track_accel = _wp_accel_z_cms/pos_delta_unit_z; 1m/s2

_track_speed = speed_z/pos_delta_unit_z; 2.5m/s或者1.5m/s

_track_leash_length = leash_z/pos_delta_unit_z;

}else{

_track_accel = MIN(_wp_accel_z_cms/pos_delta_unit_z, _wp_accel_cms/pos_delta_unit_xy);

_track_speed = MIN(speed_z/pos_delta_unit_z, _wp_speed_cms/pos_delta_unit_xy);

_track_leash_length = MIN(leash_z/pos_delta_unit_z, _pos_control.get_leash_xy()/pos_delta_unit_xy);

}

沿航迹的加速度，速度，限制最大距离都取水平方向、垂直方向较小的

// calculate slow down distance (the distance from the destination when the target point should begin to slow down)

calc_slow_down_distance(_track_speed, _track_accel);

_slow_down_dist = _track_speed * _track_speed / (4.0f*_track_accel) 路程s=v^2/2a，也就是当飞机飞了0.5s时，就该减速了。

// set recalc leash flag to false

_flags.recalc_wp_leash = false;

}

从起始点飞到目标点需要的航向角。该航向角是与地理北的夹角哟,即yaw，直接作为姿态环的航向角期望值。

float AC_WPNav::get_bearing_cd(const Vector3f &origin, const Vector3f &destination) const

{

float bearing = 9000 + atan2f(-(destination.x-origin.x), destination.y-origin.y) * 5729.57795f;

if (bearing < 0) {

bearing += 36000;

}

return bearing;

}

WP controller 和spline controller 两个控制器第一次被调用前需要初始化。

void AC_WPNav::wp_and_spline_init()

{

// check _wp_accel_cms is reasonable

if (_wp_accel_cms <= 0) {

_wp_accel_cms.set_and_save(WPNAV_ACCELERATION);

} _wp_accel_cms在执行wp controller任务时的水平最大加速度。1m/s

// also limit the accel using the maximum lean angle. This

// prevents the navigation controller from trying to move the

// target point at an unachievable rate防止飞机已经达到最大倾斜角，依然达不到最大水平加速度

float accel_limit_cms = GRAVITY_MSS * 100 * tanf(radians(_attitude_control.lean_angle_max()*0.01f));

if (_wp_accel_cms > accel_limit_cms) {

_wp_accel_cms.set(accel_limit_cms);

}

accel_limit_cms飞机最大倾斜角时的水平加速度

// initialise position controller

_pos_control.init_xy_controller();

// initialise position controller speed and acceleration

_pos_control.set_speed_xy(_wp_speed_cms);

_pos_control.set_accel_xy(_wp_accel_cms);

_pos_control.set_jerk_xy_to_default();

_pos_control.set_speed_z(-_wp_speed_down_cms, _wp_speed_up_cms);

_pos_control.set_accel_z(_wp_accel_z_cms);

_pos_control.calc_leash_length_xy();

_pos_control.calc_leash_length_z();

}

设置目标点的思路：

1. 我们给出的目标点肯定是经纬度的、高度的话可能是高于地形高度或者海拔高度。我们要计算就要把该目标点转换为基于地理坐标系的坐标。所以我们用目标点的经纬度减去地理坐标系原点的经纬度，得到了坐标x,y。z分了两种，一种是高于地理坐标系原点高度，也就是基于地理坐标系的z；一种是高于地形的，是地面高度，也就是不是基于地理坐标系的。
2. 飞机起始点的设置，这里的起始点是我们WP controller里面的起始点，也就是文章开头图里面的O。O的设置也是两种，一种是WP controller控制器已经运行了，我们就把上一次的目标点设为O，注意此时O的坐标z是高于地形高度，一种是WP controller控制器没运行，我们直接把当前飞机位置设为O，注意此时O坐标是基于地理坐标系的，z是高于原点高度，但它减了origin_terr_offset，它最终还是z为高于地形高度。

也就是，正常情况下，起始点和目标点的水平坐标是基于地理坐标系的，z必须是高于地形高度。如果_terrain_alt=flase，advance_wp_target_along_track(float dt)不会更新的！也就是_terrain_alt=true是WP controller执行的前提！我们可以默认去掉讨论_terrain_alt=flase的情况了，也就认为z是高于地形高度。也就是我们给目标点的时候必须给地形高度！！！！

求目标点到起始点的向量。水平方向上是通过该点减去起始点的经纬度算出了x,y的距离。如果该点是地形高度，则z是该点的地形高度，如果该点不是地形高度，则z是该点高于起始点高度。注意，这里的起始点origin不是wp controller里面的origin，是ekf里面的origin，是地理坐标系原点。

bool AC_WPNav::get_vector_NEU(const Location_Class &loc, Vector3f &vec, bool &terrain_alt)

{

// convert location to NEU vector3f

Vector3f res_vec;

if (!loc.get_vector_xy_from_origin_NEU(res_vec)) {

return false;

}

res_vec.x = (lat-ekf_origin.lat) * LATLON_TO_CM;纬度相减，1纬度对应111km

res_vec.y = (lng-ekf_origin.lng) * LATLON_TO_CM * longitude_scale(ekf_origin);经度相减，1经度对应111*cosa千米。a为起始点所在的纬度，不同纬度1经度对应的距离是不一样的。

EKF融合后输出的是点的经纬度，我们要把它转换为目标点到起始点的具体距离的向量。

// convert altitude

一个点的高度分为高于地形高度、高于HOME高度，高于起始点高度。

if (loc.get_alt_frame() == Location_Class::ALT_FRAME_ABOVE_TERRAIN) {

int32_t terr_alt;

if (!loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, terr_alt)) {

return false;

}

vec.z = terr_alt;

terrain_alt = true;如果loc本身就是高于地形高度，就把高度给vec.z。

} else {

terrain_alt = false;

int32_t temp_alt;

if (!loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_ORIGIN, temp_alt)) {

return false;

}

vec.z = temp_alt;高于起始点高度

terrain_alt = false;

}

// copy xy (we do this to ensure we do not adjust vector unless the overall conversion is successful

vec.x = res_vec.x;

vec.y = res_vec.y;

return true;

}

该函数是把给定目标点D转化为基于地理坐标系坐标，z为高于地面坐标。

bool AC_WPNav::set_wp_destination(const Location_Class& destination)

{

bool terr_alt;

Vector3f dest_neu;

// convert destination location to vector

if (!get_vector_NEU(destination, dest_neu, terr_alt)) {

return false;  dest_neu的坐标x,y基于地理坐标系，z是高于地形高度

}

// set target as vector from EKF origin

return set_wp_destination(dest_neu, terr_alt);

}

该函数主要是算起始点O的坐标。

bool AC_WPNav::set_wp_destination(const Vector3f& destination, bool terrain_alt)

{

Vector3f origin;

// if waypoint controller is active use the existing position target as the origin

if ((AP_HAL::millis() - _wp_last_update) < 1000) {

origin = _pos_control.get_pos_target();  如果WP controller已经运行，让上一次目标点作为起始点O

} else {

// if waypoint controller is not active, set origin to reasonable stopping point (using curr pos and velocity)

_pos_control.get_stopping_point_xy(origin);

_pos_control.get_stopping_point_z(origin); 如果WP controller没有运行，让飞机当前位置推测出来的停止位置作为起始点O

}

// convert origin to alt-above-terrain

if (terrain_alt) {

float origin_terr_offset;

if (!get_terrain_offset(origin_terr_offset)) {

return false;

}

origin.z -= origin_terr_offset; 把z变成高于地形高度

}

// set origin and destination

return set_wp_origin_and_destination(origin, destination, terrain_alt);

}

bool AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vector3f& destination, bool terrain_alt)

{

// store origin and destination locations

_origin = origin;

_destination = destination;

_terrain_alt = terrain_alt;

Vector3f pos_delta = _destination - _origin;

_track_length = pos_delta.length(); // get track length 航迹长度

// calculate each axis' percentage of the total distance to the destination

if (is_zero(_track_length)) {

// avoid possible divide by zero

_pos_delta_unit.x = 0;

_pos_delta_unit.y = 0;

_pos_delta_unit.z = 0;

}else{

_pos_delta_unit = pos_delta/_track_length; 航迹的单位方向向量

}

// calculate leash lengths

calculate_wp_leash_length(); 航迹最大限制距离

// initialise yaw heading

if (_track_length >= WPNAV_YAW_DIST_MIN) { 航迹长度大于2m才改变航向角

_yaw = get_bearing_cd(_origin, _destination); 获取飞机应该的航向角

} else {

// set target yaw to current heading target

_yaw = _attitude_control.get_att_target_euler_cd().z; 不改变航向角

}

// get origin's alt-above-terrain

float origin_terr_offset = 0.0f;

if (terrain_alt) {

if (!get_terrain_offset(origin_terr_offset)) {

return false;  获取起始点的地形距离海拔高度

}

}

// initialise intermediate point to the origin

_pos_control.set_pos_target(origin + Vector3f(0,0,origin_terr_offset));  把起始点O作为期望位置给位置控制器

_track_desired = 0;             // target is at beginning of track

_flags.reached_destination = false;

_flags.fast_waypoint = false;   // default waypoint back to slow

_flags.slowing_down = false;    // target is not slowing down yet

_flags.segment_type = SEGMENT_STRAIGHT;

_flags.new_wp_destination = true;   // flag new waypoint so we can freeze the pos controller's feed forward and smooth the transition  有新的目标点则不使用前馈控制

// initialise the limited speed to current speed along the track

const Vector3f &curr_vel = _inav.get_velocity();

// get speed along track (note: we convert vertical speed into horizontal speed equivalent)

float speed_along_track = curr_vel.x * _pos_delta_unit.x + curr_vel.y * _pos_delta_unit.y + curr_vel.z * _pos_delta_unit.z; 当前速度在航迹上面的投影

_limited_speed_xy_cms = constrain_float(speed_along_track,0,_wp_speed_cms);不能大于航迹最大速度

return true;

}

/// shift_wp_origin_to_current_pos - shifts the origin and destination so the origin starts at the current position 平移起始点和目标点，让起始点在飞机当前位置。

///     used to reset the position just before takeoff

///     relies on set_wp_destination or set_wp_origin_and_destination having been called first

void AC_WPNav::shift_wp_origin_to_current_pos()

{

// return immediately if vehicle is not at the origin

if (_track_desired > 0.0f) {

return;

} 飞机不在起始点则退出

// get current and target locations

const Vector3f curr_pos = _inav.get_position();

const Vector3f pos_target = _pos_control.get_pos_target(); 这是我们当初设置的起始点

// calculate difference between current position and target

Vector3f pos_diff = curr_pos - pos_target;

// shift origin and destination

_origin += pos_diff;

_destination += pos_diff;

// move pos controller target and disable feed forward

_pos_control.set_pos_target(curr_pos);

_pos_control.freeze_ff_xy();

_pos_control.freeze_ff_z();

}

void AC_WPNav::calc_loiter_desired_velocity(float nav_dt, float ekfGndSpdLimit)

{

// calculate a loiter speed limit which is the minimum of the value set by the WPNAV_LOITER_SPEED

// parameter and the value set by the EKF to observe optical flow limits

float gnd_speed_limit_cms = MIN(_loiter_speed_cms,ekfGndSpdLimit*100.0f);悬停最大水平速度，5m/s. ekfGndSpdLimit光流传感器速度不能太快，大概3m/s或者7m/s

gnd_speed_limit_cms = MAX(gnd_speed_limit_cms, 10.0f);要大于0.1m/s，gnd_speed_limit_cms相当于一个综合的最大水平速度。

// range check nav_dt

if( nav_dt < 0 ) {

return;

}

// check loiter speed and avoid divide by zero

if(gnd_speed_limit_cms < WPNAV_LOITER_SPEED_MIN) {

gnd_speed_limit_cms = WPNAV_LOITER_SPEED_MIN; 最小1m/s

}

_pos_control.set_speed_xy(gnd_speed_limit_cms);最大水平速度

_pos_control.set_accel_xy(_loiter_accel_cmss); 最大悬停加速度2.5m/s^2

_pos_control.set_jerk_xy(_loiter_jerk_max_cmsss); _loiter_jerk_max_cmsss加速度变化率10，这个是给pos控制器作为最大水平速度、加速度

// rotate pilot input to lat/lon frame

Vector2f desired_accel;

desired_accel.x = (_pilot_accel_fwd_cms*_ahrs.cos_yaw() - _pilot_accel_rgt_cms*_ahrs.sin_yaw());_pilot_accel_fwd_cms机体坐标系飞机期望向前飞的加速度，注意这个向前飞，是我们摇杆上的前进后退，是机体坐标系X轴在水平面的投影，所以它本质上并不是基于机体坐标系的。pilot_accel_fwd_cms是摇杆对应的输出值。

如上图所示，飞机向前飞的加速度和地理坐标系Xe Ye的加速度关系只跟yaw角有关。

desired_accel.y = (_pilot_accel_fwd_cms*_ahrs.sin_yaw() + _pilot_accel_rgt_cms*_ahrs.cos_yaw());飞机向前飞和向右飞期望加速度转换到地理坐标系下的期望加速度

// calculate the difference

Vector2f des_accel_diff = (desired_accel - _loiter_desired_accel); _loiter_desired_accel地理坐标系下期望悬停加速度

// constrain and scale the desired acceleration

float des_accel_change_total = norm(des_accel_diff.x, des_accel_diff.y);

float accel_change_max = _loiter_jerk_max_cmsss * nav_dt;能改变的最大加速度

if (_loiter_jerk_max_cmsss > 0.0f && des_accel_change_total > accel_change_max && des_accel_change_total > 0.0f) {

des_accel_diff.x = accel_change_max * des_accel_diff.x/des_accel_change_total;

des_accel_diff.y = accel_change_max * des_accel_diff.y/des_accel_change_total;

}当我们期望改变的加速度大于能改变的加速度的最大值时，我们只能取能改变的最大加速度，赋予方向

// adjust the desired acceleration

_loiter_desired_accel += des_accel_diff;

// get pos_control's feed forward velocity

const Vector3f &desired_vel_3d = _pos_control.get_desired_velocity();

Vector2f desired_vel(desired_vel_3d.x,desired_vel_3d.y);

// add pilot commanded acceleration

desired_vel.x += _loiter_desired_accel.x * nav_dt;

desired_vel.y += _loiter_desired_accel.y * nav_dt;

float desired_speed = desired_vel.length();

if (!is_zero(desired_speed)) {

Vector2f desired_vel_norm = desired_vel/desired_speed;

float drag_speed_delta = -_loiter_accel_cmss*nav_dt*desired_speed/gnd_speed_limit_cms;一次时间间隔变化的最大速度取百分比

if (_pilot_accel_fwd_cms == 0 && _pilot_accel_rgt_cms == 0) {

drag_speed_delta = MIN(drag_speed_delta,MAX(-_loiter_accel_min_cmss*nav_dt, -2.0f*desired_speed*nav_dt));

}当遥控器没有输出时，也就是飞机要悬停在当前点时

desired_speed = MAX(desired_speed+drag_speed_delta,0.0f); 适当把速度变小一点

desired_vel = desired_vel_norm*desired_speed;

}

// Apply EKF limit to desired velocity -  this limit is calculated by the EKF and adjusted as required to ensure certain sensor limits are respected (eg optical flow sensing)

float horizSpdDem = sqrtf(sq(desired_vel.x) + sq(desired_vel.y));

if (horizSpdDem > gnd_speed_limit_cms) {

desired_vel.x = desired_vel.x * gnd_speed_limit_cms / horizSpdDem;

desired_vel.y = desired_vel.y * gnd_speed_limit_cms / horizSpdDem;

}速度不能超过最大速度

// Limit the velocity to prevent fence violations

if (_avoid != NULL) {

_avoid->adjust_velocity(_pos_control.get_pos_xy_kP(), _loiter_accel_cmss, desired_vel);

}

// send adjusted feed forward velocity back to position controller

_pos_control.set_desired_velocity_xy(desired_vel.x,desired_vel.y);

}

遥控器打到哪，飞机悬停到哪。摇杆没有值时，就悬停在当前点。过程：通过摇杆值对应的roll角，_pilot_accel_rgt_cms = control_roll * _loiter_accel_cmss / 4500.0f;算出摇杆值期望的加速度，将该加速度转为地理坐标系的期望加速度，用期望加速度算出期望的速度，用期望速度算出期望位置。Pos_control控制该期望位置。当摇杆值没有输出时，飞机就悬停在当前点。

void AC_WPNav::update_loiter(float ekfGndSpdLimit, float ekfNavVelGainScaler)

{

// calculate dt

float dt = _pos_control.time_since_last_xy_update();现在到上次更新水平位置的时间间隔

// run at poscontrol update rate.

// TODO: run on user input to reduce latency, maybe if (user_input || dt >= _pos_control.get_dt_xy())    _dt_xy time difference (in seconds) between update_xy_controller and update_vel_controller_xyz calls 0.02s

if (dt >= _pos_control.get_dt_xy()) {现在到上次更新水平位置的时间间隔大于0.02s，才执行

// sanity check dt

if (dt >= 0.2f) {

dt = 0.0f;

}

// initialise ekf position reset check

check_for_ekf_position_reset();重置xy位置

// initialise pos controller speed and acceleration

_pos_control.set_speed_xy(_loiter_speed_cms);

_pos_control.set_accel_xy(_loiter_accel_cmss);

_pos_control.set_jerk_xy(_loiter_jerk_max_cmsss);

calc_loiter_desired_velocity(dt,ekfGndSpdLimit);

_pos_control.update_xy_controller(AC_PosControl::XY_MODE_POS_LIMITED_AND_VEL_FF, ekfNavVelGainScaler, true);用期望的速度算出pos_target,然后再控制pos_target，XY_MODE_POS_LIMITED_AND_VEL_FF模式，根据期望位置算出来的期望速度不能大于2m/s

}

}

void AC_WPNav::check_for_ekf_position_reset()

{

// check for position shift

Vector2f pos_shift;

uint32_t reset_ms = _ahrs.getLastPosNorthEastReset(pos_shift);返回上一次重置的时间

if (reset_ms != _loiter_ekf_pos_reset_ms) {上一次记录的重置的系统时间

_pos_control.shift_pos_xy_target(pos_shift.x * 100.0f, pos_shift.y * 100.0f);移动xy目标值，此处应该为0

_loiter_ekf_pos_reset_ms = reset_ms;

}

}