Ardupilot开源飞控之AP_Follow

1. 源由

前面在《Ardupilot开源飞控之FollowMe计划》最初“【无障碍物】主动FollowMe功能(采用GPS) ”就是在这部分实现的。

那么我们研读下这部分代码。

2. 定义

关于模型模式的相关框架，详见：ArduPilot开源飞控之飞行模式。注：ArduRover和ArduCopter类似，可以相互借鉴。

2.1 ModeFollow类

ModeFollow
├── 继承自 Mode
├── 公有成员函数 (public)
│   ├── mode_number() const
│   ├── name4() const
│   ├── update() override
│   ├── is_autopilot_mode() const
│   ├── wp_bearing() const
│   ├── nav_bearing() const
│   ├── crosstrack_error() const
│   ├── get_desired_location(Location& destination) const WARN_IF_UNUSED
│   ├── get_distance_to_destination() const
│   └── set_desired_speed(float speed)
└── 保护成员函数与变量 (protected)
    ├── _enter() override
    ├── _exit() override
    └── _desired_speed

1. 类定义

   • class ModeFollow : public Mode
     • ModeFollow 类继承自 Mode 类。

2. 公有成员函数（public）

   • Number mode_number() const override
     • 返回模式编号，具体为 Number::FOLLOW。
   • const char *name4() const override
     • 返回模式名称，为 “FOLL”。
   • void update() override
     • 更新车辆状态的方法。
   • bool is_autopilot_mode() const override
     • 返回是否为自动驾驶模式，这里返回 true。
   • float wp_bearing() const override
     • 返回导航点的航向。
   • float nav_bearing() const override
     • 返回导航航向，这里调用 wp_bearing() 方法。
   • float crosstrack_error() const override
     • 返回偏航误差，这里固定返回 0.0f。
   • bool get_desired_location(Location& destination) const override WARN_IF_UNUSED
     • 返回期望的位置，这里固定返回 false。
   • float get_distance_to_destination() const override
     • 返回到目的地的距离。
   • bool set_desired_speed(float speed) override
     • 设置期望的速度。

3. 保护成员函数（protected）

   • bool _enter() override
     • 进入模式时的操作。
   • void _exit() override
     • 退出模式时的操作。

4. 保护成员变量（protected）

   • float _desired_speed
     • 期望速度，单位为 m/s。

2.1.1 ModeFollow::update

模式更新函数，定时轮训。

SCHED_TASK(update_current_mode,   400,    200,  12),
 └──> Rover::update_current_mode
     └──> control_mode->update
         └──> ModeFollow::update

主要分为以下几个步骤：

1. 声明需要的变量。
2. 获取并检查当前车辆的速度，如果无法获取有效速度则停止车辆。
3. 获取目标车辆的距离和速度信息，如果无法获取则停止车辆。
4. 计算期望速度向量。
5. 检查期望速度是否为零，如果为零则停止车辆。
6. 设置未到达目的地状态。
7. 调整期望速度以保持在预设的速度限制内。
8. 计算车辆的期望航向。
9. 根据期望航向和速度控制车辆的转向和油门。

ModeFollow::update()
├── 声明变量
│   ├── float speed;
│   ├── Vector3f dist_vec;
│   ├── Vector3f dist_vec_offs;
│   ├── Vector3f vel_of_target;
│   ├── Vector2f desired_velocity_ne;
│   └── float desired_speed;
|
├── 获取车辆速度并检查
│   ├── if (!attitude_control.get_forward_speed(speed))
│   │   ├── g2.motors.set_throttle(0.0f);
│   │   ├── g2.motors.set_steering(0.0f);
│   │   └── return;
|
├── 获取目标距离和速度并检查
│   ├── if (!g2.follow.get_target_dist_and_vel_ned(dist_vec, dist_vec_offs, vel_of_target))
│   │   ├── _reached_destination = true;
│   │   ├── stop_vehicle();
│   │   └── return;
|
├── 计算期望速度向量
│   ├── const float kp = g2.follow.get_pos_p().kP();
│   ├── desired_velocity_ne.x = vel_of_target.x + (dist_vec_offs.x * kp);
│   ├── desired_velocity_ne.y = vel_of_target.y + (dist_vec_offs.y * kp);
|
├── 检查期望速度是否为零
│   ├── if (is_zero(desired_velocity_ne.x) && is_zero(desired_velocity_ne.y))
│   │   ├── _reached_destination = true;
│   │   ├── stop_vehicle();
│   │   └── return;
|
├── 设置未到达目的地状态
│   ├── _reached_destination = false;
|
├── 缩放期望速度以保持在水平速度限制内
│   ├── desired_speed = safe_sqrt(sq(desired_velocity_ne.x) + sq(desired_velocity_ne.y));
│   ├── if (!is_zero(desired_speed) && (desired_speed > _desired_speed))
│   │   ├── const float scalar_xy = _desired_speed / desired_speed;
│   │   ├── desired_velocity_ne *= scalar_xy;
│   │   └── desired_speed = _desired_speed;
|
├── 计算车辆航向
│   ├── const float desired_yaw_cd = wrap_180_cd(atan2f(desired_velocity_ne.y, desired_velocity_ne.x) * DEGX100);
|
├── 运行转向和油门控制器
│   ├── calc_steering_to_heading(desired_yaw_cd);
│   └── calc_throttle(desired_speed, true);

