Ardupilot開源飛控之AP_Follow

Ardupilot開源飛控之AP_Follow

  • 1. 源由
  • 2. 定義
    • 2.1 ModeFollow類
      • 2.1.1 ModeFollow::update
      • 2.1.2 ModeFollow::_enter
      • 2.1.3 ModeFollow::_exit
    • 2.2 AP_Follow類
      • 2.2.1 AP_Follow::handle_msg
      • 2.2.2 AP_Follow::get_target_location_and_velocity
      • 2.2.3 AP_Follow::get_velocity_ned
  • 3. 其他函數
    • 3.1 JitterCorrection::correct_offboard_timestamp_msec
  • 4. 總結
  • 5. 參考資料

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[]: 參數列表。
  1. 私有成員

  • 靜態成員

    • _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_useccorrect_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。

通過這種樹形結構，可以清晰地看到函數的每個步驟和邏輯分支的關系，更加容易理解整個函數的工作流程。

4. 總結

GPS下的跟隨模式，主要是通過AP_Follow和ModeFollow來實現：

  1. AP_Follow更新跟隨目標的狀態信息
  2. ModeFollow定期根據跟隨目標情況，更新自身的方向和速度，根據兩者之間的距離進行判斷

注：詳細代碼可以仔細閱讀，但是從整個設計邏輯的角度看，其實還是可以理解的。另外，也有一些關于加速度的問題，截止發稿日，還并沒有很好的處理，這個可能和軌跡預測，跟隨車輛指向變化方向改變有關系，對于攝像頭跟隨是很有意義和價值的。

5. 參考資料

【1】ArduPilot開源飛控系統之簡單介紹
【2】ArduPilot之開源代碼框架
【3】ArduPilot開源飛控之飛行模式

本文來自互聯網用戶投稿，該文觀點僅代表作者本人，不代表本站立場。本站僅提供信息存儲空間服務，不擁有所有權，不承擔相關法律責任。
如若轉載，請注明出處：http://www.pswp.cn/bicheng/18971.shtml
繁體地址，請注明出處：http://hk.pswp.cn/bicheng/18971.shtml
英文地址，請注明出處：http://en.pswp.cn/bicheng/18971.shtml

如若內容造成侵權/違法違規/事實不符，請聯系多彩編程網進行投訴反饋email:809451989@qq.com，一經查實，立即刪除！

相關文章

getContentView(mBinding.getRoot())； 會導致內存泄露嗎？里面有SurfaceView ViewBinding

getContentView(mBinding.getRoot())； 會導致內存泄露嗎？里面有SurfaceView ViewBinding

在上述代碼中&#xff0c;ActivityTestingBinding 是一個 Data Binding 庫生成的類&#xff0c;用于綁定 XML 布局到 Activity 中。inflate(getLayoutInflater()) 用于將布局文件解析并轉換為對應的視圖層次結構。然后 getWindow().setFlags() 設置窗口屬性&#xff0c;保持屏幕…
閱讀更多...
小型海外倉如何選擇第三方海外倉系統：多看多對比，性價比優先

小型海外倉如何選擇第三方海外倉系統：多看多對比，性價比優先

在現在的海外倉市場中&#xff0c;中小型海外倉&#xff0c;家庭海外倉的占比還是非常大的。這類海外倉的一個共同點就是資金有限&#xff0c;管理能力比較弱&#xff0c;很難實現規模效應。 對于這類海外倉來說&#xff0c;選擇一套合適的第三方海外倉系統&#xff0c;對提升…
閱讀更多...
好用的國產大文件傳輸軟件有哪些，快來看看吧

好用的國產大文件傳輸軟件有哪些，快來看看吧

在這個數字化飛速發展的時代&#xff0c;我們每天都在與各種文件打交道&#xff0c;從簡單的文檔到龐大的視頻素材&#xff0c;文件的體積越來越大&#xff0c;傳統的文件傳輸方式逐漸顯得力不從心。面對這個挑戰&#xff0c;大文件傳輸軟件應運而生&#xff0c;它們不僅解決了…
閱讀更多...
note-網絡是怎樣連接的4 接入網和網絡運營商

note-網絡是怎樣連接的4 接入網和網絡運營商

