自主避障无人机FastDrone的控制代码笔记

CS_Zero2026-05-22 18:48

PX4Ctrl进程

介绍

主进程

主要对象：LinearControl类的对象controller与PX4CtrlFSM类的对象fsm。

订阅mavros广播的消息遥控通道，在回调更新fsm中的rc_data。

cpp 复制代码 
    ros::Subscriber rc_sub;
    if (!param.takeoff_land.no_RC) // mavros will still publish wrong rc messages although no RC is connected
    {
        rc_sub = nh.subscribe<mavros_msgs::RCIn>("/mavros/rc/in",
                                                 10,
                                                 boost::bind(&RC_Data_t::feed, &fsm.rc_data, _1));
    }

在src/realflight_modules/px4ctrl/src/input.cpp实现，原通道输出一般在1000 - 2000，模式相关的通道归一化为0-1。

reboot_cmd是RC通道8，与教程中紧急停止开关RC的通道7不是同一个。

cpp 复制代码 
    for (int i = 0; i < 4; i++)
    {
        ch[i] = ((double)msg.channels[i] - 1500.0) / 500.0;
        if (ch[i] > DEAD_ZONE)
            ch[i] = (ch[i] - DEAD_ZONE) / (1 - DEAD_ZONE);
        else if (ch[i] < -DEAD_ZONE)
            ch[i] = (ch[i] + DEAD_ZONE) / (1 - DEAD_ZONE);
        else
            ch[i] = 0.0;
    }

    mode = ((double)msg.channels[4] - 1000.0) / 1000.0;
    gear = ((double)msg.channels[5] - 1000.0) / 1000.0;
    reboot_cmd = ((double)msg.channels[7] - 1000.0) / 1000.0;

使用ROS服务设置飞控模式、解锁与操控

cpp 复制代码 
fsm.set_FCU_mode_srv = nh.serviceClient<mavros_msgs::SetMode>("/mavros/set_mode");
fsm.arming_client_srv = nh.serviceClient<mavros_msgs::CommandBool>("/mavros/cmd/arming");
fsm.reboot_FCU_srv = nh.serviceClient<mavros_msgs::CommandLong>("/mavros/cmd/command");
# 在src/realflight_modules/px4ctrl/src/PX4CtrlFSM.h中PX4CtrlFSM类中，
ros::ServiceClient set_FCU_mode_srv;

状态机逻辑

根据state值、遥控通道、定位信息等控制state切换。

核心思路：
MANUAL_CTRL 状态（遥控器控制）

↓

多重检查条件

├─ 收到定位信息？

├─ 没有命令冲突？

├─ 速度合理？

↓

state = AUTO_HOVER

controller.resetThrustMapping()

set_hov_with_odom()

toggle_offboard_mode(true) ← ★ 切换控制模式

↓

AUTO_HOVER 状态（程序自动控制）

↓

px4ctrl可以向PX4发送控制指令

cpp 复制代码 
void PX4CtrlFSM::process()
{
    ros::Time now_time = ros::Time::now();
	Controller_Output_t u;
	Desired_State_t des(odom_data);
	bool rotor_low_speed_during_land = false;

	// STEP1: state machine runs
	switch (state)
	{
	case MANUAL_CTRL:
		if (rc_data.enter_hover_mode) // Try to jump to AUTO_HOVER
		{
			if (!odom_is_received(now_time))
			{
				ROS_ERROR("[px4ctrl] Reject AUTO_HOVER(L2). No odom!");
				break;
			}
     //...
        }
    }
}

Fast-Drone-250/src/realflight_modules/px4ctrl/src/input.cpp中函数 void RC_Data_t::feed(mavros_msgs::RCInConstPtr pMsg)切换与state相关的变量enter_hover_mode、

第5通道 mode > 0.75，设置enter_hover_mode，注释1下面的if-else被后面的覆盖了，

第6通道 gear > 0.75，is_command_mode = true