2.1.2 ModeFollow::_enter

进入模式，执行的初始化函数。

Rover::set_mode
 └──> Mode::enter
     └──> ModeFollow::_enter

判断跟随模式是否使能，使能情况下初始化期望速度。

// initialize follow mode
bool ModeFollow::_enter()
{
    if (!g2.follow.enabled()) {
        return false;
    }

    // initialise speed to waypoint speed
    _desired_speed = g2.wp_nav.get_default_speed();

    return true;
}

2.1.3 ModeFollow::_exit

退出模式，执行的“清场”函数。

Rover::set_mode
 └──> Mode::exit
     └──> ModeFollow::_exit

清场处理。

// exit handling
void ModeFollow::_exit()
{
    g2.follow.clear_offsets_if_required();
}

2.2 AP_Follow类

AP_Follow 类用于管理无人机的跟随模式，包含目标位置追踪、偏移处理、航向控制等功能。

AP_Follow
├── 枚举类型
│   ├── Option
│   └── YawBehave
├── 构造函数
│   └── AP_Follow()
├── 静态方法
│   └── get_singleton()
├── 公有方法
│   ├── enabled() const
│   ├── set_target_sysid(uint8_t sysid)
│   ├── clear_offsets_if_required()
│   ├── have_target() const
│   ├── get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const
│   ├── get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned) const
│   ├── get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_with_ofs, Vector3f &vel_ned)
│   ├── get_target_sysid() const
│   ├── get_pos_p() const
│   ├── get_yaw_behave() const
│   ├── get_target_heading_deg(float &heading) const
│   ├── handle_msg(const mavlink_message_t &msg)
│   ├── get_distance_to_target() const
│   ├── get_bearing_to_target() const
│   ├── get_last_update_ms() const
│   ├── option_is_enabled(Option option) const
│   └── var_info[]
├── 静态成员
│   └── _singleton
├── 私有方法
│   ├── get_velocity_ned(Vector3f &vel_ned, float dt) const
│   ├── init_offsets_if_required(const Vector3f &dist_vec_ned)
│   ├── get_offsets_ned(Vector3f &offsets) const
│   ├── rotate_vector(const Vector3f &vec, float angle_deg) const
│   └── clear_dist_and_bearing_to_target()
└── 私有成员变量
    ├── _enabled
    ├── _sysid
    ├── _dist_max
    ├── _offset_type
    ├── _offset
    ├── _yaw_behave
    ├── _alt_type
    ├── _p_pos
    ├── _options
    ├── _last_location_update_ms
    ├── _target_location
    ├── _target_velocity_ned
    ├── _target_accel_ned
    ├── _last_heading_update_ms
    ├── _target_heading
    ├── _automatic_sysid
    ├── _dist_to_target
    ├── _bearing_to_target
    └── _offsets_were_zero

1. 公有成员

• 枚举类型 Option 和 YawBehave

  • Option 枚举：定义跟随选项参数。
  • YawBehave 枚举：定义航向行为参数。

• 构造函数

  • AP_Follow(): 类的构造函数。

• 静态方法

  • get_singleton(): 返回单例对象。

