diff --git a/doc/“智能伤员感知救助”软件设计规格说明书.docx b/doc/“智能伤员感知救助”软件设计规格说明书.docx
index 9ba0ed0e..8831a8eb 100644
Binary files a/doc/“智能伤员感知救助”软件设计规格说明书.docx and b/doc/“智能伤员感知救助”软件设计规格说明书.docx differ
diff --git a/doc/“智能伤员感知救助”软件需求规格说明书.docx b/doc/“智能伤员感知救助”软件需求规格说明书.docx
index b57311de..b4ce2532 100644
Binary files a/doc/“智能伤员感知救助”软件需求规格说明书.docx and b/doc/“智能伤员感知救助”软件需求规格说明书.docx differ
diff --git a/model/“智能伤员感知救助”软件需求模型.vsdx b/model/“智能伤员感知救助”软件需求模型.vsdx
index bc2ebe23..49127741 100644
Binary files a/model/“智能伤员感知救助”软件需求模型.vsdx and b/model/“智能伤员感知救助”软件需求模型.vsdx differ
diff --git a/src/无人机飞行控制代码/UAV_control.cpp b/src/无人机飞行控制代码/UAV_control.cpp
new file mode 100644
index 00000000..7e07cda7
--- /dev/null
+++ b/src/无人机飞行控制代码/UAV_control.cpp
@@ -0,0 +1,694 @@
+#include "uav_estimator.h"
+
+UAV_estimator::UAV_estimator(ros::NodeHandle &nh)
+{
+    // 【参数】编号
+    nh.param<int>("uav_id", uav_id, 1);
+    // 【参数】定位源, 定义见UAVState.msg
+    nh.param<int>("control/location_source", location_source, prometheus_msgs::UAVState::GPS);
+    // 【参数】最大安全速度
+    nh.param<float>("control/maximum_safe_vel_xy", maximum_safe_vel_xy, 4.0f);
+    nh.param<float>("control/maximum_safe_vel_z", maximum_safe_vel_z, 3.0f);
+    // 【参数】最大vision/px4速度误差
+    nh.param<float>("control/maximum_vel_error_for_vision", maximum_vel_error_for_vision, 1.0f);
+
+    // 【变量】无人机名字
+    uav_name = "/uav" + std::to_string(uav_id);
+    // 【变量】节点名字
+    node_name = "[uav_estimator_uav" + std::to_string(uav_id) + "]";
+
+    // 【订阅】无人机当前状态 - 来自飞控
+    px4_state_sub = nh.subscribe<mavros_msgs::State>(uav_name + "/mavros/state", 1, &UAV_estimator::px4_state_cb, this);
+
+    // 【订阅】无人机电池状态 - 来自飞控
+    px4_battery_sub = nh.subscribe<sensor_msgs::BatteryState>(uav_name + "/mavros/battery", 1, &UAV_estimator::px4_battery_cb, this);
+
+    // 【订阅】无人机当前位置 坐标系:ENU系  - 来自飞控
+    // 【备注】所有状态量在飞控中均为NED系，但在ros中mavros将其转换为ENU系处理。所以，在ROS中，所有和mavros交互的量都为ENU系
+    px4_position_sub = nh.subscribe<geometry_msgs::PoseStamped>(uav_name + "/mavros/local_position/pose", 1, &UAV_estimator::px4_pos_cb, this);
+
+    // 【订阅】无人机当前速度 坐标系:ENU系 - 来自飞控
+    px4_velocity_sub = nh.subscribe<geometry_msgs::TwistStamped>(uav_name + "/mavros/local_position/velocity_local", 1, &UAV_estimator::px4_vel_cb, this);
+
+    // 【订阅】无人机当前欧拉角 坐标系:ENU系 - 来自飞控
+    px4_attitude_sub = nh.subscribe<sensor_msgs::Imu>(uav_name + "/mavros/imu/data", 1, &UAV_estimator::px4_att_cb, this);
+
+    // 根据设定的定位来源订阅不同的定位数据
+    if (location_source == prometheus_msgs::UAVState::MOCAP)
+    {
+        // 【订阅】mocap估计位置
+        mocap_sub = nh.subscribe<geometry_msgs::PoseStamped>("/vrpn_client_node" + uav_name + "/pose", 1, &UAV_estimator::mocap_cb, this);
+    }
+    else if (location_source == prometheus_msgs::UAVState::T265)
+    {
+        // 【订阅】T265估计位置
+    }
+    else if (location_source == prometheus_msgs::UAVState::GAZEBO)
+    {
+        // 【订阅】gazebo仿真真值
+        gazebo_sub = nh.subscribe<nav_msgs::Odometry>(uav_name + "/prometheus/ground_truth", 1, &UAV_estimator::gazebo_cb, this);
+    }
+    else if (location_source == prometheus_msgs::UAVState::FAKE_ODOM)
+    {
+        // 【订阅】fake odom
+        fake_odom_sub = nh.subscribe<nav_msgs::Odometry>(uav_name + "/prometheus/fake_odom", 10, &UAV_estimator::fake_odom_cb, this);
+    }
+    else if (location_source == prometheus_msgs::UAVState::GPS || location_source == prometheus_msgs::UAVState::RTK)
+    {
+        // 【订阅】GPS状态，来自飞控
+        gps_status_sub = nh.subscribe<mavros_msgs::GPSRAW>(uav_name + "/mavros/gpsstatus/gps1/raw", 10, &UAV_estimator::gps_status_cb, this);
+        // 【订阅】无人机当前经纬度，来自飞控
+        px4_global_position_sub = nh.subscribe<sensor_msgs::NavSatFix>(uav_name + "/mavros/global_position/global", 1, &UAV_estimator::px4_global_pos_cb, this);
+        // 【订阅】设置ENU坐标系下无人机的位置偏移量  坐标系:ENU系 - 来自地面站/终端窗口
+        set_local_pose_offset_sub = nh.subscribe<prometheus_msgs::GPSData>(uav_name + "/prometheus/set_local_offset_pose", 1, &UAV_estimator::set_local_pose_offset_cb, this);
+        // 【发布】ENU坐标系下的位置偏移量
+        local_pose_offset_pub = nh.advertise<prometheus_msgs::OffsetPose>(uav_name + "/prometheus/offset_pose", 1);
+    }
+    else if (location_source == prometheus_msgs::UAVState::UWB)
+    {
+        // uwb todo
+    }
+    else
+    {
+        cout << YELLOW << node_name << ": wrong location_source param, no external location information input!" << TAIL << endl;
+    }
+
+    // 【发布】无人机状态合集,包括位置\速度\姿态\模式等,供上层节点使用
+    uav_state_pub = nh.advertise<prometheus_msgs::UAVState>(uav_name + "/prometheus/state", 1);
+
+    // 【发布】无人机位置和偏航角，传输至PX4_EKF2模块用于位置姿态估计 坐标系 ENU系
+    px4_vision_pose_pub = nh.advertise<geometry_msgs::PoseStamped>(uav_name + "/mavros/vision_pose/pose", 1);
+
+    // 【发布】无人机里程计,主要用于RVIZ显示
+    uav_odom_pub = nh.advertise<nav_msgs::Odometry>(uav_name + "/prometheus/odom", 1);
+
+    // 【发布】无人机运动轨迹,主要用于RVIZ显示
+    uav_trajectory_pub = nh.advertise<nav_msgs::Path>(uav_name + "/prometheus/trajectory", 1);
+
+    // 【发布】无人机位置(带图标),用于RVIZ显示
+    uav_mesh_pub = nh.advertise<visualization_msgs::Marker>(uav_name + "/prometheus/uav_mesh", 1);
+
+    // 【发布】运行状态信息(-> 通信节点 -> 地面站)
+    ground_station_info_pub = nh.advertise<prometheus_msgs::TextInfo>("/uav" + std::to_string(uav_id) + "/prometheus/text_info", 1);
+
+    if (location_source == prometheus_msgs::UAVState::MOCAP || location_source == prometheus_msgs::UAVState::T265 || location_source == prometheus_msgs::UAVState::GAZEBO)
+    {
+        // 【定时器】当需要使用外部定位设备时，需要定时发送vision信息至飞控,并保证一定频率
+        timer_px4_vision_pub = nh.createTimer(ros::Duration(0.02), &UAV_estimator::timercb_pub_vision_pose, this);
+    }
+
+    // 【定时器】定时发布 uav_state, uav_odom 保证50Hz以上
+    timer_uav_state_pub = nh.createTimer(ros::Duration(0.02), &UAV_estimator::timercb_pub_uav_state, this);
+
+    // 【定时器】定时发布 rviz显示,保证1Hz以上
+    timer_rviz_pub = nh.createTimer(ros::Duration(0.05), &UAV_estimator::timercb_rviz, this);
+
+    // 变量初始化
+    uav_state.uav_id = uav_id;
+    uav_state.connected = false;
+    uav_state.armed = false;
+    uav_state.mode = "";
+    uav_state.location_source = location_source;
+    uav_state.odom_valid = false;
+    uav_state.gps_status = prometheus_msgs::UAVState::GPS_FIX_TYPE_NO_GPS;
+    uav_state.position[0] = 0.0;
+    uav_state.position[1] = 0.0;
+    uav_state.position[2] = 0.0;
+    //该经纬度为阿木实验室测试场地(小花园)的经纬度
+    uav_state.latitude = 30.7852600;
+    uav_state.longitude = 103.8610300;
+    uav_state.altitude = 100.0;
+    uav_state.velocity[0] = 0.0;
+    uav_state.velocity[1] = 0.0;
+    uav_state.velocity[2] = 0.0;
+    uav_state.attitude_q.w = 1.0;
+    uav_state.attitude_q.x = 0.0;
+    uav_state.attitude_q.y = 0.0;
+    uav_state.attitude_q.z = 0.0;
+    uav_state.attitude[0] = 0.0;
+    uav_state.attitude[1] = 0.0;
+    uav_state.attitude[2] = 0.0;
+    uav_state.attitude_rate[0] = 0.0;
+    uav_state.attitude_rate[1] = 0.0;
+    uav_state.attitude_rate[2] = 0.0;
+    uav_state.battery_state = 0.0;
+    uav_state.battery_percetage = 0.0;
+
+    offset_pose.x = 0.0;
+    offset_pose.y = 0.0;
+
+    // 【函数】打印参数
+    printf_param();
+    cout << GREEN << node_name << " init! " << TAIL << endl;
+
+    // 地面站消息打印
+    text_info.header.stamp = ros::Time::now();
+    text_info.MessageType = prometheus_msgs::TextInfo::INFO;
+    text_info.Message = node_name + " init.";
+    ground_station_info_pub.publish(text_info);
+}
+
+void UAV_estimator::timercb_pub_uav_state(const ros::TimerEvent &e)
+{
+    if (!uav_state_update)
+    {
+        return;
+    }
+
+    // 无人机状态检查：
+    // 1，检查odom状态
+    // 2，待补充
+    check_uav_state();
+
+    // 发布uav_state
+    uav_state.header.stamp = ros::Time::now();
+    uav_state_pub.publish(uav_state);
+
+    // 发布无人机当前odometry(有些节点需要Odometry这个数据类型)
+    uav_odom.header.stamp = ros::Time::now();
+    uav_odom.header.frame_id = "world";
+    uav_odom.child_frame_id = "base_link";
+    uav_odom.pose.pose.position.x = uav_state.position[0];
+    uav_odom.pose.pose.position.y = uav_state.position[1];
+    uav_odom.pose.pose.position.z = uav_state.position[2];
+    // 导航算法规定 高度不能小于0
+    if (uav_odom.pose.pose.position.z <= 0)
+    {
+        uav_odom.pose.pose.position.z = 0.01;
+    }
+    uav_odom.pose.pose.orientation = uav_state.attitude_q;
+    uav_odom.twist.twist.linear.x = uav_state.velocity[0];
+    uav_odom.twist.twist.linear.y = uav_state.velocity[1];
+    uav_odom.twist.twist.linear.z = uav_state.velocity[2];
+    uav_odom_pub.publish(uav_odom);
+}
+
+void UAV_estimator::timercb_pub_vision_pose(const ros::TimerEvent &e)
+{
+    if (location_source == prometheus_msgs::UAVState::GAZEBO)
+    {
+        vision_pose = gazebo_pose;
+    }
+    else if (location_source == prometheus_msgs::UAVState::MOCAP)
+    {
+        vision_pose = mocap_pose;
+    }
+    else if (location_source == prometheus_msgs::UAVState::T265)
+    {
+        vision_pose = t265_pose;
+    }
+    else
+    {
+        vision_pose_error = true;
+        return;
+    }
+
+    Eigen::Vector3d pos_vision = Eigen::Vector3d(vision_pose.pose.position.x, vision_pose.pose.position.y, vision_pose.pose.position.z);
+    Eigen::Vector3d pos_px4 = Eigen::Vector3d(uav_state.position[0], uav_state.position[1], uav_state.position[2]);
+
+    // vision位置和px4回传位置相差较多，一般是PX4中EKF2参数设置错误导致PX4没有收到vision定位数据导致
+    // 无人机发生剧烈位移，也会出现本错误，这个需要根据实际测试结果来确定
+    if ((pos_vision - pos_px4).norm() > maximum_vel_error_for_vision)
+    {
+        vision_pose_error = true;
+    }
+    else
+    {
+        vision_pose_error = false;
+    }
+
+    px4_vision_pose_pub.publish(vision_pose);
+}
+
+void UAV_estimator::timercb_rviz(const ros::TimerEvent &e)
+{
+    // 发布无人机运动轨迹，用于rviz显示
+    geometry_msgs::PoseStamped uav_pos;
+    uav_pos.header.stamp = ros::Time::now();
+    uav_pos.header.frame_id = "world";
+    uav_pos.pose.position.x = uav_state.position[0];
+    uav_pos.pose.position.y = uav_state.position[1];
+    uav_pos.pose.position.z = uav_state.position[2];
+    uav_pos.pose.orientation = uav_state.attitude_q;
+    pos_vector.insert(pos_vector.begin(), uav_pos);
+    if (pos_vector.size() > TRA_WINDOW)
+    {
+        pos_vector.pop_back();
+    }
+    nav_msgs::Path uav_trajectory;
+    uav_trajectory.header.stamp = ros::Time::now();
+    uav_trajectory.header.frame_id = "world";
+    uav_trajectory.poses = pos_vector;
+    uav_trajectory_pub.publish(uav_trajectory);
+
+    // 发布无人机marker
+    visualization_msgs::Marker meshROS;
+    meshROS.header.frame_id = "world";
+    meshROS.header.stamp = ros::Time::now();
+    meshROS.ns = "mesh";
+    meshROS.id = 0;
+    meshROS.type = visualization_msgs::Marker::MESH_RESOURCE;
+    meshROS.action = visualization_msgs::Marker::ADD;
+    meshROS.pose.position.x = uav_state.position[0];
+    meshROS.pose.position.y = uav_state.position[1];
+    meshROS.pose.position.z = uav_state.position[2];
+    meshROS.pose.orientation.w = uav_state.attitude_q.w;
+    meshROS.pose.orientation.x = uav_state.attitude_q.x;
+    meshROS.pose.orientation.y = uav_state.attitude_q.y;
+    meshROS.pose.orientation.z = uav_state.attitude_q.z;
+    meshROS.scale.x = 1.0;
+    meshROS.scale.y = 1.0;
+    meshROS.scale.z = 1.0;
+    meshROS.color.a = 1.0;
+    meshROS.color.r = 0.0;
+    meshROS.color.g = 0.0;
+    meshROS.color.b = 1.0;
+    meshROS.mesh_resource = std::string("package://prometheus_uav_control/meshes/hummingbird.mesh");
+    uav_mesh_pub.publish(meshROS);
+
+    // 发布TF用于RVIZ显示（用于lidar）
+    static tf2_ros::TransformBroadcaster broadcaster;
+    geometry_msgs::TransformStamped tfs;
+    //  |----头设置
+    tfs.header.frame_id = "world";       //相对于世界坐标系
+    tfs.header.stamp = ros::Time::now(); //时间戳
+    //  |----坐标系 ID
+    tfs.child_frame_id = uav_name + "/lidar_link"; //子坐标系，无人机的坐标系
+    //  |----坐标系相对信息设置  偏移量  无人机相对于世界坐标系的坐标
+    tfs.transform.translation.x = uav_state.position[0];
+    tfs.transform.translation.y = uav_state.position[1];
+    tfs.transform.translation.z = uav_state.position[2];
+    //  |--------- 四元数设置
+    tfs.transform.rotation = uav_state.attitude_q;
+    //  |--------- 广播器发布数据
+    broadcaster.sendTransform(tfs);
+}
+
+void UAV_estimator::px4_state_cb(const mavros_msgs::State::ConstPtr &msg)
+{
+    uav_state.connected = msg->connected;
+    uav_state.armed = msg->armed;
+    uav_state.mode = msg->mode;
+    uav_state_update = true;
+}
+
+void UAV_estimator::px4_pos_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
+{
+    // 统一坐标系（RTK情况下，可以设置offset_pose，其他情况offset_pose为零）
+    uav_state.position[0] = msg->pose.position.x + offset_pose.x;
+    uav_state.position[1] = msg->pose.position.y + offset_pose.y;
+    uav_state.position[2] = msg->pose.position.z;
+}
+
+void UAV_estimator::px4_global_pos_cb(const sensor_msgs::NavSatFix::ConstPtr &msg)
+{
+    uav_state.latitude = msg->latitude;
+    uav_state.longitude = msg->longitude;
+    uav_state.altitude = msg->altitude;
+}
+
+void UAV_estimator::px4_vel_cb(const geometry_msgs::TwistStamped::ConstPtr &msg)
+{
+    uav_state.velocity[0] = msg->twist.linear.x;
+    uav_state.velocity[1] = msg->twist.linear.y;
+    uav_state.velocity[2] = msg->twist.linear.z;
+}
+
+void UAV_estimator::px4_att_cb(const sensor_msgs::Imu::ConstPtr &msg)
+{
+    Eigen::Quaterniond q_fcu = Eigen::Quaterniond(msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z);
+    Eigen::Vector3d euler_fcu = quaternion_to_euler(q_fcu);
+
+    uav_state.attitude_q = msg->orientation;
+    uav_state.attitude[0] = euler_fcu[0];
+    uav_state.attitude[1] = euler_fcu[1];
+    uav_state.attitude[2] = euler_fcu[2];
+    uav_state.attitude_rate[0] = msg->angular_velocity.x;
+    uav_state.attitude_rate[1] = msg->angular_velocity.y;
+    uav_state.attitude_rate[2] = msg->angular_velocity.z;
+}
+
+void UAV_estimator::px4_battery_cb(const sensor_msgs::BatteryState::ConstPtr &msg)
+{
+    uav_state.battery_state = msg->voltage;
+    uav_state.battery_percetage = msg->percentage;
+}
+
+void UAV_estimator::gps_status_cb(const mavros_msgs::GPSRAW::ConstPtr &msg)
+{
+    uav_state.gps_status = msg->fix_type;
+}
+
+void UAV_estimator::mocap_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
+{
+    mocap_pose = *msg;
+    get_mocap_stamp = ros::Time::now(); // 记录时间戳，防止超时
+}
+
+void UAV_estimator::gazebo_cb(const nav_msgs::Odometry::ConstPtr &msg)
+{
+    gazebo_pose.header = msg->header;
+    gazebo_pose.pose = msg->pose.pose;
+    get_gazebo_stamp = ros::Time::now(); // 记录时间戳，防止超时
+}
+
+void UAV_estimator::t265_cb(const nav_msgs::Odometry::ConstPtr &msg)
+{
+    t265_pose.header = msg->header;
+    t265_pose.pose = msg->pose.pose;
+    get_t265_stamp = ros::Time::now(); // 记录时间戳，防止超时
+}
+
+void UAV_estimator::fake_odom_cb(const nav_msgs::Odometry::ConstPtr &msg)
+{
+    // uav_state 直接赋值
+    uav_state.connected = true;
+    uav_state.armed = true;
+    uav_state.odom_valid = true;
+    uav_state.mode = "OFFBOARD";
+    uav_state.position[0] = msg->pose.pose.position.x;
+    uav_state.position[1] = msg->pose.pose.position.y;
+    uav_state.position[2] = msg->pose.pose.position.z;
+    uav_state.velocity[0] = msg->twist.twist.linear.x;
+    uav_state.velocity[1] = msg->twist.twist.linear.y;
+    uav_state.velocity[2] = msg->twist.twist.linear.z;
+    uav_state.attitude_q = msg->pose.pose.orientation;
+    Eigen::Quaterniond q = Eigen::Quaterniond(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z);
+    Eigen::Vector3d euler = quaternion_to_euler(q);
+    uav_state.attitude[0] = euler[0];
+    uav_state.attitude[1] = euler[1];
+    uav_state.attitude[2] = euler[2];
+    uav_state.attitude_rate[0] = 0.0;
+    uav_state.attitude_rate[1] = 0.0;
+    uav_state.attitude_rate[2] = 0.0;
+
+    uav_state_update = true;
+}
+
+void UAV_estimator::check_uav_state()
+{
+    // 检查odom状态
+    odom_state = check_uav_odom();
+
+    if (odom_state == 1 && last_odom_state != 1)
+    {
+        cout << RED << node_name << "--->  Odom invalid: Get Vision Pose Timeout! " << TAIL << endl;
+    }
+    else if (odom_state == 2 && last_odom_state != 2)
+    {
+        cout << RED << node_name << "--->  Odom invalid: Velocity too large! " << TAIL << endl;
+    }
+    else if (odom_state == 3 && last_odom_state != 3)
+    {
+        cout << RED << node_name << "--->  Odom invalid: vision_pose_error! " << TAIL << endl;
+    }
+    else if (odom_state == 4 && last_odom_state != 4)
+    {
+        cout << RED << node_name << "--->  Odom invalid: GPS/RTK location error! " << TAIL << endl;
+    }
+    else if (odom_state == 5 && last_odom_state != 5)
+    {
+        cout << YELLOW << node_name << "--->  Odom invalid: RTK not fixed! " << TAIL << endl;
+    }
+
+    if (odom_state == 9 || odom_state == 5)
+    {
+        uav_state.odom_valid = true;
+    }
+    else
+    {
+        uav_state.odom_valid = false;
+    }
+    last_odom_state = odom_state;
+}
+
+int UAV_estimator::check_uav_odom()
+{
+    ros::Time time_now = ros::Time::now();
+
+    // odom失效可能原因1：外部定位数据接收超时
+    if (location_source == prometheus_msgs::UAVState::GAZEBO && (time_now - get_gazebo_stamp).toSec() > GAZEBO_TIMEOUT)
+    {
+        return 1;
+    }
+    else if (location_source == prometheus_msgs::UAVState::MOCAP && (time_now - get_mocap_stamp).toSec() > MOCAP_TIMEOUT)
+    {
+        return 1;
+    }
+    else if (location_source == prometheus_msgs::UAVState::T265 && (time_now - get_t265_stamp).toSec() > T265_TIMEOUT)
+    {
+        return 1;
+    }
+
+    // odom失效可能原因2：无人机合速度过大，认为定位模块失效
+    Eigen::Vector2d uav_vel_xy = Eigen::Vector2d(uav_state.velocity[0], uav_state.velocity[1]);
+    if (uav_vel_xy.norm() > maximum_safe_vel_xy || uav_state.velocity[2] > maximum_safe_vel_z)
+    {
+        return 2;
+    }
+
+    // odom失效可能原因3：无人机位置与外部定位设备原始值相差过多
+    if (vision_pose_error)
+    {
+        return 3;
+    }
+
+    // odom失效可能原因4:GPS定位模块数据异常,无法获取定位数据
+    if (location_source == prometheus_msgs::UAVState::GPS)
+    {
+        if (uav_state.gps_status != prometheus_msgs::UAVState::GPS_FIX_TYPE_3D_FIX)
+        {
+            return 4;
+        }
+    }
+
+    if (location_source == prometheus_msgs::UAVState::RTK)
+    {
+        if (uav_state.gps_status < prometheus_msgs::UAVState::GPS_FIX_TYPE_3D_FIX)
+        {
+            return 4;
+        }
+        // odom数据可信度降低可能原因5:RTK定位精度未达到FIXED状态(非odom失效状态)
+        else if (uav_state.gps_status <= prometheus_msgs::UAVState::GPS_FIX_TYPE_RTK_FLOATR)
+        {
+            return 5;
+        }
+    }
+    //UWB todo
+
+    return 9;
+}
+
+void UAV_estimator::printf_uav_state()
+{
+    cout << GREEN << ">>>>>>>>>>>>>>>>>>>> UAV [" << uav_id << "] State  <<<<<<<<<<<<<<<<<<<<" << TAIL << endl;
+    //固定的浮点显示
+    cout.setf(ios::fixed);
+    // setprecision(n) 设显示小数精度为n位
+    cout << setprecision(NUM_POINT);
+    //左对齐
+    cout.setf(ios::left);
+    // 强制显示小数点
+    cout.setf(ios::showpoint);
+    // 强制显示符号
+    cout.setf(ios::showpos);
+
+    // 打印 无人机状态
+    if (uav_state.connected == true)
+    {
+        cout << GREEN << "PX4 Status:  [ Connected ] ";
+    }
+    else
+    {
+        cout << RED << "PX4 Status:[ Unconnected ] ";
+    }
+    //是否上锁
+    if (uav_state.armed == true)
+    {
+        cout << GREEN << "[  Armed   ] ";
+    }
+    else
+    {
+        cout << RED << "[ DisArmed ] ";
+    }
+
+    cout << "[ " << uav_state.mode << " ] " << TAIL << endl;
+
+    switch (location_source)
+    {
+    case prometheus_msgs::UAVState::MOCAP:
+        cout << GREEN << "Location: [ MOCAP ] " << TAIL << endl;
+        cout << GREEN << "MOCAP_pos [X Y Z] : " << mocap_pose.pose.position.x << " [ m ] " << mocap_pose.pose.position.y << " [ m ] " << mocap_pose.pose.position.z << " [ m ] " << TAIL << endl;
+        break;
+    case prometheus_msgs::UAVState::T265:
+        cout << GREEN << "Location: [ T265 ] " << TAIL << endl;
+        cout << GREEN << "T265_pos [X Y Z] : " << t265_pose.pose.position.x << " [ m ] " << t265_pose.pose.position.y << " [ m ] " << t265_pose.pose.position.z << " [ m ] " << TAIL << endl;
+        break;
+    case prometheus_msgs::UAVState::GAZEBO:
+        cout << GREEN << "Location: [ GAZEBO ] " << TAIL << endl;
+        cout << GREEN << "GAZEBO_pos [X Y Z] : " << gazebo_pose.pose.position.x << " [ m ] " << gazebo_pose.pose.position.y << " [ m ] " << gazebo_pose.pose.position.z << " [ m ] " << TAIL << endl;
+        break;
+    case prometheus_msgs::UAVState::FAKE_ODOM:
+        cout << GREEN << "Location: [ FAKE_ODOM ] " << TAIL << endl;
+        break;
+    case prometheus_msgs::UAVState::GPS:
+        cout << GREEN << "Location: [ GPS ] " << TAIL;
+        printf_gps_status();
+        break;
+    case prometheus_msgs::UAVState::RTK:
+        cout << GREEN << "Location: [ RTK ] " << TAIL;
+        printf_gps_status();
+    case prometheus_msgs::UAVState::UWB:
+        cout << GREEN << "Location: [ UWB ] " << TAIL;
+        // todo
+        break;
+    }
+
+    if (uav_state.odom_valid)
+    {
+        cout << GREEN << "Odom Status     : [ Valid ] " << TAIL << endl;
+    }
+    else
+    {
+        cout << RED << "Odom Status     : [ Invalid ] " << TAIL << endl;
+    }
+
+    cout << GREEN << "UAV_pos [X Y Z] : " << uav_state.position[0] << " [ m ] " << uav_state.position[1] << " [ m ] " << uav_state.position[2] << " [ m ] " << TAIL << endl;
+    cout << GREEN << "UAV_vel [X Y Z] : " << uav_state.velocity[0] << " [m/s] " << uav_state.velocity[1] << " [m/s] " << uav_state.velocity[2] << " [m/s] " << TAIL << endl;
+    cout << GREEN << "UAV_att [R P Y] : " << uav_state.attitude[0] * 180 / M_PI << " [deg] " << uav_state.attitude[1] * 180 / M_PI << " [deg] " << uav_state.attitude[2] * 180 / M_PI << " [deg] " << TAIL << endl;
+
+    cout << GREEN << "Battery Voltage : " << uav_state.battery_state << " [V] "
+         << "  Battery Percent : " << uav_state.battery_percetage << TAIL << endl;
+}
+
+void UAV_estimator::printf_gps_status()
+{
+    // 确认一下，哪些是红色，哪些是绿色，todo...
+    if (location_source == prometheus_msgs::UAVState::GPS)
+    {
+        switch (uav_state.gps_status)
+        {
+        case prometheus_msgs::UAVState::GPS_FIX_TYPE_NO_GPS:
+            cout << RED << " [GPS_FIX_TYPE_NO_GPS] " << TAIL << endl;
+            break;
+        case prometheus_msgs::UAVState::GPS_FIX_TYPE_NO_FIX:
+            cout << RED << " [GPS_FIX_TYPE_NO_FIX] " << TAIL << endl;
+            break;
+        case prometheus_msgs::UAVState::GPS_FIX_TYPE_2D_FIX:
+            cout << YELLOW << " [GPS_FIX_TYPE_2D_FIX] " << TAIL << endl;
+            break;
+        case prometheus_msgs::UAVState::GPS_FIX_TYPE_3D_FIX:
+            cout << GREEN << " [GPS_FIX_TYPE_3D_FIX] " << TAIL << endl;
+            break;
+        }
+    }
+    if (location_source == prometheus_msgs::UAVState::RTK)
+    {
+        switch (uav_state.gps_status)
+        {
+        case prometheus_msgs::UAVState::GPS_FIX_TYPE_NO_GPS:
+            cout << RED << " [GPS_FIX_TYPE_NO_GPS] " << TAIL << endl;
+            break;
+        case prometheus_msgs::UAVState::GPS_FIX_TYPE_NO_FIX:
+            cout << RED << " [GPS_FIX_TYPE_NO_FIX] " << TAIL << endl;
+            break;
+        case prometheus_msgs::UAVState::GPS_FIX_TYPE_2D_FIX:
+            cout << RED << " [GPS_FIX_TYPE_2D_FIX] " << TAIL << endl;
+            break;
+        case prometheus_msgs::UAVState::GPS_FIX_TYPE_3D_FIX:
+            cout << YELLOW << " [GPS_FIX_TYPE_3D_FIX] " << TAIL << endl;
+            break;
+        case prometheus_msgs::UAVState::GPS_FIX_TYPE_DGPS:
+            cout << YELLOW << " [GPS_FIX_TYPE_DGPS] " << TAIL << endl;
+            break;
+        case prometheus_msgs::UAVState::GPS_FIX_TYPE_RTK_FLOATR:
+            cout << YELLOW << " [GPS_FIX_TYPE_RTK_FLOATR] " << TAIL << endl;
+            break;
+        case prometheus_msgs::UAVState::GPS_FIX_TYPE_RTK_FIXEDR:
+            cout << GREEN << " [GPS_FIX_TYPE_RTK_FIXEDR] " << TAIL << endl;
+            break;
+        }
+        // offset_pose
+        cout << GREEN << "offset_pose [X Y] : " << offset_pose.x << " [ m ] " << offset_pose.y << " [ m ] " << TAIL << endl;
+    }
+
+    // 确定下单位，todo
+    cout << GREEN << "GPS [lat lon alt] : " << uav_state.latitude << " [ deg ] " << uav_state.longitude << " [ deg ] " << uav_state.altitude << " [ m ] " << TAIL << endl;
+}
+
+void UAV_estimator::printf_param()
+{
+    cout << GREEN << ">>>>>>>>>>>>>>>> UAV estimator Param <<<<<<<<<<<<<<<<" << TAIL << endl;
+    cout << GREEN << "uav_id                        : " << uav_id << " " << TAIL << endl;
+    cout << GREEN << "maximum_safe_vel_xy           : " << maximum_safe_vel_xy << " [m/s] " << TAIL << endl;
+    cout << GREEN << "maximum_safe_vel_z            : " << maximum_safe_vel_z << " [m/s] " << TAIL << endl;
+    cout << GREEN << "maximum_vel_error_for_vision  : " << maximum_vel_error_for_vision << " [m/s] " << TAIL << endl;
+
+    if (location_source == prometheus_msgs::UAVState::GAZEBO)
+    {
+        cout << GREEN << "location_source: [GAZEBO] " << TAIL << endl;
+    }
+    else if (location_source == prometheus_msgs::UAVState::MOCAP)
+    {
+        cout << GREEN << "location_source: [MOCAP] " << TAIL << endl;
+    }
+    else if (location_source == prometheus_msgs::UAVState::T265)
+    {
+        cout << GREEN << "location_source: [T265] " << TAIL << endl;
+    }
+    else if (location_source == prometheus_msgs::UAVState::FAKE_ODOM)
+    {
+        cout << GREEN << "location_source: [FAKE_ODOM] " << TAIL << endl;
+    }
+    else if (location_source == prometheus_msgs::UAVState::GPS)
+    {
+        cout << GREEN << "location_source: [GPS] " << TAIL << endl;
+    }
+    else if (location_source == prometheus_msgs::UAVState::RTK)
+    {
+        cout << GREEN << "location_source: [GPS] " << TAIL << endl;
+    }
+    else if (location_source == prometheus_msgs::UAVState::UWB)
+    {
+        cout << GREEN << "location_source: [GPS] " << TAIL << endl;
+    }
+    else
+    {
+        cout << GREEN << "location_source: [UNKNOW] " << endl;
+    }
+}
+
+void UAV_estimator::set_local_pose_offset_cb(const prometheus_msgs::GPSData::ConstPtr &msg)
+{
+    GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
+    Eigen::Vector3d origin_gps;
+    Eigen::Vector3d origin_ecef;
+    Eigen::Vector3d current_uav_gps;
+    Eigen::Vector3d current_uav_ecef;
+    Eigen::Vector3d ecef_offset;
+    Eigen::Vector3d enu_offset;
+
+    origin_gps[0] = msg->latitude;
+    origin_gps[1] = msg->longitude;
+    origin_gps[2] = msg->altitude;
+
+    current_uav_gps[0] = uav_state.latitude;
+    current_uav_gps[1] = uav_state.longitude;
+    current_uav_gps[2] = uav_state.altitude;
+
+    earth.Forward(origin_gps[0], origin_gps[1], origin_gps[2], origin_ecef[0], origin_ecef[1], origin_ecef[2]);
+    earth.Forward(current_uav_gps[0], current_uav_gps[1], current_uav_gps[2], current_uav_ecef[0], current_uav_ecef[1], current_uav_ecef[2]);
+
+    ecef_offset = current_uav_ecef - origin_ecef;
+    enu_offset = mavros::ftf::transform_frame_ecef_enu(ecef_offset, origin_gps);
+
+    // estimator节点根据GPS初始位置解算得到初始偏差，只发布一次
+    // 发布出来的uav_state，在uav1的坐标系下
+    // 每个飞机的local_position还是在起飞坐标系
+    // 所以发布控制指令也要加上偏差
+    offset_pose.uav_id = uav_id;
+    offset_pose.x = enu_offset[0];
+    offset_pose.y = enu_offset[1];
+
+    local_pose_offset_pub.publish(offset_pose);
+}
diff --git a/其他/宣传彩页-智能伤员感知救助系统.jpg b/其他/宣传彩页-智能伤员感知救助系统.jpg
new file mode 100644
index 00000000..04ebe629
Binary files /dev/null and b/其他/宣传彩页-智能伤员感知救助系统.jpg differ