助記提要 網絡包從用戶傳輸到互聯網的過程信號的調制方式ADSL使用多個頻率的合成波傳輸信號分離器的作用電話線的特點光纖的構造光纖的原理單模光纖和多模光纖光纖接入網的兩種接入方式PPP撥號上網過程ADSL和FTTH使用PPPoE的方式PPPoE的規則隧道其他接入認證方式 PPPoA和DHCP網…
閱讀更多...
基于大數據的高校生源可視化分析系統

基于大數據的高校生源可視化分析系統

基于大數據的高校生源可視化分析系統 “A Visual Analysis System for Higher Education Student Enrollment based on Big Data” 完整下載鏈接:基于大數據的高校生源可視化分析系統 文章目錄 基于大數據的高校生源可視化分析系統摘要第一章 引言1.1 研究背景1.2 研究目的1.…
閱讀更多...
adam優化器計算過程(tensorflow)

adam優化器計算過程(tensorflow)

一、adam原理 原理 應用 優點 缺點 二、手動實現 一步一步計算 三、使用tensorflow api實現 api使用 四、一個具體的深度學習的例子
閱讀更多...
隨后記： uniapp uview u-dropdown 下拉菜單固定高度滑動不生效

隨后記： uniapp uview u-dropdown 下拉菜單固定高度滑動不生效

使用u-dropdown 下拉組件 按照uview官網講解使用 配置根本不生效 scroll-y"true" style"height: 200rpx;" 但是在下拉的時候&#xff0c;不能上下滑動 &#xff0c;原因是自帶的遮罩層擋住了 解決辦法&#xff1a;在下拉菜單打開和關閉的時候&#xff0c…
閱讀更多...
linux 目錄 /usr/lib和 /usr/lib64區別

linux 目錄 /usr/lib和 /usr/lib64區別

在 Linux 系統中&#xff0c;/usr/lib 和 /usr/lib64 目錄通常用于存儲庫文件&#xff08;libraries&#xff09;&#xff0c;這些庫文件是程序運行時所需的共享代碼和數據。這兩個目錄之間的主要區別在于它們所包含的庫文件的架構&#xff08;architecture&#xff09;和用途。…
閱讀更多...
Python函數式編程進階：用函數實現設計模式

Python函數式編程進階：用函數實現設計模式

文章目錄 函數式編程進階&#xff1a;用函數實現設計模式案例實現&#xff1a;構建“策略”模式使用函數實現”策略“模式享元 選擇最佳策略&#xff1a;簡單的方式 globals關鍵字 函數式編程進階&#xff1a;用函數實現設計模式 案例實現&#xff1a;構建“策略”模式 策略模…
閱讀更多...
Java 18新特性：探索Java的未來

Java 18新特性：探索Java的未來

目錄 1. 增強的模式匹配 2. JEP 411&#xff1a;String解構 3. JEP 395&#xff1a;Records增強 4. JEP 398&#xff1a;Deprecate警告增強 5. JEP 409&#xff1a;Sealed類和接口增強 6. API改進 6.1 集合API改進 6.2 流API改進 6.3 IO/NIO API改進 7. 性能優化 7.…
閱讀更多...
從0開始帶你成為Kafka消息中間件高手---第三講

從0開始帶你成為Kafka消息中間件高手---第三講

從0開始帶你成為Kafka消息中間件高手—第三講 實際上來說&#xff0c;每次leader接收到一條消息&#xff0c;都會更新自己的LEO&#xff0c;也就是log end offset&#xff0c;把最后一位offset 1&#xff0c;這個大家都能理解吧&#xff1f;接著各個follower會從leader請求同…
閱讀更多...
k8s 中svc映射的外部端口是如何監聽的，netstat命令為什么查不到？

k8s 中svc映射的外部端口是如何監聽的，netstat命令為什么查不到？

在Kubernetes中&#xff0c;Service&#xff08;服務&#xff09;是一種抽象&#xff0c;用于將一組Pod&#xff08;容器&#xff09;公開為一個網絡服務。Service可以通過ClusterIP&#xff08;集群內部IP&#xff09;、NodePort&#xff08;節點端口&#xff09;或LoadBalanc…
閱讀更多...
KDD 2024｜基于隱空間因果推斷的微服務系統根因定位