• 公有方法

  • enabled() const: 返回库是否启用。
  • set_target_sysid(uint8_t sysid): 设置跟随目标的系统ID。
  • clear_offsets_if_required(): 如有必要，重置偏移量。
  • have_target() const: 返回是否有有效的目标位置估计。
  • get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const: 获取目标位置和速度。
  • get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned) const: 获取目标位置和速度（包含偏移）。
  • get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_with_ofs, Vector3f &vel_ned): 获取到目标的距离和速度。
  • get_target_sysid() const: 获取目标系统ID。
  • get_pos_p() const: 获取位置控制器。
  • get_yaw_behave() const: 获取用户定义的航向行为。
  • get_target_heading_deg(float &heading) const: 获取目标航向。
  • handle_msg(const mavlink_message_t &msg): 解析包含目标位置、速度和姿态的MAVLink消息。
  • get_distance_to_target() const: 获取到目标的水平距离。
  • get_bearing_to_target() const: 获取到目标的方位。
  • get_last_update_ms() const: 获取最后一次位置更新的系统时间。
  • option_is_enabled(Option option) const: 返回是否启用某个跟随选项。
  • var_info[]: 参数列表。

2. 私有成员

• 静态成员

  • _singleton: 单例对象指针。

• 私有方法

  • get_velocity_ned(Vector3f &vel_ned, float dt) const: 获取NED坐标系中的速度估计。
  • init_offsets_if_required(const Vector3f &dist_vec_ned): 初始化偏移量。
  • get_offsets_ned(Vector3f &offsets) const: 获取NED坐标系中的偏移量。
  • rotate_vector(const Vector3f &vec, float angle_deg) const: 顺时针旋转3D向量。
  • clear_dist_and_bearing_to_target(): 重置到目标的距离和方位。

• 私有成员变量

  • _enabled: 是否启用该子系统。
  • _sysid: 目标的MAVLink系统ID。
  • _dist_max: 到目标的最大距离，超出此距离的目标将被忽略。
  • _offset_type: 偏移坐标系类型。
  • _offset: 与目标的偏移量。
  • _yaw_behave: 跟随车辆的航向行为。
  • _alt_type: 跟随模式下的高度源。
  • _p_pos: 位置误差P控制器。
  • _options: 跟随模式的选项。
  • _last_location_update_ms: 最后一次位置更新的系统时间。
  • _target_location: 目标的最后已知位置。
  • _target_velocity_ned: 目标在NED坐标系中的速度。
  • _target_accel_ned: 目标在NED坐标系中的加速度。
  • _last_heading_update_ms: 最后一次航向更新的系统时间。
  • _target_heading: 目标的航向。
  • _automatic_sysid: 是否自动锁定系统ID。
  • _dist_to_target: 到目标的最新距离。
  • _bearing_to_target: 到目标的最新方位。
  • _offsets_were_zero: 偏移量是否最初为零然后初始化为与目标的偏移量。
  • _jitter: 抖动校正器，最大传输延迟为3秒。

2.2.1 AP_Follow::handle_msg

