前面在《Ardupilot开源飞控之FollowMe计划》最初“【无障碍物】主动FollowMe功能(采用GPS) ”就是在这部分实现的。
那么我们研读下这部分代码。
关于模型模式的相关框架，详见：ArduPilot开源飞控之飞行模式。注：ArduRover和ArduCopter类似，可以相互借鉴。
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
类定义
class ModeFollow : public Mode
ModeFollow 类继承自 Mode 类。公有成员函数（public）
Number mode_number() const override
Number::FOLLOW。const char *name4() const override
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
保护成员函数（protected）
bool _enter() override
void _exit() override
保护成员变量（protected）
float _desired_speed
模式更新函数，定时轮训。
SCHED_TASK(update_current_mode, 400, 200, 12),
└──> Rover::update_current_mode
└──> control_mode->update
└──> ModeFollow::update
主要分为以下几个步骤：
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);
进入模式，执行的初始化函数。
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;
}
退出模式，执行的“清场”函数。
Rover::set_mode
└──> Mode::exit
└──> ModeFollow::_exit
清场处理。
// exit handling
void ModeFollow::_exit()
{
g2.follow.clear_offsets_if_required();
}
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
枚举类型 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[]: 参数列表。静态成员
_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秒。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类，则直接返回。消息解析：
msgid，分别处理不同类型的MAVLink消息：
MAVLINK_MSG_ID_GLOBAL_POSITION_INT：处理全局位置消息，更新目标的经纬度、高度、速度和航向信息。如果消息无效（经纬度为零），则忽略消息。MAVLINK_MSG_ID_FOLLOW_TARGET：处理跟踪目标消息，更新目标的经纬度、高度、速度和航向信息。如果消息无效（经纬度为零或不包含位置数据），则忽略消息。更新检查和日志记录：
该方法通过处理不同类型的MAVLink消息来跟踪目标的实时位置和运动信息，并在需要时进行日志记录，以确保目标跟踪的精度和可靠性。
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
判断是否启用：
_enabled 是否为 true。_enabled 为 false，函数立即返回 false。检查是否超时：
_last_location_update_ms 是否为 0，或当前时间与 _last_location_update_ms 的差值是否超过 AP_FOLLOW_TIMEOUT_MS。false。计算时间差 (dt)：
获取速度估算：
get_velocity_ned(vel_ned, dt) 获取速度估算值。false。投影车辆位置：
vel_ned.x * dt 和 vel_ned.y * dt 用于更新水平位置。vel_ned.z * 100.0f * dt 用于更新垂直位置（速度单位转换为 cm/s，并乘以时间差）。loc。返回最新的位置信息：
true。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;
}
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);
计算时间差异:
diff_us 计算本地时间 local_usec 和外部时间 offboard_usec 之间的差异。初次初始化或时间差异超前的处理:
link_offset_usec，则更新 link_offset_usec 并标记为已初始化。这部分处理外部时间戳过于超前的情况。估算校正后的外部时间:
offboard_usec 加上 link_offset_usec 来估算校正后的时间 estimate_us。检查消息是否太过于滞后:
estimate_us 调整为本地时间减去最大滞后时间，并更新 link_offset_usec。最小样本处理:
min_sample_us。min_sample_counter。diff_us 小于记录的最小样本 min_sample_us，则更新最小样本。convergence_loops，则更新 link_offset_usec 为最小样本，并重置样本计数器。返回校正后的时间:
estimate_us。通过这种树形结构，可以清晰地看到函数的每个步骤和逻辑分支的关系，更加容易理解整个函数的工作流程。
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;
GPS下的跟随模式，主要是通过AP_Follow和ModeFollow来实现：
AP_Follow更新跟随目标的状态信息ModeFollow定期根据跟随目标情况，更新自身的方向和速度，根据两者之间的距离进行判断注：详细代码可以仔细阅读，但是从整个设计逻辑的角度看，其实还是可以理解的。另外，也有一些关于加速度的问题，截止发稿日，还并没有很好的处理，这个可能和轨迹预测，跟随车辆指向变化方向改变有关系，对于摄像头跟随是很有意义和价值的。
【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码框架
【3】ArduPilot开源飞控之飞行模式