ardupilot 地形資料觸發RTL模式

目錄

文章目錄

• 目錄
• 摘要
• 1.監控地形資料變化
• 2.設定地形資料的狀态

摘要

本節主要記錄自己分析ardupilot中仿地資料丢失觸發RTL模式的過程。

1.監控地形資料變化

void Copter::three_hz_loop()
{
    // check if we've lost contact with the ground station
	//檢查我們是否與地面站失去聯系
    failsafe_gcs_check();

    //檢測假如我們丢失地形資料---- check if we've lost terrain data
    failsafe_terrain_check();

#if AC_FENCE == ENABLED
    // check if we have breached a fence
    // 檢查我們是否突破了栅欄
    fence_check();
#endif // AC_FENCE_ENABLED

    //打開噴灑使能
#if SPRAYER_ENABLED == ENABLED
//    sprayer.update();
#endif

    update_events();

    // update ch6 in flight tuning
    //更新ch6在飛行曲線
    tuning();
}
           

這部分重點監控地形資料是否有異常，通過函數failsafe_terrain_check();來分析。

void Copter::failsafe_terrain_check()
{
    // trigger with 5 seconds of failures while in AUTO mode
	//在自動模式下觸發5秒失敗
    bool valid_mode = (control_mode == AUTO || control_mode == GUIDED || control_mode == GUIDED_NOGPS || control_mode == RTL);
    //逾時處理：timeout
    bool timeout = (failsafe.terrain_last_failure_ms - failsafe.terrain_first_failure_ms) > FS_TERRAIN_TIMEOUT_MS;
    //當滿足這些模式，并且确實沒有仿地資料了，産生觸發事件
    bool trigger_event = valid_mode && timeout;

    //檢測為清除事件--------check for clearing of event
    if (trigger_event != failsafe.terrain)
    {
        if (trigger_event)
        {
        	//地形資料保護處理
            failsafe_terrain_on_event();
        } else
        {
           //log記錄資訊
            Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_TERRAIN, ERROR_CODE_ERROR_RESOLVED);
            failsafe.terrain = false;
        }
    }
}
           

從這段代碼可以看出要想監控地形資料并且資料有異常需要滿足兩個條件：

1.模式滿足：

AUTO ，GUIDED ，GUIDED_NOGPS ，RTL

2.長時間擷取不到地形資料

由于failsafe.terrain初始是0，valid_mode && timeout同時滿足是1時，導緻trigger_event=1，if (trigger_event != failsafe.terrain)條件滿足，進入if,将會執行函數 failsafe_terrain_on_event();

void Copter::failsafe_terrain_on_event()
{
	//設定地形資料丢失保護變量為1，發送到地面站顯示地形資料丢失
    failsafe.terrain = true;
    gcs().send_text(MAV_SEVERITY_CRITICAL,"Failsafe: Terrain data missing");
    //記錄資料到log中
    Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_TERRAIN, ERROR_CODE_FAILSAFE_OCCURRED);
    //判斷是否沒有解鎖
    if (should_disarm_on_failsafe()) //判斷目前模式是不是解鎖了
    {

        init_disarm_motors();       //上鎖電機
#if MODE_RTL_ENABLED == ENABLED
    } else if (control_mode == RTL)  //目前模式是RTL模式
    {
        mode_rtl.restart_without_terrain();
#endif
    } else                          //設定模式為RTL模式
    {
        set_mode_RTL_or_land_with_pause(MODE_REASON_TERRAIN_FAILSAFE);
    }
}
           

産生以上情況會出現：飛控觸發RTL模式返航，并且地面站收到"Failsafe: Terrain data missing"列印資訊，如果飛機落地後，繼續執行任務，飛機将不會執行，因為飛機目前模式時RTL模式，地面站會提示"Restarting RTL - Terrain data missing"

是以産生根源在于仿地資料沒有擷取，也就是導緻timeout =1；

是以我們需要查找一下這部分代碼