AP_Follow::handle_msg(const mavlink_message_t &msg)
|
|-- 初步检查
|   |-- if (!_enabled)  //未使能`AP_Follow`类
|   |   |-- return;
|   |
|   |-- if (msg.sysid == mavlink_system.sysid)  //自身MAVLink消息忽略
|   |   |-- return;
|   |
|   |-- if (_sysid != 0 && msg.sysid != _sysid)  //非跟踪目标MAVLink消息忽略
|       |-- if (_automatic_sysid)  // 使能超时自动重置跟踪目标
|           |-- if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_SYSID_TIMEOUT_MS))
|               |-- _sysid.set(0);
|       |-- return;
|
|-- 消息解析
|   |-- bool updated = false;
|
|   |-- switch (msg.msgid)
|       |
|       |-- case MAVLINK_MSG_ID_GLOBAL_POSITION_INT
|       |   |-- mavlink_global_position_int_t packet;
|       |   |-- mavlink_msg_global_position_int_decode(&msg, &packet);
|       |
|       |   |-- if ((packet.lat == 0 && packet.lon == 0))
|       |       |-- return;
|       |
|       |   |-- _target_location.lat = packet.lat;
|       |   |-- _target_location.lng = packet.lon;
|       |
|       |   |-- if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE)
|       |       |-- _target_location.set_alt_cm(packet.relative_alt / 10, Location::AltFrame::ABOVE_HOME);
|       |   |   else
|       |       |-- _target_location.set_alt_cm(packet.alt / 10, Location::AltFrame::ABSOLUTE);
|       |
|       |   |-- _target_velocity_ned.x = packet.vx * 0.01f;
|       |   |-- _target_velocity_ned.y = packet.vy * 0.01f;
|       |   |-- _target_velocity_ned.z = packet.vz * 0.01f;
|       |
|       |   |-- _last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.time_boot_ms, AP_HAL::millis());
|       |   |-- if (packet.hdg <= 36000)
|       |       |-- _target_heading = packet.hdg * 0.01f;
|       |       |-- _last_heading_update_ms = _last_location_update_ms;
|       |
|       |   |-- if (_sysid == 0)
|       |       |-- _sysid.set(msg.sysid);
|       |       |-- _automatic_sysid = true;
|       |
|       |   |-- updated = true;
|       |   |-- break;
|
|       |-- case MAVLINK_MSG_ID_FOLLOW_TARGET
|       |   |-- mavlink_follow_target_t packet;
|       |   |-- mavlink_msg_follow_target_decode(&msg, &packet);
|       |
|       |   |-- if ((packet.lat == 0 && packet.lon == 0))
|       |       |-- return;
|       |
|       |   |-- if ((packet.est_capabilities & (1<<0)) == 0)
|       |       |-- return;
|       |
|       |   |-- Location new_loc = _target_location;
|       |   |-- new_loc.lat = packet.lat;
|       |   |-- new_loc.lng = packet.lon;
|       |   |-- new_loc.set_alt_cm(packet.alt * 100, Location::AltFrame::ABSOLUTE);
|       |
|       |   |-- if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE && !new_loc.change_alt_frame(Location::AltFrame::ABOVE_HOME))
|       |       |-- return;
|       |
|       |   |-- _target_location = new_loc;
|       |
|       |   |-- if (packet.est_capabilities & (1<<1))
|       |       |-- _target_velocity_ned.x = packet.vel[0];
|       |       |-- _target_velocity_ned.y = packet.vel[1];
|       |       |-- _target_velocity_ned.z = packet.vel[2];
|       |   |   else
|       |       |-- _target_velocity_ned.zero();
|       |
|       |   |-- _last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.timestamp, AP_HAL::millis());
|       |
|       |   |-- if (packet.est_capabilities & (1<<3))
|       |       |-- Quaternion q{packet.attitude_q[0], packet.attitude_q[1], packet.attitude_q[2], packet.attitude_q[3]};
|       |       |-- float r, p, y;
|       |       |-- q.to_euler(r, p, y);
|       |       |-- _target_heading = degrees(y);
|       |       |-- _last_heading_update_ms = _last_location_update_ms;
|       |
|       |   |-- if (_sysid == 0)
|       |       |-- _sysid.set(msg.sysid);
|       |       |-- _automatic_sysid = true;
|       |
|       |   |-- updated = true;
|       |   |-- break;
|
|-- if (updated)
|   |-- #if HAL_LOGGING_ENABLED
|       |-- Location loc_estimate{};
|       |-- Vector3f vel_estimate;
|       |-- UNUSED_RESULT(get_target_location_and_velocity(loc_estimate, vel_estimate));
|       |
|       |-- AP::logger().WriteStreaming("FOLL",
|                                       "TimeUS,Lat,Lon,Alt,VelN,VelE,VelD,LatE,LonE,AltE",  // labels
|                                       "sDUmnnnDUm",    // units
|                                       "F--B000--B",    // mults
|                                       "QLLifffLLi",    // fmt
|                                       AP_HAL::micros64(),
|                                       _target_location.lat,
|                                       _target_location.lng,
|                                       _target_location.alt,
|                                       (double)_target_velocity_ned.x,
|                                       (double)_target_velocity_ned.y,
|                                       (double)_target_velocity_ned.z,
|                                       loc_estimate.lat,
|                                       loc_estimate.lng,
|                                       loc_estimate.alt);
|   |-- #endif

• 初步检查：

  • 启用检查：如果未启用AP_Follow类，则直接返回。
  • 跳过自有消息：如果消息来自当前系统（即自反馈消息），则跳过。
  • 目标系统检查：如果消息不来自当前目标系统且不是自动系统ID，则跳过，并在必要时重置系统ID。

• 消息解析：

  • 消息类型判断：根据消息的msgid，分别处理不同类型的MAVLink消息：
    • MAVLINK_MSG_ID_GLOBAL_POSITION_INT：处理全局位置消息，更新目标的经纬度、高度、速度和航向信息。如果消息无效（经纬度为零），则忽略消息。
    • MAVLINK_MSG_ID_FOLLOW_TARGET：处理跟踪目标消息，更新目标的经纬度、高度、速度和航向信息。如果消息无效（经纬度为零或不包含位置数据），则忽略消息。