cpp 复制代码 
    if (!have_init_last_mode)
    {
        have_init_last_mode = true;
        last_mode = mode;
    }
    if (!have_init_last_gear)
    {
        have_init_last_gear = true;
        last_gear = gear;
    }
    if (!have_init_last_reboot_cmd)
    {
        have_init_last_reboot_cmd = true;
        last_reboot_cmd = reboot_cmd;
    }
  
    // 1
    // 注意遥控第5通道档位控制模式，只能从小PWM值到高PWM值切换，才能使得enter_hover_mode为true。
    // 这个条件的含义，在mode、last_mode均大于阈值，即通道5在拨下后，想要使得enter_hover_mode = true，得上下拨通道5
    if (last_mode < API_MODE_THRESHOLD_VALUE && mode > API_MODE_THRESHOLD_VALUE)
        enter_hover_mode = true;
    else
        enter_hover_mode = false;

    if (mode > API_MODE_THRESHOLD_VALUE)
        is_hover_mode = true;
    else
        is_hover_mode = false;
    if (is_hover_mode)
    {
        if (last_gear < GEAR_SHIFT_VALUE && gear > GEAR_SHIFT_VALUE)
            enter_command_mode = true;
        else if (gear < GEAR_SHIFT_VALUE)
            enter_command_mode = false;

        if (gear > GEAR_SHIFT_VALUE)
            is_command_mode = true;
        else
            is_command_mode = false;
    }

src/realflight_modules/px4ctrl/config/ctrl_param_fpv.yaml中thrust_model的hover_percentage设置等于无人机重力的推力对应油门百分比，thr2acc_则是单位百分比产生的加速度，不包含重力加速度。

cpp 复制代码 
void 
LinearControl::resetThrustMapping(void)
{
  thr2acc_ = param_.gra / param_.thr_map.hover_percentage;
  P_ = 1e6;
}
参数 符号 单位 物理意义
param_.gra ggg m/s² 重力加速度。无人机所在环境的重力加速度（通常为9.8 m/s²）
param_.thr_map.hover_percentage ThoverT_{hover}Thover 百分比 (0~1) 悬停油门比例。指无人机保持静止悬停时所需的油门信号占总油门范围的百分比
thr2acc_ kthr2acck_{thr2acc}kthr2acc m/s²/% 推力-加速度映射系数 。表示单位油门信号能产生的加速度，是后续控制中用来从期望加速度计算所需油门的关键参数
P_ P0P_0P0 无量纲 递推最小二乘算法的初始协方差。用于后续的在线推力模型估计，初值较大表示对初始估计的不确定性很高

ROS消息控制起飞降落的实现

自定义控制起降的ROS消息，FAST-DRONE-250/src/utils/quadrotor_msgs/msg/TakeoffLand.msg，

前两项是枚举，第三项是用到的。

txt 复制代码 
uint8 TAKEOFF = 1
uint8 LAND = 2
uint8 takeoff_land_cmd

使用方式，rostopic pub -1 /px4ctrl/takeoff_land quadrotor_msgs/TakeoffLand "takeoff_land_cmd: 2" 或 rostopic pub -1 /px4ctrl/takeoff_land quadrotor_msgs/TakeoffLand "takeoff_land_cmd: 1"

PX4Ctrl接收该消息的回调函数，

cpp 复制代码 
void Takeoff_Land_Data_t::feed(quadrotor_msgs::TakeoffLandConstPtr pMsg)
{
    msg = *pMsg;
    rcv_stamp = ros::Time::now();
    triggered = true;
    takeoff_land_cmd = pMsg->takeoff_land_cmd;
}

主进程循环调用 void PX4CtrlFSM::process()，

先判断rc_data.enter_hover_mode，由于为flase，进入else，

前面执行takeoff.sh使得takeoff_land_data.takeoff_land_cmd == quadrotor_msgs::TakeoffLand::TAKEOFF成立，

获得cmd信息，

cpp 复制代码 
// 从HOVER状态切换到CMD_CTRL状态，
else if (rc_data.is_command_mode && cmd_is_received(now_time))
{
    if (state_data.current_state.mode == "OFFBOARD")
    {
        state = CMD_CTRL;
        des = get_cmd_des();  // 获取命令期望值
    }
}
// 或者处于CMD_CTRL状态，
{
    des = get_cmd_des();
}

在switch语句后，按步骤执行，

cpp 复制代码 
        if (state == AUTO_HOVER || state == CMD_CTRL)
	{
		// controller.estimateThrustModel(imu_data.a, bat_data.volt, param);
		controller.estimateThrustModel(imu_data.a, param);
	}

	// STEP3: solve and update new control commands
	if (rotor_low_speed_during_land) // used at the start of auto takeoff
	{
		motors_idling(imu_data, u);
	}
	else
	{
		debug_msg = controller.calculateControl(des, odom_data, imu_data, u);
		debug_msg.header.stamp = now_time;
		debug_pub.publish(debug_msg);
	}

	// STEP4: publish control commands to mavros
	if (param.use_bodyrate_ctrl)
	{
		publish_bodyrate_ctrl(u, now_time);
	}
	else
	{
		publish_attitude_ctrl(u, now_time);
	}