void Copter::failsafe_terrain_set_status(bool data_ok)
{
    uint32_t now = millis();

    // record time of first and latest failures (i.e. duration of failures)
    //記錄第一次和最近一次故障的時間（即故障持續時間）
    if (!data_ok)
    {
        failsafe.terrain_last_failure_ms = now;
        if (failsafe.terrain_first_failure_ms == 0)
        {
            failsafe.terrain_first_failure_ms = now;
        }
    } else
    {
        // failures cleared after 0.1 seconds of persistent successes
    	//持續成功0.1秒後清除失敗
        if (now - failsafe.terrain_last_failure_ms > 100)
        {
            failsafe.terrain_last_failure_ms = 0;
            failsafe.terrain_first_failure_ms = 0;
        }
    }
}
           

這段代碼根據data_ok=0或者1的情況對仿地資料進行5s的監控，如果5s資料都不正常，說明我們的仿地出問題了。

到這裡我們需要查找哪裡調用failsafe_terrain_set_status函數

可以看出調用這裡的主要是圖上代碼，我們需要根據實際情況定位分析問題。我導出自己飛行産生上述問題的日志進行分析我們在哪種模式下會調用上述代碼。

是以我們需要重點分析AUTO模式下的failsafe_terrain_set_status函數。

2.設定地形資料的狀态

分析目标：failsafe_terrain_set_status（）

從飛行日志可以看出，飛機是在起飛點出現RTL模式的

其中auto模式下總共有三個地方使用failsafe_terrain_set_status（）

第一處使用上述代碼

void Copter::ModeAuto::loiter_run()
{
    // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
    if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {
        zero_throttle_and_relax_ac();
        return;
    }

    // accept pilot input of yaw
    float target_yaw_rate = 0;
    if (!copter.failsafe.radio) {
        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
    }

    // set motors to full range
    motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);

    // run waypoint and z-axis position controller
    //第一處使用上述代碼
    copter.failsafe_terrain_set_status(wp_nav->update_wpnav());

    pos_control->update_z_controller();
    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate);
}
           

第二處使用上述代碼

void Copter::ModeAuto::takeoff_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())
    {
        //初始化航點目标----------initialise wpnav targets
        wp_nav->shift_wp_origin_to_current_pos();
        zero_throttle_and_relax_ac();
        //清除積分項，當我們起飛時-----clear i term when we're taking off
        set_throttle_takeoff();
        return;
    }

    //處理飛行偏航輸入-------process pilot's yaw input
    float target_yaw_rate = 0;
    if (!copter.failsafe.radio)
    {
        //擷取飛行器目标偏航速度----get pilot's desired yaw rate
        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
    }

#if FRAME_CONFIG == HELI_FRAME
    // helicopters stay in landed state until rotor speed runup has finished
    if (motors->rotor_runup_complete())
    {
        set_land_complete(false);
    } else
    {
        // initialise wpnav targets
        wp_nav->shift_wp_origin_to_current_pos();
    }
#else
    set_land_complete(false); //設定着落标志
#endif

    //設定電機全速運轉-----set motors to full range
    motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);

    //運作航點控制器-------run waypoint controller
    copter.failsafe_terrain_set_status(wp_nav->update_wpnav());//更新導航算法

    //号召垂直速度控制器----- call z-axis position controller (wpnav should have already updated it's alt target)
    pos_control->update_z_controller();

    //号召姿态控制器--------call attitude controller
    copter.auto_takeoff_attitude_run(target_yaw_rate);
}


           

第三處使用代碼

void Copter::ModeAuto::wp_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())
    {
        // To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off
        //    (of course it would be better if people just used take-off)
    	//将航點原點重置到目前位置，因為無人接可能在地面上，是以我們不希望它在起飛時向左或向右傾斜。當然，如果人們隻是起飛的話會更好）
        zero_throttle_and_relax_ac();
        //清楚我們起飛的時間-------- clear i term when we're taking off
        set_throttle_takeoff();
        return;
    }

    //處理偏航輸入------- process pilot's yaw input
    float target_yaw_rate = 0;
    if (!copter.failsafe.radio) //遙控器沒有丢失
    {
        //獲得飛行員所需的偏航角速度---- get pilot's desired yaw rate
        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
        if (!is_zero(target_yaw_rate))
        {
            auto_yaw.set_mode(AUTO_YAW_HOLD); //沒有的話，設定自動保持
        }
    }

    //将電機設定為全範圍--- set motors to full range
    motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);

    //運作航點控制器---- run waypoint controller
    copter.failsafe_terrain_set_status(wp_nav->update_wpnav());

    // call z-axis position controller (wpnav should have already updated it's alt target)
    //呼叫Z軸位置控制器（wpnav應該已經更新了它的alt目标）
    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(), 0); //把遙控器的偏航輸入取消
