深入ArduPilot源码手把手实现自定义绕点盘旋飞行模式无人机开发者常遇到标准飞行模式无法满足特定需求的情况。比如在巡检、测绘或影视拍摄中让无人机围绕某个坐标点保持固定半径盘旋是常见需求。本文将带您从零开始在ArduPilot中实现一个全新的OrbitPoint飞行模式完整覆盖从枚举定义到地面站适配的全流程。1. 开发环境准备与源码结构分析在开始编码前需要先建立ArduPilot的开发环境。推荐使用Ubuntu 20.04 LTS作为开发系统通过以下命令安装基础依赖sudo apt-get install git gcc-arm-none-eabi python3-pip pip3 install future pyserial克隆ArduPilot源码仓库并切换到Copter子项目git clone https://github.com/ArduPilot/ardupilot.git cd ardupilot git submodule update --init --recursive cd ArduCopter飞行模式相关的核心代码分布在以下位置libraries/AP_Motors/电机控制库libraries/AC_AttitudeControl/姿态控制算法mode.h/mode.cpp飞行模式基类mode_*.h/mode_*.cpp各具体模式实现建议先熟悉Mode基类的设计它定义了所有飞行模式必须实现的接口class Mode { public: virtual void run() 0; // 主运行函数100Hz调用 virtual bool init(bool ignore_checks) 0; // 模式初始化 virtual const char *name() const 0; // 模式全称 virtual const char *name4() const 0; // 4字符缩写 };2. 创建OrbitPoint飞行模式类2.1 添加模式枚举定义首先在modes.h的Mode::Number枚举中添加新模式enum class Number : uint8_t { // ...已有模式... ORBIT_POINT 29, // 自动绕指定点盘旋 // 确保数值不与现有模式冲突 };2.2 定义模式类框架新建mode_orbit.h头文件继承自Mode基类#pragma once #include Mode.h class ModeOrbitPoint : public Mode { public: using Mode::Mode; // 继承基类构造函数 bool init(bool ignore_checks) override; void run() override; // 禁用复制构造 ModeOrbitPoint(const ModeOrbitPoint other) delete; ModeOrbitPoint operator(const ModeOrbitPoint) delete; protected: const char *name() const override { return ORBIT_POINT; } const char *name4() const override { return ORBT; } private: enum class SubMode { INITIALIZE, APPROACH, ORBITING, RECOVERY }; SubMode _submode SubMode::INITIALIZE; Vector3f _target_pos; // 目标点ECEF坐标 float _orbit_radius 10.0f; // 默认盘旋半径(m) uint32_t _last_update_ms; // 最后更新时间戳 };2.3 实现核心逻辑创建mode_orbit.cpp实现文件首先完成初始化方法bool ModeOrbitPoint::init(bool ignore_checks) { if (!AP::ahrs().home_is_set()) { return false; // 需要GPS定位 } // 获取当前目标点可扩展为从地面站接收 _target_pos AP::ahrs().get_home(); // 检查盘旋半径参数 if (_orbit_radius 2.0f || _orbit_radius 100.0f) { _orbit_radius 10.0f; // 默认值 } _submode SubMode::APPROACH; return true; }实现主运行逻辑处理不同子状态void ModeOrbitPoint::run() { const uint32_t now AP_HAL::millis(); if (now - _last_update_ms 10) { return; // 保持100Hz运行频率 } _last_update_ms now; switch (_submode) { case SubMode::APPROACH: handle_approach(); break; case SubMode::ORBITING: handle_orbiting(); break; case SubMode::RECOVERY: handle_recovery(); break; default: break; } }实现盘旋控制算法简化版void ModeOrbitPoint::handle_orbiting() { // 获取当前位置和姿态 Vector3f current_pos; if (!AP::ahrs().get_relative_position_NED_origin(current_pos)) { _submode SubMode::RECOVERY; return; } // 计算目标点相对位置 Vector3f rel_pos _target_pos - current_pos; float distance rel_pos.length(); // 保持盘旋半径 if (fabsf(distance - _orbit_radius) 1.0f) { Vector3f desired_vel rel_pos.normalized() * (distance - _orbit_radius) * 0.5f; // 此处应调用位置控制器接口 pos_control-set_velocity(desired_vel); } // 计算切向速度绕点运动 Vector3f tangent_vel Vector3f(-rel_pos.y, rel_pos.x, 0).normalized() * 2.0f; vel_control-set_velocity(tangent_vel); // 高度保持 pos_control-set_alt_target_from_climb_rate(0.0f); }3. 集成到飞控主系统3.1 添加类实例在Copter.h的飞行模式声明区域添加#if MODE_ORBIT_POINT_ENABLED ENABLED ModeOrbitPoint mode_orbit_point; #endif3.2 注册模式映射修改mode.cpp中的mode_from_mode_num函数Mode *Copter::mode_from_mode_num(const Mode::Number mode) { switch (mode) { // ...已有模式... case Mode::Number::ORBIT_POINT: return mode_orbit_point; default: return nullptr; } }3.3 配置编译选项在config.h中添加新模式编译开关#ifndef MODE_ORBIT_POINT_ENABLED #define MODE_ORBIT_POINT_ENABLED ENABLED #endif设置默认飞行模式参数#ifndef FLIGHT_MODE_6 #define FLIGHT_MODE_6 Mode::Number::ORBIT_POINT #endif4. 地面站适配与参数配置4.1 添加MAVLink枚举修改mavlink/ardupilotmega.xmlenum nameCOPTER_MODE !-- 已有模式 -- entry value29 nameCOPTER_MODE_ORBIT_POINT/ /enum4.2 创建模式参数组在Parameters.cpp中添加专用参数// Param: ORBT_RADIUS // DisplayName: Orbit radius // Description: Default orbit radius in meters // Range: 2 100 // Units: m // Increment: 1 // User: Advanced GSCALAR(orbit_radius, ORBT_RADIUS, 10.0f);4.3 地面站界面适配主流地面站如Mission Planner通常会自动识别新飞行模式。如需自定义界面可修改飞行模式选择下拉菜单盘旋半径参数调节控件目标点设置界面5. 调试技巧与常见问题5.1 仿真测试使用SITL仿真环境测试新模式sim_vehicle.py -v ArduCopter --console --map关键调试命令mode orbit切换到盘旋模式param set ORBT_RADIUS 15设置盘旋半径wp set 1 纬度 经度 高度设置目标点5.2 常见错误处理错误现象可能原因解决方案模式无法切换初始化失败检查GPS状态和home位置盘旋半径不稳定控制器参数不当调整PID参数高度漂移气压计干扰检查舱体密封性5.3 性能优化建议位置控制优化// 在模式初始化时设置控制器参数 pos_control-set_max_speed_accel_xy(5.0f, 2.0f); pos_control-set_max_accel_z(2.0f, 1.0f);抗风扰处理// 在run()中添加风力补偿 Vector3f wind_comp ahrs.wind_estimate() * 0.3f; vel_control-set_desired_velocity(desired_vel wind_comp);紧急情况处理if (copter.failsafe.radio || copter.failsafe.battery) { _submode SubMode::RECOVERY; copter.set_mode(Mode::Number::RTL, ModeReason::FAILSAFE); }6. 进阶扩展方向6.1 动态半径调整通过遥控器旋钮实时调整盘旋半径// 在run()中读取RC输入 float radius_adjust copter.channel_rudder-get_control_in() * 0.1f; _orbit_radius constrain_float(_orbit_radius radius_adjust, 2.0f, 100.0f);6.2 多目标点路径扩展为按顺序绕多个点盘旋std::vectorVector3f _orbit_points; uint8_t _current_point_idx 0; void switch_to_next_point() { _current_point_idx (_current_point_idx 1) % _orbit_points.size(); _target_pos _orbit_points[_current_point_idx]; }6.3 视觉辅助定位结合OpenCV实现视觉定位# 视觉处理伪代码 import cv2 target cv2.imread(target_pattern.png) orb cv2.ORB_create() kp1, des1 orb.detectAndCompute(target, None) # ...特征匹配计算相对位置...实现这类扩展时需要注意新增功能需进行充分的单元测试保持与现有飞行模式的兼容性考虑计算资源占用情况
深入ArduPilot源码：手把手教你为Copter新增一个自定义飞行模式（以‘绕点盘旋’为例）
深入ArduPilot源码手把手实现自定义绕点盘旋飞行模式无人机开发者常遇到标准飞行模式无法满足特定需求的情况。比如在巡检、测绘或影视拍摄中让无人机围绕某个坐标点保持固定半径盘旋是常见需求。本文将带您从零开始在ArduPilot中实现一个全新的OrbitPoint飞行模式完整覆盖从枚举定义到地面站适配的全流程。1. 开发环境准备与源码结构分析在开始编码前需要先建立ArduPilot的开发环境。推荐使用Ubuntu 20.04 LTS作为开发系统通过以下命令安装基础依赖sudo apt-get install git gcc-arm-none-eabi python3-pip pip3 install future pyserial克隆ArduPilot源码仓库并切换到Copter子项目git clone https://github.com/ArduPilot/ardupilot.git cd ardupilot git submodule update --init --recursive cd ArduCopter飞行模式相关的核心代码分布在以下位置libraries/AP_Motors/电机控制库libraries/AC_AttitudeControl/姿态控制算法mode.h/mode.cpp飞行模式基类mode_*.h/mode_*.cpp各具体模式实现建议先熟悉Mode基类的设计它定义了所有飞行模式必须实现的接口class Mode { public: virtual void run() 0; // 主运行函数100Hz调用 virtual bool init(bool ignore_checks) 0; // 模式初始化 virtual const char *name() const 0; // 模式全称 virtual const char *name4() const 0; // 4字符缩写 };2. 创建OrbitPoint飞行模式类2.1 添加模式枚举定义首先在modes.h的Mode::Number枚举中添加新模式enum class Number : uint8_t { // ...已有模式... ORBIT_POINT 29, // 自动绕指定点盘旋 // 确保数值不与现有模式冲突 };2.2 定义模式类框架新建mode_orbit.h头文件继承自Mode基类#pragma once #include Mode.h class ModeOrbitPoint : public Mode { public: using Mode::Mode; // 继承基类构造函数 bool init(bool ignore_checks) override; void run() override; // 禁用复制构造 ModeOrbitPoint(const ModeOrbitPoint other) delete; ModeOrbitPoint operator(const ModeOrbitPoint) delete; protected: const char *name() const override { return ORBIT_POINT; } const char *name4() const override { return ORBT; } private: enum class SubMode { INITIALIZE, APPROACH, ORBITING, RECOVERY }; SubMode _submode SubMode::INITIALIZE; Vector3f _target_pos; // 目标点ECEF坐标 float _orbit_radius 10.0f; // 默认盘旋半径(m) uint32_t _last_update_ms; // 最后更新时间戳 };2.3 实现核心逻辑创建mode_orbit.cpp实现文件首先完成初始化方法bool ModeOrbitPoint::init(bool ignore_checks) { if (!AP::ahrs().home_is_set()) { return false; // 需要GPS定位 } // 获取当前目标点可扩展为从地面站接收 _target_pos AP::ahrs().get_home(); // 检查盘旋半径参数 if (_orbit_radius 2.0f || _orbit_radius 100.0f) { _orbit_radius 10.0f; // 默认值 } _submode SubMode::APPROACH; return true; }实现主运行逻辑处理不同子状态void ModeOrbitPoint::run() { const uint32_t now AP_HAL::millis(); if (now - _last_update_ms 10) { return; // 保持100Hz运行频率 } _last_update_ms now; switch (_submode) { case SubMode::APPROACH: handle_approach(); break; case SubMode::ORBITING: handle_orbiting(); break; case SubMode::RECOVERY: handle_recovery(); break; default: break; } }实现盘旋控制算法简化版void ModeOrbitPoint::handle_orbiting() { // 获取当前位置和姿态 Vector3f current_pos; if (!AP::ahrs().get_relative_position_NED_origin(current_pos)) { _submode SubMode::RECOVERY; return; } // 计算目标点相对位置 Vector3f rel_pos _target_pos - current_pos; float distance rel_pos.length(); // 保持盘旋半径 if (fabsf(distance - _orbit_radius) 1.0f) { Vector3f desired_vel rel_pos.normalized() * (distance - _orbit_radius) * 0.5f; // 此处应调用位置控制器接口 pos_control-set_velocity(desired_vel); } // 计算切向速度绕点运动 Vector3f tangent_vel Vector3f(-rel_pos.y, rel_pos.x, 0).normalized() * 2.0f; vel_control-set_velocity(tangent_vel); // 高度保持 pos_control-set_alt_target_from_climb_rate(0.0f); }3. 集成到飞控主系统3.1 添加类实例在Copter.h的飞行模式声明区域添加#if MODE_ORBIT_POINT_ENABLED ENABLED ModeOrbitPoint mode_orbit_point; #endif3.2 注册模式映射修改mode.cpp中的mode_from_mode_num函数Mode *Copter::mode_from_mode_num(const Mode::Number mode) { switch (mode) { // ...已有模式... case Mode::Number::ORBIT_POINT: return mode_orbit_point; default: return nullptr; } }3.3 配置编译选项在config.h中添加新模式编译开关#ifndef MODE_ORBIT_POINT_ENABLED #define MODE_ORBIT_POINT_ENABLED ENABLED #endif设置默认飞行模式参数#ifndef FLIGHT_MODE_6 #define FLIGHT_MODE_6 Mode::Number::ORBIT_POINT #endif4. 地面站适配与参数配置4.1 添加MAVLink枚举修改mavlink/ardupilotmega.xmlenum nameCOPTER_MODE !-- 已有模式 -- entry value29 nameCOPTER_MODE_ORBIT_POINT/ /enum4.2 创建模式参数组在Parameters.cpp中添加专用参数// Param: ORBT_RADIUS // DisplayName: Orbit radius // Description: Default orbit radius in meters // Range: 2 100 // Units: m // Increment: 1 // User: Advanced GSCALAR(orbit_radius, ORBT_RADIUS, 10.0f);4.3 地面站界面适配主流地面站如Mission Planner通常会自动识别新飞行模式。如需自定义界面可修改飞行模式选择下拉菜单盘旋半径参数调节控件目标点设置界面5. 调试技巧与常见问题5.1 仿真测试使用SITL仿真环境测试新模式sim_vehicle.py -v ArduCopter --console --map关键调试命令mode orbit切换到盘旋模式param set ORBT_RADIUS 15设置盘旋半径wp set 1 纬度 经度 高度设置目标点5.2 常见错误处理错误现象可能原因解决方案模式无法切换初始化失败检查GPS状态和home位置盘旋半径不稳定控制器参数不当调整PID参数高度漂移气压计干扰检查舱体密封性5.3 性能优化建议位置控制优化// 在模式初始化时设置控制器参数 pos_control-set_max_speed_accel_xy(5.0f, 2.0f); pos_control-set_max_accel_z(2.0f, 1.0f);抗风扰处理// 在run()中添加风力补偿 Vector3f wind_comp ahrs.wind_estimate() * 0.3f; vel_control-set_desired_velocity(desired_vel wind_comp);紧急情况处理if (copter.failsafe.radio || copter.failsafe.battery) { _submode SubMode::RECOVERY; copter.set_mode(Mode::Number::RTL, ModeReason::FAILSAFE); }6. 进阶扩展方向6.1 动态半径调整通过遥控器旋钮实时调整盘旋半径// 在run()中读取RC输入 float radius_adjust copter.channel_rudder-get_control_in() * 0.1f; _orbit_radius constrain_float(_orbit_radius radius_adjust, 2.0f, 100.0f);6.2 多目标点路径扩展为按顺序绕多个点盘旋std::vectorVector3f _orbit_points; uint8_t _current_point_idx 0; void switch_to_next_point() { _current_point_idx (_current_point_idx 1) % _orbit_points.size(); _target_pos _orbit_points[_current_point_idx]; }6.3 视觉辅助定位结合OpenCV实现视觉定位# 视觉处理伪代码 import cv2 target cv2.imread(target_pattern.png) orb cv2.ORB_create() kp1, des1 orb.detectAndCompute(target, None) # ...特征匹配计算相对位置...实现这类扩展时需要注意新增功能需进行充分的单元测试保持与现有飞行模式的兼容性考虑计算资源占用情况
