APM 路径规划笔记

20180711：
类名：AC_WPNav
功能：完成Auto、Loiter、Abmode模式中的路径规划


为计算方便设置默认值，实际值有可能又变化
_wp_accel_cms = 100
_wp_accel_z_cms = 100
_jerk_cmsss = 1700（AC_PosControl）


_wp_speed_cms = 500
_wp_speed_up_cms = 250
_wp_speed_down_cms = 150


_wp_radius_cm = 200


类名：AC_PosControl
功能：位置控制器


通过AC_WPNav给出的数值的计算值
_leash = 100/(2*1*1) + 500*500/(2*100) = 1300
_speed_down_cms = -150
_leash_down_z = 100/(2*1*1) + 150*150/(2*100) = 162.5
_speed_up_cms = 250
_leash_up_z = 100/(2*1*1) + 250*250/(2*100) = 362.5


现假设，P1(300,400,300)点为源点，P2(1200,1600,300)为目的点,P（400,500,300）为飞机当前所在点
_pos_delta_unit = (P2-P1)/|P2-P1| = (0.6,0.8,0)
pos_delta_unit_xy = norm(_pos_delta_unit.x, _pos_delta_unit.y);
pos_delta_unit_z = fabsf(_pos_delta_unit.z);


根据上述数据，算得(AC_WPNav::calculate_wp_leash_length)：
_track_accel = _wp_accel_cms/pos_delta_unit_xy = 100
_track_speed = _wp_speed_cms/pos_delta_unit_xy = 500
_track_leash_length = _leash/pos_delta_unit_xy = 1300


_slow_down_dist = _track_speed * _track_speed / (4.0f*_track_accel) = 625


以update_wpnav()、advance_wp_target_along_track(dt)为例：
curr_delta = P-P1 = (100,100,0)
track_covered = curr_delta · _pos_delta_unit = 140
track_covered_pos = _pos_delta_unit * track_covered = (84,112,0)
track_error = curr_delta - track_covered_pos = (16,-12,0)
track_error_xy = norm(track_error.x, track_error.y) = 20
track_error_z = fabsf(track_error.z) = 0
leash_z = _leash_up_z = 362.5


track_leash_length_abs = fabsf(_track_leash_length) = 1300
track_error_max_abs = MAX(_track_leash_length*track_error_z/leash_z, _track_leash_length*track_error_xy/_pos_control.get_leash_xy()) = 20
track_leash_slack = (track_leash_length_abs > track_error_max_abs) ? safe_sqrt(sq(_track_leash_length) - sq(track_error_max_abs)) : 0 = 1299.84
track_desired_max = track_covered + track_leash_slack = 140+1299.84 = 1439.84




注：
  // get_leash_xy - returns horizontal leash length in cm
  float AC_PosControl :: get_leash_xy() const { return _leash; }


  设置目标点时，设置_limited_speed_xy_cms = constrain_float(speed_along_track,0,_wp_speed_cms);
其中，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;