//        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate);
    } 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(), auto_yaw.yaw(),true);
    }


}


           

從三處代碼結合日志分析，我們用到的是第三處代碼。因為我的整個過程是APP引導執行：APP引導飛控進行起飛，然後起飛成功進行切換自動模式。從日志分析可以看出出現問題在自動模式下。

//運作航點控制器---- run waypoint controller
    copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
           

最終根源還是在代碼：wp_nav->update_wpnav()中

bool AC_WPNav::update_wpnav()
{
    bool ret = true;

    //從位置控制器擷取時間------ get dt from pos controller
    float dt = _pos_control.get_dt();

    // 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_cmss);
    _pos_control.set_accel_z(_wp_accel_z_cmss);

    //必要時推進目标----- advance the target if necessary
    if (!advance_wp_target_along_track(dt))
    {
        // To-Do: handle inability to advance along track (probably because of missing terrain data)
    	//處理無法沿軌道前進的問題（可能是因為缺少地形資料）
        ret = false;
    }

    // freeze feedforwards during known discontinuities
    //已知不連續時當機前饋
    if (_flags.new_wp_destination)
    {
        _flags.new_wp_destination = false;
        _pos_control.freeze_ff_z();
    }
    //運作位置控制器
    _pos_control.update_xy_controller(1.0f);
    check_wp_leash_length();

    _wp_last_update = AP_HAL::millis();

    return ret;
}
           

隻有下面代碼中advance_wp_target_along_track(dt)傳回0才能保證 ret = false;

//必要時推進目标----- advance the target if necessary
    if (!advance_wp_target_along_track(dt))
    {
        // To-Do: handle inability to advance along track (probably because of missing terrain data)
    	//處理無法沿軌道前進的問題（可能是因為缺少地形資料）
        ret = false;
    }
           

