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模式