KDD 2024｜基于隱空間因果推斷的微服務系統根因定位

簡介&#xff1a;本文介紹了由清華大學、南開大學、eBay、微軟、中國科學院計算機網絡信息中心等單位共同合作的論文《基于隱空間因果推斷的受限可觀測性場景的微服務系統根因定位》。該論文已被KDD 2024會議錄用。 論文標題&#xff1a;Microservice Root Cause Analysis Wit…
閱讀更多...
10年老運營人吐血整理，給新媒體運營人的20條建議！沈陽新媒體運營培訓

10年老運營人吐血整理，給新媒體運營人的20條建議！沈陽新媒體運營培訓

對于企業&#xff0c;在新媒體平臺開設官方賬號應該是已經成為標配。不僅是對企業新媒體運營需求量提高&#xff0c;新媒體人的薪資也是水漲船高。 另外值得注意的是&#xff0c;企業對資深新媒體運營人才尤為重視&#xff0c;這表現在他們不惜重金招聘高薪新媒體運營人才&…
閱讀更多...
Linux新增磁盤掛載分區

Linux新增磁盤掛載分區

1. 查看磁盤分區名稱 lsblk 可見&#xff0c;新增的分區為 sdb 2.格式化磁盤 mkfs.xfs -f /dev/sdb 3.掛在磁盤到 /ocean目錄&#xff0c;掛在前先創建空目錄 /ocean mkdir /oceanmount /dev/sdb /ocean 執行后&#xff0c;可用 df -h 查看分區是否成功 4.持久化磁盤分區&a…
閱讀更多...
查詢MongoDB中某個數據庫的占用空間大小

查詢MongoDB中某個數據庫的占用空間大小

要查詢MongoDB中某個數據庫的占用空間大小&#xff0c;可以使用以下幾種方法&#xff1a; 方法一&#xff1a;使用 MongoDB Shell (mongo) 1. **連接到 MongoDB**&#xff1a; bash mongo 2. **選擇數據庫**&#xff1a; javascript use yourDatabaseName …
閱讀更多...
AI推介-大語言模型LLMs論文速覽（arXiv方向）：2024.05.20-2024.05.25

AI推介-大語言模型LLMs論文速覽（arXiv方向）：2024.05.20-2024.05.25

文章目錄~ 1.STRIDE: A Tool-Assisted LLM Agent Framework for Strategic and Interactive Decision-Making2. M 3 M^3 M3GPT: An Advanced Multimodal, Multitask Framework for Motion Comprehension and Generation3.MindStar: Enhancing Math Reasoning in Pre-trained LL…
閱讀更多...
計算機圖形學入門03：二維變換

計算機圖形學入門03：二維變換

變換(Transformation)可分為模型(Model)變換和視圖(Viewing)變換。在3D虛擬場景中相機的移動和旋轉&#xff0c;角色人物動畫都需要變換&#xff0c;用來描述物體運動。將三維世界投影變換到2D屏幕上成像出來&#xff0c;也需要變換。 1.縮放變換 縮放(Scale)變換&#xff1a; …
閱讀更多...
【B站 heima】小兔鮮Vue3 項目學習筆記 Day06

【B站 heima】小兔鮮Vue3 項目學習筆記 Day06

文章目錄 購物車本地1. 列表購物車基礎數據渲染2. 列表購物車單選功能3. 列表購物車全選功能4. 列表購物車統計列表實現5. 接口-加入購物車6. 接口-刪除購物車7. 退出登錄-清空購物車數據8. 合并購物車到服務器(重要) 結算1. 路由配置和基礎數據渲染2. 地址切換-打開彈框交互實…
閱讀更多...
跨境人必讀：X（原Twitter）和Facebook區別是什么？

跨境人必讀：X（原Twitter）和Facebook區別是什么？

在今日全球化的商業環境中&#xff0c;跨境電商領域的企業和獨立站賣家正逐漸認識到社交媒體營銷的巨大潛力。特別是X&#xff08;原Twitter&#xff09;和Facebook&#xff0c;作為領先的社交媒體平臺&#xff0c;它們的使用不僅能夠提升品牌知名度&#xff0c;還能直接影響銷…
閱讀更多...
最新文章

Copyright @ 2022~2023