bool AC_WPNav::advance_wp_target_along_track(float dt)
{

	 //車輛沿軌道行駛的距離OH（機關：cm）。從軌道到車輛畫一條垂直線來測量。這裡表示的是線段OH
    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.
    //距軌道覆寫位置（即線路上最接近車輛的點）和車輛的距離誤差（機關：cm）--CH向量
    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
    //單次運作允許的最大目标距離：OM
    float track_desired_max;    // the farthest distance (in cm) along the track that the leash will allow WP控制器本次更新飛機能在OD上跑與O的最遠距離哦,即OMAX
    //WP控制器本次更新飛機能在能在OD上跑的最遠距離，注意是飛機本次跑的距離。即HMAX
    float track_leash_slack;    // additional distance (in cm) along the track from our track_covered position that our leash will allow--
    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是高于起始點高度,terr_offset等于海拔高度減去測距儀測量高度，如果有則傳回true ;
    {
        return false;
    }

    //從線段原點計算三維矢量，這裡表示的是OC-------- calculate 3d vector from segment's origin,注意這個_origin是變化的，到一個航點就更新一次
    Vector3f curr_delta = (curr_pos - Vector3f(0,0,terr_offset)) - _origin; //我們求得curr_pos是基于地理坐标系,Z是海拔高度，這裡我們應該減去地形高度，得到距離當地地面的高度

    //計算我們沿着軌道走了多遠，這裡表示的是線段OH------- calculate how far along the track we are，其中_pos_delta_unit.y表示每個軸在從起點到終點的總軌迹中所占的百分比
    track_covered = curr_delta.x * _pos_delta_unit.x + curr_delta.y * _pos_delta_unit.y + curr_delta.z * _pos_delta_unit.z;

    //計算從起點到終點的路段上最接近車輛的點，這裡轉換成OH向量----- calculate the point closest to the vehicle on the segment from origin to destination
    Vector3f track_covered_pos = _pos_delta_unit * track_covered; //

    //計算從車輛到從起點到終點段上最近點的距離矢量，這裡表示的是CH向量--- calculate the distance vector from the vehicle to the closest point on the segment from origin to destination
    track_error = curr_delta - track_covered_pos;  //計算出誤差x，y,z

    //計算出誤差是HC那麼長----calculate the horizontal error
    _track_error_xy = norm(track_error.x, track_error.y); //_track_error_xy=20

    //計算垂直方向的誤差-----calculate the vertical error
    float track_error_z = fabsf(track_error.z);

    //如果我們向上移動，擷取向上的速度；如果我們向下移動，擷取向下的速度----- get up leash if we are moving up, down leash if we are moving down
    float leash_z = track_error.z >= 0 ? _pos_control.get_leash_up_z() : _pos_control.get_leash_down_z(); //leash_z = _leash_up_z = 362.5

    // use pythagoras's theorem calculate how far along the track we could move the intermediate target before reaching the end of the leash
    //   track_desired_max is the distance from the vehicle to our target point along the track.  It is the "hypotenuse" which we want to be no longer than our leash (aka _track_leash_length)
    //   track_error is the line from the vehicle to the closest point on the track.  It is the "opposite" side
    //   track_leash_slack is the line from the closest point on the track to the target point.  It is the "adjacent" side.  We adjust this so the track_desired_max is no longer than the leash
    //利用畢達哥拉斯定理（勾股定理）計算出在到達牽索末端之前，我們可以沿着軌道移動中間目标的距離。
    //按照目前給定的條件，理論上，無人機在一個循環時間可以跑的距離track_leash_length_abs=1300，這裡可以用CM1來表示
    float track_leash_length_abs = fabsf(_track_leash_length);  //CM1
    //計算軌迹誤差的最大絕對值-----track_error_max_abs=_track_error_xy=20
    float track_error_max_abs = MAX(_track_leash_length*track_error_z/leash_z, _track_leash_length*_track_error_xy/_pos_control.get_leash_xy());
    //計算出HM1的長度
    track_leash_slack = (track_leash_length_abs > track_error_max_abs) ? safe_sqrt(sq(_track_leash_length) - sq(track_error_max_abs)) : 0;
    //這個值就是OM1，表示的是目标長度，這裡假設C目前的位置是虛拟的，是以一般一個周期track_desired_max的值是1300左右
    track_desired_max = track_covered + track_leash_slack;

    //檢查目标是否已經超出控制範圍----- check if target is already beyond the leash
    if (_track_desired > track_desired_max)  //這個——track_desired表示的是一個不斷變化的值，直到達到目标點附近
    {
        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; //假設curr_vel.x=300，curr_vel.y=400

    //計算速度從線性轉換到sqrt的點----- calculate point at which velocity switches from linear to sqrt
    float linear_velocity = _wp_speed_cms; //設定的航點速度linear_velocity=500
    float kP = _pos_control.get_pos_xy_p().kP();
    if (is_positive(kP)) // avoid divide by zero
    {
        linear_velocity = _track_accel/kP; //linear_velocity=100
    }

    //讓限制速度在軌道上的某個範圍内高于或低于目前速度----- let the limited_speed_xy_cms be some range above or below current velocity along track
    if (speed_along_track < -linear_velocity)
    {
        // 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; //v=v0+2at
        }
        // 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);

        //檢查我們是否應該開始減速----- check if we should begin slowing down
        if (!_flags.fast_waypoint) //如果我們忽略航路點半徑，并考慮中間目标到達航點後，航路點完成，則為真。
        {
            float dist_to_dest = _track_length - _track_desired; //每次的位移與期望到達的位置的誤差
            if (!_flags.slowing_down && dist_to_dest <= _slow_down_dist) //_slow_down_dist:一旦車輛在離目的地的這段距離内，就應該開始減速,這個值是全速度運作一周期距離的一半:s/2
            {
                _flags.slowing_down = true;
            }
            //如果目标減速，限制速度----- 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)); //開始進行減速
            }
        }

        // 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); //被限制在400-600之間的速度
        }
    }
    //推進目前目标------ advance the current target
    if (!reached_leash_limit) //當航迹達到系索極限，我們需要減慢目标點時為真。
    {
    	_track_desired += _limited_speed_xy_cms * dt; //目标速度=目前速度+v*t

    	//如果我們到達牽索末端，減速------ reduce speed if we reach end of leash
        if (_track_desired > track_desired_max)
        {
        	_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); //_track_length表示每個周期運作的期望位置長度這個是一個固定值1300
    } else
    {
        _track_desired = constrain_float(_track_desired, 0, _track_length + WPNAV_WP_FAST_OVERSHOOT_MAX);//快速航點允許2米超調，以便順利過渡到下一個航路點。
    }

    //重新計算所需位置------ recalculate the desired position
    Vector3f final_target = _origin + _pos_delta_unit * _track_desired;
    //将最終目标Z轉換為EKF原點以上的高度------ 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 ) //_flags.reached_destination=1,表示到達目的地
    {
        if( _track_desired >= _track_length ) //_track_length這個是我們的總體航線
        {
            // "fast" waypoints are complete once the intermediate point reaches the destination
            if (_flags.fast_waypoint)
            {
                _flags.reached_destination = true; //說明到達航點
            }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; //如果目前點到目标點的距離，在半徑内，說明到達目标點
//                    gcs().send_text(MAV_SEVERITY_INFO, "_flags.reached_destination=%d ",_flags.reached_destination);
                }
            }
        }
    }
    //航點距離必需大于3米
    //使目标偏航更新到下一個航路點的最小航迹長度。在此距離下，偏航目标将被當機在目前航向上
    //如果起點和終點水準距離至少為2米，則更新目标偏航。----- update the target yaw if origin and destination are at least 2m apart horizontally
    if (_track_length_xy >= WPNAV_YAW_DIST_MIN)//将導緻目标偏航更新到下一個航路點的最小航迹長度。在該距離下，偏航目标将在目前航向當機。
    {

        if (_pos_control.get_leash_xy() < WPNAV_YAW_DIST_MIN)//以目前的速度和加速度在一個周期最大可以跑多遠_pos_control.get_leash_xy()
        {

            // if the leash is short (i.e. moving slowly) and destination is at least 2m horizontally, point along the segment from origin to destination
        	//如果牽索較短（即緩慢移動），且目的地水準方向至少為2米，則沿着從起點到終點的段指向。
        	set_yaw_cd(get_bearing_cd(_origin, _destination));

        } else
        {

            Vector3f horiz_leash_xy = final_target - curr_pos; //最終目标需要的水準位置資訊
            //設定垂直方向的變化是0
            horiz_leash_xy.z = 0;
            if (horiz_leash_xy.length() > MIN(WPNAV_YAW_DIST_MIN, _pos_control.get_leash_xy()*WPNAV_YAW_LEASH_PCT_MIN))
            {
            	 // test2=RadiansToCentiDegrees(atan2f(horiz_leash_xy.y,horiz_leash_xy.x));
            	 set_yaw_cd(RadiansToCentiDegrees(atan2f(horiz_leash_xy.y,horiz_leash_xy.x))); //RadiansToCentiDegrees:表示弧度到度
            }

        }
    }

    //成功沿軌道前進------- successfully advanced along track
    return true;
}
           

根源還是下面代碼成立

if (_terrain_alt && !get_terrain_offset(terr_offset)) //_terrain_alt=true目标點和起始點的高度是高于地形高度，false是高于起始點高度,terr_offset等于海拔高度減去測距儀測量高度，如果有則傳回true ;
    {
        return false;
    }
           

是以還是出現地形資料不對才會出現RTL模式