• 更新检查和日志记录：

  • 如果目标数据被更新，则在启用日志记录时记录目标的估计位置和速度，以及车辆的当前位置和速度。

该方法通过处理不同类型的MAVLink消息来跟踪目标的实时位置和运动信息，并在需要时进行日志记录，以确保目标跟踪的精度和可靠性。

2.2.2 AP_Follow::get_target_location_and_velocity

get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) -> bool
└── 判断是否启用 (_enabled)
    ├── 否：立即返回 false
    └── 是：继续
        └── 检查是否超时
            ├── 是：立即返回 false
            └── 否：继续
                └── 计算自上次位置更新后的时间差 (dt)
                    └── 获取速度估算 (get_velocity_ned)
                        ├── 失败：返回 false
                        └── 成功：继续
                            └── 投影车辆位置 (_target_location)
                                ├── 使用速度 (vel_ned.x 和 vel_ned.y) 更新水平位置
                                └── 使用速度 (vel_ned.z) 更新垂直位置
                                    └── 更新后的位置信息 (last_loc) 赋值给 loc
                                        └── 返回 true

1. 判断是否启用：

   • 检查 _enabled 是否为 true。
   • 如果 _enabled 为 false，函数立即返回 false。

2. 检查是否超时：

   • 检查 _last_location_update_ms 是否为 0，或当前时间与 _last_location_update_ms 的差值是否超过 AP_FOLLOW_TIMEOUT_MS。
   • 如果是超时条件，则函数立即返回 false。

3. 计算时间差 (dt)：

   • 计算自上次位置更新以来的时间差，单位为秒。

4. 获取速度估算：

   • 调用 get_velocity_ned(vel_ned, dt) 获取速度估算值。
   • 如果获取失败，函数返回 false。

5. 投影车辆位置：

   • 根据当前速度估算值和时间差，更新目标位置。
   • 水平方向：vel_ned.x * dt 和 vel_ned.y * dt 用于更新水平位置。
   • 垂直方向：vel_ned.z * 100.0f * dt 用于更新垂直位置（速度单位转换为 cm/s，并乘以时间差）。
   • 更新后的目标位置赋值给 loc。

6. 返回最新的位置信息：

   • 函数成功执行完毕，返回 true。

2.2.3 AP_Follow::get_velocity_ned

AP_Follow: Calculate acceleration target #20922

// get velocity estimate in m/s in NED frame using dt since last update
bool AP_Follow::get_velocity_ned(Vector3f &vel_ned, float dt) const
{
    vel_ned = _target_velocity_ned + (_target_accel_ned * dt);
    return true;
}

3. 其他函数

3.1 JitterCorrection::correct_offboard_timestamp_msec

JitterCorrection::correct_offboard_timestamp_msec
 └──> correct_offboard_timestamp_usec

correct_offboard_timestamp_usec(uint64_t offboard_usec, uint64_t local_usec)
|
|-- 计算本地时间与外部时间的差异
|   |
|   |-- int64_t diff_us = int64_t(local_usec) - int64_t(offboard_usec);
|
|-- 初次初始化或时间差异超前的处理
|   |
|   |-- if (!initialised || diff_us < link_offset_usec)
|       |
|       |-- 设置 link_offset_usec 为当前差异
|       |-- link_offset_usec = diff_us;
|       |-- 标记为已初始化
|       |-- initialised = true;
|
|-- 估算校正后的外部时间
|   |
|   |-- int64_t estimate_us = offboard_usec + link_offset_usec;
|
|-- 检查消息是否太过于滞后
|   |
|   |-- if (estimate_us + (max_lag_ms*1000U) < int64_t(local_usec))
|       |
|       |-- 将估算时间调整为最大滞后时间
|       |-- estimate_us = local_usec - (max_lag_ms*1000U);
|       |-- 更新 link_offset_usec
|       |-- link_offset_usec = estimate_us - offboard_usec;
|
|-- 最小样本处理
|   |
|   |-- if (min_sample_counter == 0)
|       |
|       |-- 初始化 min_sample_us
|       |-- min_sample_us = diff_us;
|
|   |-- 增加样本计数器
|   |   |-- min_sample_counter++;
|
|   |-- 如果当前差异小于最小样本，更新最小样本
|   |   |-- if (diff_us < min_sample_us)
|           |-- min_sample_us = diff_us;
|
|   |-- 达到收敛循环次数后，更新 link_offset_usec
|   |   |-- if (min_sample_counter == convergence_loops)
|           |
|           |-- 更新 link_offset_usec 为最小样本
|           |-- link_offset_usec = min_sample_us;
|           |-- 重置样本计数器
|           |-- min_sample_counter = 0;
|
|-- 返回校正后的时间
|   |-- return uint64_t(estimate_us);

