bool Copter::rtl_init(bool ignore_checks)
{
if (position_ok() || ignore_checks) {
rtl_build_path(!failsafe.terrain); 如果有地形数据，创建返回路线
rtl_climb_start();1，设置飞机上升点，rtl_state = RTL_InitialClimb; rtl_state_complete = false;给WP控制器的climb target
return true;
}else{
return false;
}
}
Location_Class类 包含点的经纬度和三种高度
rtl_path结构体，描述了返航航迹，包括起始点、上升目标点、返回目标点、下降目标点。
void Copter::rtl_build_path(bool terrain_following_allowed)
{
// origin point is our stopping point
Vector3f stopping_point;
pos_control.get_stopping_point_xy(stopping_point);
pos_control.get_stopping_point_z(stopping_point);
rtl_path.origin_point = Location_Class(stopping_point); 把飞机停止点当做起始点
rtl_path.origin_point.change_alt_frame(Location_Class::ALT_FRAME_ABOVE_HOME);
把起始点高度改成高于HOME点
// compute return target
rtl_compute_return_target(terrain_following_allowed); 计算返回目标点，目标点在HOME点正上方，高度是地形高度。
// climb target is above our origin point at the return altitude
rtl_path.climb_target = Location_Class(rtl_path.origin_point.lat, rtl_path.origin_point.lng, rtl_path.return_target.alt, rtl_path.return_target.get_alt_frame()); 爬升点，在起始点正上方，高度是返回点的高度，（地形高度）
// descent target is below return target at rtl_alt_final
rtl_path.descent_target = Location_Class(rtl_path.return_target.lat, rtl_path.return_target.lng, g.rtl_alt_final, Location_Class::ALT_FRAME_ABOVE_HOME);
下降点，在返回点的正下方，高度是HOME高度
// set land flag
rtl_path.land = g.rtl_alt_final <= 0;
}
// compute the return target - home or rally point
// return altitude in cm above home at which vehicle should return home
// return target's altitude is updated to a higher altitude that the vehicle can safely return at (frame may also be set)
返回点在HOME点的正上方，函数主要是计算返回点高度
void Copter::rtl_compute_return_target(bool terrain_following_allowed)
{
// set return target to nearest rally point or home position (Note: alt is absolute)
#if AC_RALLY == ENABLED
rtl_path.return_target = rally.calc_best_rally_or_home_location(current_loc, ahrs.get_home().alt);
#else
rtl_path.return_target = ahrs.get_home();返回HOME点
#endif
// curr_alt is current altitude above home or above terrain depending upon use_terrain
int32_t curr_alt = current_loc.alt;获得当前点高度，可能是地形高度，可能是高于HOME点高度
// decide if we should use terrain altitudes
rtl_path.terrain_used = terrain_use() && terrain_following_allowed;
if (rtl_path.terrain_used) {如果使用地形高度
// attempt to retrieve terrain alt for current location, stopping point and origin检查当前点、返回点、起始点的地形高度。
int32_t origin_terr_alt, return_target_terr_alt;
if (!rtl_path.origin_point.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, origin_terr_alt) ||获取起始点地形高度
!rtl_path.return_target.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, return_target_terr_alt) || 获取返回点地形高度
!current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, curr_alt)) {获取当前点地形高度
rtl_path.terrain_used = false;万一有一个获取地形高度失败，则false
Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
}
}
// convert return-target alt (which is an absolute alt) to alt-above-home or alt-above-terrain
if (!rtl_path.terrain_used 如果没使用地形高度|| !rtl_path.return_target.change_alt_frame(Location_Class::ALT_FRAME_ABOVE_TERRAIN)) 如果改成地形高度也没有成功{
if (!rtl_path.return_target.change_alt_frame(Location_Class::ALT_FRAME_ABOVE_HOME)) 再一次改成HOME高度，还是没有成功{
// this should never happen but just in case
rtl_path.return_target.set_alt_cm(0, Location_Class::ALT_FRAME_ABOVE_HOME);那就把它改成高于HOME高度且数值为0
}
rtl_path.terrain_used = false;
}
// set new target altitude to return target altitude
// Note: this is alt-above-home or terrain-alt depending upon use_terrain
// Note: ignore negative altitudes which could happen if user enters negative altitude for rally point or terrain is higher at rally point compared to home
int32_t target_alt = MAX(rtl_path.return_target.alt, 0);如果返回点的高度为负值，则让他为0
// increase target to maximum of current altitude + climb_min and rtl altitude
target_alt = MAX(target_alt, curr_alt + MAX(0, g.rtl_climb_min));增加目标点，是当前点加最小上升高度
target_alt = MAX(target_alt, MAX(g.rtl_altitude, RTL_ALT_MIN));2m
// reduce climb if close to return target
float rtl_return_dist_cm = rtl_path.return_target.get_distance(rtl_path.origin_point) * 100.0f;计算起始点到返回点的距离
// don't allow really shallow slopes
if (g.rtl_cone_slope >= RTL_MIN_CONE_SLOPE) {最小的椎体斜坡0.5
target_alt = MAX(curr_alt, MIN(target_alt, MAX(rtl_return_dist_cm*g.rtl_cone_slope, curr_alt+RTL_ABS_MIN_CLIMB)));
}起始点到返回点距离乘以椎体斜坡率，当前高度加2.5m，
// set returned target alt to new target_alt
rtl_path.return_target.set_alt_cm(target_alt, rtl_path.terrain_used ? Location_Class::ALT_FRAME_ABOVE_TERRAIN : Location_Class::ALT_FRAME_ABOVE_HOME);
#if AC_FENCE == ENABLED
// ensure not above fence altitude if alt fence is enabled
// Note: because the rtl_path.climb_target's altitude is simply copied from the return_target's altitude,
// if terrain altitudes are being used, the code below which reduces the return_target's altitude can lead to
// the vehicle not climbing at all as RTL begins. This can be overly conservative and it might be better
// to apply the fence alt limit independently on the origin_point and return_target
if ((fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) {
// get return target as alt-above-home so it can be compared to fence's alt
if (rtl_path.return_target.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, target_alt)) {
float fence_alt = fence.get_safe_alt()*100.0f;
if (target_alt > fence_alt) {
// reduce target alt to the fence alt
rtl_path.return_target.alt -= (target_alt - fence_alt);
}
}
}
#endif
// ensure we do not descend
rtl_path.return_target.alt = MAX(rtl_path.return_target.alt, curr_alt);
}
// rtl_run - runs the return-to-launch controller
// should be called at 100hz or more
void Copter::rtl_run()
{
// check if we need to move to next state
if (rtl_state_complete) { 当前状态是否完成
switch (rtl_state) {
case RTL_InitialClimb:
rtl_return_start();3，rtl_state = RTL_ReturnHome； rtl_state_complete = false。飞机已经上升，该函数主要让Yaw角指向HOME点 给WP控制器的是return target
break;
case RTL_ReturnHome:
rtl_loiterathome_start();5，rtl_state = RTL_LoiterAtHome; rtl_state_complete = false,yaw角调整到起飞时的初始角
break;
case RTL_LoiterAtHome:
if (rtl_path.land || failsafe.radio) {
rtl_land_start(); 7.着陆
}else{
rtl_descent_start();7.下降rtl_state = RTL_FinalDescent;
rtl_state_complete = false;
}
break;
case RTL_FinalDescent:
// do nothing
break; 9.下降结束
case RTL_Land:
// do nothing - rtl_land_run will take care of disarming motors
break; 9.着陆结束
}
}
// call the correct run function
switch (rtl_state) {
case RTL_InitialClimb:
rtl_climb_return_run(); 2，完成climb_target，让飞机上升，rtl_state = RTL_InitialClimb，rtl_state_complete = true
break;
case RTL_ReturnHome:
rtl_climb_return_run();4完成return target，rtl_state = RTL_ReturnHome，rtl_state_complete = true。此时飞机飞到HOME的正上方
break;
case RTL_LoiterAtHome:
rtl_loiterathome_run();6，一直盘旋，调整yaw角
break;
case RTL_FinalDescent:
rtl_descent_run();8.下降
break;
case RTL_Land:
rtl_land_run(); 8.着陆
break;
}
}
void Copter::rtl_climb_start()
{
rtl_state = RTL_InitialClimb;
rtl_state_complete = false;
// initialise waypoint and spline controller
wp_nav.wp_and_spline_init(); 初始化WP控制器
// RTL_SPEED == 0 means use WPNAV_SPEED
if (g.rtl_speed_cms != 0) {
wp_nav.set_speed_xy(g.rtl_speed_cms);设置WP控制器的最大水平速度，没有则用默认5m/s
}
// set the destination
if (!wp_nav.set_wp_destination(rtl_path.climb_target)) {设置目标点
// this should not happen because rtl_build_path will have checked terrain data was available
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
set_mode(LAND, MODE_REASON_TERRAIN_FAILSAFE);
return;
}
wp_nav.set_fast_waypoint(true);目标点为fast wp
// hold current yaw during initial climb
set_auto_yaw_mode(AUTO_YAW_HOLD);在爬升期间yaw角模式设为遥控器控制模式
}
// rtl_climb_return_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller
// called by rtl_run at 100hz or more
void Copter::rtl_climb_return_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
// reset attitude control targets
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// To-Do: re-initialise wpnav targets
return;
}不需要
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.radio) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());获取遥控器给出的yaw角
if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD); 如果遥控器有输出yaw角，设置为遥控器控制yaw角模式
}
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);电机可以全速转动
// run waypoint controller
failsafe_terrain_set_status(wp_nav.update_wpnav()); 运行wp控制器
// call z-axis position controller (wpnav should have already updated it's alt target)
pos_control.update_z_controller();
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); WP控制器输出的roll,pitch。遥控器输出的yaw
}else{
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true, get_smoothing_gain());WP控制器输出的roll,pitch。Yaw不变
}
// check if we've completed this stage of RTL
rtl_state_complete = wp_nav.reached_wp_destination();
}