综合条件成立，执行

c++ 复制代码 
			state = AUTO_TAKEOFF;
			controller.resetThrustMapping();
			set_start_pose_for_takeoff_land(odom_data);
			toggle_offboard_mode(true);				  // toggle on offboard before arm
			for (int i = 0; i < 10 && ros::ok(); ++i) // wait for 0.1 seconds to allow mode change by FMU // mark
			{
				ros::Duration(0.01).sleep();
				ros::spinOnce();
			}
			if (param.takeoff_land.enable_auto_arm)
			{
				toggle_arm_disarm(true);
			}
			takeoff_land.toggle_takeoff_land_time = now_time;

函数 toggle_offboard_mode在 px4ctrl/src/PX4CtrlFSM.cpp，参数true意为进入offboard模式，反之退出并回到进入前的模式。

c++ 复制代码 
bool PX4CtrlFSM::toggle_offboard_mode(bool on_off)
{
	mavros_msgs::SetMode offb_set_mode;

	if (on_off)
	{
		state_data.state_before_offboard = state_data.current_state;
		if (state_data.state_before_offboard.mode == "OFFBOARD") // Not allowed
			state_data.state_before_offboard.mode = "MANUAL";

		offb_set_mode.request.custom_mode = "OFFBOARD";
		if (!(set_FCU_mode_srv.call(offb_set_mode) && offb_set_mode.response.mode_sent))
		{
			ROS_ERROR("Enter OFFBOARD rejected by PX4!");
			return false;
		}
	}
	else
	{
		offb_set_mode.request.custom_mode = state_data.state_before_offboard.mode;
		if (!(set_FCU_mode_srv.call(offb_set_mode) && offb_set_mode.response.mode_sent))
		{
			ROS_ERROR("Exit OFFBOARD rejected by PX4!");
			return false;
		}
	}

	return true;

	// if (param.print_dbg)
	// 	printf("offb_set_mode mode_sent=%d(uint8_t)\n", offb_set_mode.response.mode_sent);
}

逻辑背景：上述takeoff.sh执行前的操作及其顺序，遥控器5通道拨到内侧，六通道拨到下侧，油门打到中位

再执行roslaunch px4ctrl run_ctrl.launch
src/realflight_modules/px4ctrl/src/input.cpp

c++ 复制代码 
    if (last_mode < API_MODE_THRESHOLD_VALUE && mode > API_MODE_THRESHOLD_VALUE)
        enter_hover_mode = true;
    else
        enter_hover_mode = false;

因此，enter_hover_mode = false;

double des_a_z = exp((delta_t - AutoTakeoffLand_t::MOTORS_SPEEDUP_TIME) * 6.0) * 7.0 - 7.0;

实现控制

配置文件"px4ctrl/config/ctrl_param_fpv.yaml"中use_bodyrate_ctrl: false，因此使用的是publish_attitude_ctrl(u, now_time);

细节

  • 起飞

    分三个状态，准备、上升、完成。

    准备，3秒时长，

  • 估计推力与加速度系数

  • 计算控制输出

cpp 复制代码 
	if (rotor_low_speed_during_land) // used at the start of auto takeoff
	{
		motors_idling(imu_data, u);
	}
	else
	{
		debug_msg = controller.calculateControl(des, odom_data, imu_data, u);
		debug_msg.header.stamp = now_time;
		debug_pub.publish(debug_msg);
	}

有趣细节

  1. 检查IMU消息频率的写法很妙，小于100Hz就警告
cpp 复制代码 
    // check the frequency
    static int one_min_count = 9999;
    static ros::Time last_clear_count_time = ros::Time(0.0);
    if ( (now - last_clear_count_time).toSec() > 1.0 )
    {
        if ( one_min_count < 100 )
        {
            ROS_WARN("IMU frequency seems lower than 100Hz, which is too low!");
        }
        one_min_count = 0;
        last_clear_count_time = now;
    }
    one_min_count ++;
  1. 判断降落的逻辑
