APM程序分析-Control_rtl.cpp

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();

}