1. 计算时间差异:

   • diff_us 计算本地时间 local_usec 和外部时间 offboard_usec 之间的差异。

2. 初次初始化或时间差异超前的处理:

   • 如果还未初始化或时间差异小于 link_offset_usec，则更新 link_offset_usec 并标记为已初始化。这部分处理外部时间戳过于超前的情况。

3. 估算校正后的外部时间:

   • 使用外部时间 offboard_usec 加上 link_offset_usec 来估算校正后的时间 estimate_us。

4. 检查消息是否太过于滞后:

   • 如果估算的时间加上最大滞后时间小于本地时间，说明消息太过滞后，需要将 estimate_us 调整为本地时间减去最大滞后时间，并更新 link_offset_usec。

5. 最小样本处理:

   • 用于记录和更新传输延迟的最小样本，逐步收敛传输延迟的估算值。
   • 如果样本计数器为 0，初始化最小样本 min_sample_us。
   • 增加样本计数器 min_sample_counter。
   • 如果当前差异 diff_us 小于记录的最小样本 min_sample_us，则更新最小样本。
   • 如果样本计数器达到收敛循环次数 convergence_loops，则更新 link_offset_usec 为最小样本，并重置样本计数器。

6. 返回校正后的时间:

   • 最后返回校正后的时间 estimate_us。

通过这种树形结构，可以清晰地看到函数的每个步骤和逻辑分支的关系，更加容易理解整个函数的工作流程。

3.2 AR_AttitudeControl::get_forward_speed

AR_AttitudeControl::get_forward_speed(float &speed) const
├── 声明局部变量：Vector3f velocity;
├── 获取姿态航向参考：const AP_AHRS &_ahrs = AP::ahrs();
├── 检查是否成功获取NED（北东地）坐标系下的速度：
│   ├── if (!_ahrs.get_velocity_NED(velocity)) {
│   │   ├── 如果失败，使用GPS数据：
│   │   │   ├── 检查GPS状态是否为3D Fix：
│   │   │   │   ├── if (AP::gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
│   │   │   │   │   ├── 检查航向与地面航向的差异：
│   │   │   │   │   │   ├── if (abs(wrap_180_cd(_ahrs.yaw_sensor - AP::gps().ground_course_cd())) <= 9000) {
│   │   │   │   │   │   │   ├── 差异在±90度以内，使用正向地面速度：speed = AP::gps().ground_speed();
│   │   │   │   │   │   │   └── 否则使用反向地面速度：speed = -AP::gps().ground_speed();
│   │   │   │   │   │   └── }
│   │   │   │   │   ├── 返回true表示成功：return true;
│   │   │   │   └── }
│   │   │   └── 否则返回false表示失败：return false;
│   │   └── }
│   └── }
├── 计算车体前进速度：
│   ├── speed = velocity.x*_ahrs.cos_yaw() + velocity.y*_ahrs.sin_yaw();
└── 返回true表示成功：return true;

1. 尝试从姿态航向参考系统（AHRS）获取NED坐标系下的速度。
2. 如果获取失败，则检查GPS状态。
   • 如果GPS状态至少是3D Fix，则根据车体航向与GPS航向的差异，确定地面速度是正向还是反向。
   • 否则返回获取失败。
3. 如果获取NED坐标系速度成功，则转换为车体坐标系下的前进速度。
4. 最后返回true表示成功。

4. 总结

GPS下的跟随模式，主要是通过AP_Follow和ModeFollow来实现：

1. AP_Follow更新跟随目标的状态信息
2. ModeFollow定期根据跟随目标情况，更新自身的方向和速度，根据两者之间的距离进行判断

注：详细代码可以仔细阅读，但是从整个设计逻辑的角度看，其实还是可以理解的。另外，也有一些关于加速度的问题，截止发稿日，还并没有很好的处理，这个可能和轨迹预测，跟随车辆指向变化方向改变有关系，对于摄像头跟随是很有意义和价值的。

5. 参考资料

【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码框架
【3】ArduPilot开源飞控之飞行模式