cpp 复制代码 
void PX4CtrlFSM::land_detector(const State_t state, const Desired_State_t &des, const Odom_Data_t &odom)
{
	static State_t last_state = State_t::MANUAL_CTRL;
	if (last_state == State_t::MANUAL_CTRL && (state == State_t::AUTO_HOVER || state == State_t::AUTO_TAKEOFF))
	{
		takeoff_land.landed = false; // Always holds
	}
	last_state = state;

	if (state == State_t::MANUAL_CTRL && !state_data.current_state.armed)
	{
		takeoff_land.landed = true;
		return; // No need of other decisions
	}

	// land_detector parameters
	constexpr double POSITION_DEVIATION_C = -0.5; // Constraint 1: target position below real position for POSITION_DEVIATION_C meters.
	constexpr double VELOCITY_THR_C = 0.1;		  // Constraint 2: velocity below VELOCITY_MIN_C m/s.
	constexpr double TIME_KEEP_C = 3.0;			  // Constraint 3: Time(s) the Constraint 1&2 need to keep.

	static ros::Time time_C12_reached; // time_Constraints12_reached
	static bool is_last_C12_satisfy;
	if (takeoff_land.landed)
	{
		time_C12_reached = ros::Time::now();
		is_last_C12_satisfy = false;
	}
	else
	{
		bool C12_satisfy = (des.p(2) - odom.p(2)) < POSITION_DEVIATION_C && odom.v.norm() < VELOCITY_THR_C;
		if (C12_satisfy && !is_last_C12_satisfy)
		{
			time_C12_reached = ros::Time::now();
		}
		else if (C12_satisfy && is_last_C12_satisfy)
		{
			if ((ros::Time::now() - time_C12_reached).toSec() > TIME_KEEP_C) // Constraint 3 reached
			{
				takeoff_land.landed = true;
			}
		}

		is_last_C12_satisfy = C12_satisfy;
	}
}
相关推荐
小O的算法实验室7 小时前
2023年IEEE TMC，基于进化多目标强化学习的无人机辅助移动边缘计算轨迹控制与任务卸载，深度解析+性能实测
无人机
Deepoch7 小时前
野外作业新突破：Deepoc技术让无人机机群实现“去中心化”自主协同
无人机·开发板·具身模型·deepoc
派勤电子11 小时前
测绘差 1 米白跑一天、电力巡检漏缺陷、植保打药打不准?高性能工控机才是高精度无人机的核心底气
无人机·高性能工控机·无人机工控机·无人机工控主板·巡检无人机漏缺陷·植保无人机打药打不准·高精度无人机主板
XMAIPC_Robot13 小时前
深度无人机自动驾驶仪，中小型无人机硬件在环仿真飞行
运维·arm开发·人工智能·fpga开发·无人机·边缘计算
Evand J1 天前
【MATLAB集群控制导航7】多无人机三维编队轨迹规划仿真。RRT*+Catmull-Rom路径平滑+Frenet 编队保持。附MATLAB代码链接
开发语言·matlab·无人机
数智工坊2 天前
【DACS论文阅读】跨域混合采样如何让语义分割模型从合成数据无缝迁移到真实世界
论文阅读·人工智能·算法·机器人·无人机
Evand J2 天前
【课题推荐】三模型IMM交互式多模型滤波算法，匀速/左转/右转目标跟踪，附MATLAB代码测试结果
算法·matlab·目标跟踪·无人机·imm·多模型
小许同学记录成长2 天前
轻量正射实现原理技术文档
算法·无人机
AI浩3 天前
UAV-DETR：面向反无人机目标检测的 DETR 框架
人工智能·目标检测·无人机
热门推荐
01GitHub 镜像站点02Codex 接入 DeepSeek API 完整配置文档03装上就回不去了：CodeGraph 让 AI 编程效率飙升 92%，它到底做了什么？04CC-Switch & Claude 基于 Linux 服务器安装使用指南05【踩坑记录 | 第一篇】微软商店无法使用时，如何手动安装 OpenAI Codex？附`.msix`文件系统错误解决方法06裂开！ChatGPT 居然开始要手机号验证，附详细解决方法07几个好用的ip纯净度检测网站08【AI】2026 年具身智能模型和世界模型总结09用了半年 OpenRouter，我换到了 Ofox.ai — 两个 AI API 聚合平台的真实对比10codex app每次打开重连5次Reconnecting问题解决