Offboard Control

本文指导如何通过QGroundControl设置RC开关激活场外模式,配置随播计算机接口以实现MAVLink通信,以及介绍三种硬件设置方法:串行无线电、板载处理器和WiFi链接到ROS。适用于希望拓展无人机功能的开发者。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

1、将RC开关映射到场外模式激活

QGroundControl中加载参数并查找RC_MAP_OFFB_SW参数,您可以为其分配要用于激活板外模式的RC通道2


2、启用配套计算机界面

设置默认的随播计算机消息流TELEM 2,请设置以下参数:

为了接收MAVLink,配套计算机需要运行一些与串口通信的软件。最常见的选项是:

 

3、硬件设置

3种设置板外通信的方法

  • 串行无线电
  1. 一个连接到自动驾驶仪的UART端口
  2. 一个连接到地面站计算机

 

  • 板载处理器

  • 板载处理器和wifi链接到ROS

/** * @file uav_fsm_node.cpp * @brief Enhanced UAV Offboard Control with Robust Mode Handling * @version 3.0 * @date 2023-10-15 */ #include <ros/ros.h> #include <geometry_msgs/PoseStamped.h> #include <mavros_msgs/CommandBool.h> #include <mavros_msgs/SetMode.h> #include <mavros_msgs/State.h> #include <mavros_msgs/PositionTarget.h> #include <sensor_msgs/NavSatFix.h> #include <std_srvs/Trigger.h> // 全局变量存储当前状态 mavros_msgs::State current_state; sensor_msgs::NavSatFix global_position; bool has_global_position = false; void state_cb(const mavros_msgs::State::ConstPtr& msg) { current_state = *msg; } void global_position_cb(const sensor_msgs::NavSatFix::ConstPtr& msg) { global_position = *msg; has_global_position = true; } // 检查GPS定位质量 bool check_position_quality() { if (!has_global_position) { ROS_WARN("No global position data available"); return false; } // 检查GPS状态 if (global_position.status.status < sensor_msgs::NavSatStatus::STATUS_FIX) { ROS_WARN_THROTTLE(1.0, "Waiting for GPS fix (current status: %d)", global_position.status.status); return false; } // 检查位置协方差 (低精度时拒绝解锁) if (global_position.position_covariance[0] > 1.0 || global_position.position_covariance[4] > 1.0) { ROS_WARN_THROTTLE(1.0, "GPS accuracy too low (covariance: %.2f, %.2f)", global_position.position_covariance[0], global_position.position_covariance[4]); return false; } return true; } // 等待Offboard模式稳定 bool wait_for_offboard_stable(ros::Duration timeout) { ros::Time start = ros::Time::now(); ros::Rate rate(10); while (ros::ok() && (ros::Time::now() - start < timeout)) { if (current_state.mode == "OFFBOARD") { // 连续5次检查确认模式稳定 int stable_count = 0; for (int i = 0; i < 5; i++) { if (current_state.mode == "OFFBOARD") { stable_count++; } ros::spinOnce(); rate.sleep(); } if (stable_count == 5) { ROS_INFO("OFFBOARD mode stable"); return true; } } ros::spinOnce(); rate.sleep(); } ROS_WARN("Timed out waiting for OFFBOARD mode to stabilize"); return false; } int main(int argc, char **argv) { ros::init(argc, argv, "uav_fsm_node"); ros::NodeHandle nh; // 参数配置 double setpoint_x = 0.0; double setpoint_y = 0.0; double setpoint_z = 2.0; double retry_interval = 2.0; // 重试间隔(秒) bool require_gps_fix = false; // 默认不要求GPS定位(适用于仿真) int offboard_stabilize_time = 2; // Offboard模式稳定时间(秒) // 从参数服务器获取参数 nh.param("uav_fsm_node/setpoint_z", setpoint_z, 2.0); nh.param("uav_fsm_node/require_gps_fix", require_gps_fix, false); nh.param("uav_fsm_node/offboard_stabilize_time", offboard_stabilize_time, 2); ROS_INFO("Target altitude: %.1f meters | GPS required: %s | Stabilize time: %d sec", setpoint_z, require_gps_fix ? "Yes" : "No", offboard_stabilize_time); // 初始化订阅者和发布者 ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State> ("mavros/state", 10, state_cb); ros::Subscriber global_pos_sub = nh.subscribe<sensor_msgs::NavSatFix> ("mavros/global_position/global", 10, global_position_cb); ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped> ("mavros/setpoint_position/local", 10); // 初始化服务客户端 ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool> ("mavros/cmd/arming"); ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode> ("mavros/set_mode"); ros::ServiceClient set_stream_rate_client = nh.serviceClient<mavros_msgs::StreamRate> ("mavros/set_stream_rate"); // 设置MAVLink流速率 if (set_stream_rate_client.exists()) { mavros_msgs::StreamRate stream_rate; stream_rate.request.stream_id = 0; // 所有流 stream_rate.request.message_rate = 50; // 50Hz stream_rate.request.on_off = 1; if (set_stream_rate_client.call(stream_rate) { ROS_INFO("Set MAVLink stream rate to %d Hz", stream_rate.request.message_rate); } else { ROS_WARN("Failed to set MAVLink stream rate"); } } // 设置发布频率 (必须 > 2Hz) ros::Rate rate(30.0); // 提高到30Hz确保稳定 // 等待飞控连接 ROS_INFO("Waiting for FCU connection..."); while (ros::ok() && !current_state.connected) { ros::spinOnce(); rate.sleep(); } ROS_INFO("FCU connected"); // 设置目标位置 geometry_msgs::PoseStamped target_pose; target_pose.header.frame_id = "map"; target_pose.pose.position.x = setpoint_x; target_pose.pose.position.y = setpoint_y; target_pose.pose.position.z = setpoint_z; target_pose.pose.orientation.w = 1.0; // 四元数单位姿态 // 预先发送设定点(200次,确保飞控接收足够) ROS_INFO("Sending initial setpoints..."); for (int i = 200; ros::ok() && i > 0; --i) { target_pose.header.stamp = ros::Time::now(); local_pos_pub.publish(target_pose); ros::spinOnce(); rate.sleep(); } // 准备服务请求 mavros_msgs::SetMode offb_set_mode; offb_set_mode.request.custom_mode = "OFFBOARD"; mavros_msgs::CommandBool arm_cmd; arm_cmd.request.value = true; // 状态变量 enum State { PREFLIGHT, OFFBOARD_REQUEST, OFFBOARD_STABILIZE, ARM_REQUEST, FLYING }; State current_state_machine = PREFLIGHT; ros::Time last_request_time = ros::Time::now(); int arm_attempt_count = 0; const int max_arm_attempts = 10; int offboard_attempt_count = 0; const int max_offboard_attempts = 5; ROS_INFO("Starting state machine..."); while (ros::ok()) { // 持续发布目标点(所有状态都需要) target_pose.header.stamp = ros::Time::now(); local_pos_pub.publish(target_pose); // 状态机处理 switch (current_state_machine) { case PREFLIGHT: // 检查定位质量 if (require_gps_fix && !check_position_quality()) { ROS_WARN_THROTTLE(5.0, "Waiting for position estimate improvement..."); } else if (ros::Time::now() - last_request_time > ros::Duration(retry_interval)) { current_state_machine = OFFBOARD_REQUEST; ROS_INFO("Attempting to switch to OFFBOARD mode..."); last_request_time = ros::Time::now(); offboard_attempt_count = 0; } break; case OFFBOARD_REQUEST: if (set_mode_client.call(offb_set_mode)) { if (offb_set_mode.response.mode_sent) { ROS_INFO("OFFBOARD mode requested, waiting for stabilization..."); current_state_machine = OFFBOARD_STABILIZE; } else { ROS_WARN("OFFBOARD request rejected. Retrying..."); offboard_attempt_count++; } } else { ROS_ERROR("Failed to call set_mode service"); offboard_attempt_count++; } // 如果多次尝试失败,退回PREFLIGHT if (offboard_attempt_count >= max_offboard_attempts) { ROS_ERROR("Max OFFBOARD attempts reached. Returning to PREFLIGHT."); current_state_machine = PREFLIGHT; offboard_attempt_count = 0; last_request_time = ros::Time::now(); } else { last_request_time = ros::Time::now(); } break; case OFFBOARD_STABILIZE: if (current_state.mode == "OFFBOARD") { // 等待模式稳定 if (wait_for_offboard_stable(ros::Duration(offboard_stabilize_time))) { ROS_INFO("OFFBOARD mode stabilized"); current_state_machine = ARM_REQUEST; arm_attempt_count = 0; } else { ROS_WARN("Failed to stabilize OFFBOARD mode. Re-requesting..."); current_state_machine = OFFBOARD_REQUEST; } } else { ROS_WARN("OFFBOARD mode not active. Re-requesting..."); current_state_machine = OFFBOARD_REQUEST; } last_request_time = ros::Time::now(); break; case ARM_REQUEST: // 确保仍然在OFFBOARD模式 if (current_state.mode != "OFFBOARD") { ROS_WARN("Lost OFFBOARD mode. Reverting to mode switch..."); current_state_machine = OFFBOARD_REQUEST; last_request_time = ros::Time::now(); break; } // 检查定位质量 if (require_gps_fix && !check_position_quality()) { ROS_WARN_THROTTLE(1.0, "Position estimate not ready for arming"); last_request_time = ros::Time::now(); // 重置计时器 break; } // 尝试解锁 if (ros::Time::now() - last_request_time > ros::Duration(retry_interval)) { arm_attempt_count++; if (arming_client.call(arm_cmd)) { if (arm_cmd.response.success) { ROS_INFO("Vehicle armed successfully"); current_state_machine = FLYING; } else { ROS_WARN("Arming command rejected (Attempt %d/%d). Reason: %s", arm_attempt_count, max_arm_attempts, arm_cmd.response.result == 0 ? "SUCCESS" : arm_cmd.response.result == 1 ? "TEMPORARILY_REJECTED" : arm_cmd.response.result == 2 ? "DENIED" : arm_cmd.response.result == 3 ? "FAILED" : "UNKNOWN"); } } else { ROS_ERROR("Failed to call arming service"); } // 如果多次尝试失败,退回PREFLIGHT if (arm_attempt_count >= max_arm_attempts) { ROS_ERROR("Max arming attempts reached. Returning to PREFLIGHT."); current_state_machine = PREFLIGHT; arm_attempt_count = 0; } last_request_time = ros::Time::now(); } break; case FLYING: // 安全监控 if (!current_state.armed) { ROS_ERROR("Vehicle disarmed unexpectedly!"); current_state_machine = ARM_REQUEST; last_request_time = ros::Time::now(); } else if (current_state.mode != "OFFBOARD") { ROS_WARN("Exited OFFBOARD mode unexpectedly!"); current_state_machine = OFFBOARD_REQUEST; last_request_time = ros::Time::now(); } // 主飞行控制循环 - 在此添加航点导航代码 break; } ros::spinOnce(); rate.sleep(); } ROS_INFO("Node shutdown"); return 0; } 运行后报错[INFO] [1754118580.931778963, 442.467000000]: OFFBOARD mode enabled [WARN] [1754118581.056115029, 442.516000000]: Lost OFFBOARD mode. Reverting to mode switch... [INFO] [1754118581.173202117, 442.568000000]: OFFBOARD mode enabled [WARN] [1754118585.620143284, 444.628000000]: Arming command rejected. Retrying...
最新发布
08-03
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值