diff --git a/README.md b/README.md
index 54464a30..c2054b10 100644
--- a/README.md
+++ b/README.md
@@ -1,2 +1,2 @@
-“无人战场伤员搬运”软件系统设计
+“智能伤员救助”软件系统设计
 
diff --git a/doc/“无人战场伤员搬运”软件系统的需求构思及描述.docx b/doc/“智能伤员感知救助”软件系统的需求构思及描述.docx
similarity index 100%
rename from doc/“无人战场伤员搬运”软件系统的需求构思及描述.docx
rename to doc/“智能伤员感知救助”软件系统的需求构思及描述.docx
diff --git a/doc/“无人战场伤员搬运”软件设计规格说明书.docx b/doc/“智能伤员感知救助”软件设计规格说明书.docx
similarity index 100%
rename from doc/“无人战场伤员搬运”软件设计规格说明书.docx
rename to doc/“智能伤员感知救助”软件设计规格说明书.docx
diff --git a/doc/“无人战场伤员搬运”软件需求规格说明书.docx b/doc/“智能伤员感知救助”软件需求规格说明书.docx
similarity index 100%
rename from doc/“无人战场伤员搬运”软件需求规格说明书.docx
rename to doc/“智能伤员感知救助”软件需求规格说明书.docx
diff --git a/model/“无人战场救援”软件需求模型.eddx b/model/“智能伤员感知救助”软件需求模型.eddx
similarity index 100%
rename from model/“无人战场救援”软件需求模型.eddx
rename to model/“智能伤员感知救助”软件需求模型.eddx
diff --git a/model/“无人战场救援”软件需求模型.vsdx b/model/“智能伤员感知救助”软件需求模型.vsdx
similarity index 100%
rename from model/“无人战场救援”软件需求模型.vsdx
rename to model/“智能伤员感知救助”软件需求模型.vsdx
diff --git a/src/无人机飞行控制代码/terminal_control.cpp b/src/无人机飞行控制代码/terminal_control.cpp
new file mode 100644
index 00000000..e88ed59b
--- /dev/null
+++ b/src/无人机飞行控制代码/terminal_control.cpp
@@ -0,0 +1,864 @@
+/***************************************************************************************************************************
+* terminal_control.cpp
+*
+* Author: Qyp
+*
+* Update Time: 2020.11.4
+*
+* Introduction:  test function for sending ControlCommand.msg
+***************************************************************************************************************************/
+#include <ros/ros.h>
+#include <iostream>
+
+#include <prometheus_msgs/ControlCommand.h>
+#include <mavros_msgs/CommandBool.h>
+#include <mavros_msgs/SetMode.h>
+#include <mavros_msgs/State.h>
+#include <nav_msgs/Path.h>
+
+#include "controller_test.h"
+#include "KeyboardEvent.h"
+
+
+//json数据库
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/socket.h>
+#include <arpa/inet.h>
+#include <unistd.h>
+#include <jsoncpp/json/json.h>
+
+#define HOST "192.168.43.58"
+#define PORT 1900
+
+#define VEL_XY_STEP_SIZE 0.1
+#define VEL_Z_STEP_SIZE 0.1
+#define YAW_STEP_SIZE 0.08
+#define TRA_WINDOW 2000
+#define NODE_NAME "terminal_control"
+
+using namespace std;
+
+//即将发布的command
+prometheus_msgs::ControlCommand Command_to_pub;
+//轨迹容器
+std::vector<geometry_msgs::PoseStamped> posehistory_vector_;
+
+
+float time_trajectory = 0.0;
+// 轨迹追踪总时长，键盘控制时固定时长，指令输入控制可调
+float trajectory_total_time = 50.0;
+
+
+//发布
+ros::Publisher move_pub;
+ros::Publisher ref_trajectory_pub;
+//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>　函数声明　<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
+void mainloop1();
+void mainloop2();
+void generate_com(int Move_mode, float state_desired[4]);
+void Draw_in_rviz(const prometheus_msgs::PositionReference& pos_ref, bool draw_trajectory);
+void timerCallback(const ros::TimerEvent& e)
+{
+    cout << ">>>>>>>>>>>>>>>> Welcome to use Prometheus Terminal Control <<<<<<<<<<<<<<<<"<< endl;
+    cout << "ENTER key to control the drone: " <<endl;
+    cout << "1 for Arm, Space for Takeoff, L for Land, H for Hold, 0 for Disarm, 8/9 for Trajectory tracking" <<endl;
+    cout << "Move mode is fixed (XYZ_VEL,BODY_FRAME): w/s for body_x, a/d for body_y, k/m for z, q/e for body_yaw" <<endl;
+    cout << "CTRL-C to quit." <<endl;
+}
+//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>　主函数　<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "terminal_control");
+    ros::NodeHandle nh;
+
+    //　【发布】　控制指令
+    move_pub = nh.advertise<prometheus_msgs::ControlCommand>("/prometheus/control_command", 10);
+
+    //　【发布】　参考轨迹
+    ref_trajectory_pub = nh.advertise<nav_msgs::Path>("/prometheus/reference_trajectory", 10);
+    
+    //用于控制器测试的类，功能例如：生成圆形轨迹，８字轨迹等
+    Controller_Test Controller_Test;    // 打印参数
+    Controller_Test.printf_param();
+
+    // 初始化命令 - Idle模式 电机怠速旋转 等待来自上层的控制指令
+    Command_to_pub.Mode                                = prometheus_msgs::ControlCommand::Idle;
+    Command_to_pub.Command_ID                          = 0;
+    Command_to_pub.source = NODE_NAME;
+    Command_to_pub.Reference_State.Move_mode           = prometheus_msgs::PositionReference::XYZ_POS;
+    Command_to_pub.Reference_State.Move_frame          = prometheus_msgs::PositionReference::ENU_FRAME;
+    Command_to_pub.Reference_State.position_ref[0]     = 0;
+    Command_to_pub.Reference_State.position_ref[1]     = 0;
+    Command_to_pub.Reference_State.position_ref[2]     = 0;
+    Command_to_pub.Reference_State.velocity_ref[0]     = 0;
+    Command_to_pub.Reference_State.velocity_ref[1]     = 0;
+    Command_to_pub.Reference_State.velocity_ref[2]     = 0;
+    Command_to_pub.Reference_State.acceleration_ref[0] = 0;
+    Command_to_pub.Reference_State.acceleration_ref[1] = 0;
+    Command_to_pub.Reference_State.acceleration_ref[2] = 0;
+    Command_to_pub.Reference_State.yaw_ref             = 0;
+
+    //固定的浮点显示
+    cout.setf(ios::fixed);
+    //setprecision(n) 设显示小数精度为n位
+    cout<<setprecision(2);
+    //左对齐
+    cout.setf(ios::left);
+    // 强制显示小数点
+    cout.setf(ios::showpoint);
+    // 强制显示符号
+    //cout.setf(ios::showpos);
+
+    // 选择通过终端输入控制或键盘控制
+    int Remote_Mode;
+    cout << ">>>>>>>>>>>>>>>> Welcome to use Prometheus Terminal Control <<<<<<<<<<<<<<<<"<< endl;
+    cout << "Please choose the Remote Mode: 0 for command input control, 1 for keyboard input control"<<endl;
+    cin >> Remote_Mode;
+
+    if (Remote_Mode == 0)
+    {
+        cout << "Command input control mode"<<endl;
+        mainloop1();
+    }else if(Remote_Mode == 1)
+    {
+        ros::Timer timer = nh.createTimer(ros::Duration(30.0), timerCallback);
+        cout << "Keyboard input control mode"<<endl;
+        mainloop2();
+    }
+
+    return 0;
+}
+
+void mainloop1()
+{
+    int Control_Mode = 0;
+    int Move_mode = 0;
+    int Move_frame = 0;
+    int Trjectory_mode = 0;
+    float state_desired[4];
+    Controller_Test Controller_Test;
+
+    while(ros::ok())
+    {
+        // Waiting for input
+        cout << ">>>>>>>>>>>>>>>> Welcome to use Prometheus Terminal Control <<<<<<<<<<<<<<<<"<< endl;
+        cout << "Please choose the Command.Mode: 0 for Idle, 1 for Takeoff, 2 for Hold, 3 for Land, 4 for Move, 5 for Disarm, 6 for User_Mode1, 7 for User_Mode2"<<endl;
+        cout << "Input 999 to switch to offboard mode and arm the drone (ONLY for simulation, please use RC in experiment!!!)"<<endl;
+        cin  >> Control_Mode;
+
+        if(Control_Mode == prometheus_msgs::ControlCommand::Move)
+        {
+            cout << "Please choose the Command.Reference_State.Move_mode: 0 for XYZ_POS, 1 for XY_POS_Z_VEL, 2 for XY_VEL_Z_POS, 3 for XYZ_VEL, 5 for TRAJECTORY"<<endl;
+            cin >> Move_mode;
+
+            if(Move_mode == prometheus_msgs::PositionReference::TRAJECTORY)
+            {
+                cout << "For safety, please move the drone near to the trajectory start point firstly!!!"<<endl;
+                cout << "Please choose the trajectory type: 0 for Circle, 1 for Eight Shape, 2 for Step, 3 for Line"<<endl;
+                cin >> Trjectory_mode;  
+                cout << "Input the trajectory_total_time:"<<endl;
+                cin >> trajectory_total_time;
+            }else
+            {
+                cout << "Please choose the Command.Reference_State.Move_frame: 0 for ENU_FRAME, 1 for BODY_FRAME"<<endl;
+                cin >> Move_frame; 
+                cout << "Please input the reference state [x y z yaw]: "<< endl;
+                cout << "setpoint_t[0] --- x [m] : "<< endl;
+                cin >> state_desired[0];
+                cout << "setpoint_t[1] --- y [m] : "<< endl;
+                cin >> state_desired[1];
+                cout << "setpoint_t[2] --- z [m] : "<< endl;
+                cin >> state_desired[2];
+                cout << "setpoint_t[3] --- yaw [du] : "<< endl;
+                cin >> state_desired[3];
+            }
+  
+        }
+        else if(Control_Mode == prometheus_msgs::ControlCommand::User_Mode1)
+        {
+            cout << "Please choose the Command.Reference_State.Move_mode: 0 for XYZ_POS, 1 for XY_POS_Z_VEL, 2 for XY_VEL_Z_POS, 3 for XYZ_VEL, 5 for TRAJECTORY"<<endl;
+            Move_mode = 5;
+
+            if(Move_mode == prometheus_msgs::PositionReference::TRAJECTORY)
+            {
+                cout << "For safety, please move the drone near to the trajectory start point firstly!!!"<<endl;
+                cout << "Please choose the trajectory type: 0 for Circle, 1 for Eight Shape, 2 for Step, 3 for Line"<<endl;
+                Trjectory_mode = 0;  
+                cout << "Input the trajectory_total_time:"<<endl;
+                trajectory_total_time = 100;
+                cout << "Please choose the Command.Reference_State.Move_frame: 0 for ENU_FRAME, 1 for BODY_FRAME"<<endl;
+                Move_frame = 1; 
+                cout << "Please input the reference state [x y z yaw]: "<< endl;
+                cout << "setpoint_t[0] --- x [m] : "<< endl;
+                cin >> state_desired[0];
+                cout << "setpoint_t[1] --- y [m] : "<< endl;
+                cin >> state_desired[1];
+                cout << "setpoint_t[2] --- z [m] : "<< endl;
+                cin >> state_desired[2];
+                cout << "setpoint_t[3] --- yaw [du] : "<< endl;
+                cin >> state_desired[3];
+            }
+        }
+        else if(Control_Mode == 999)
+        {
+            Command_to_pub.header.stamp = ros::Time::now();
+            Command_to_pub.Mode = prometheus_msgs::ControlCommand::Idle;
+            Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+            Command_to_pub.source = NODE_NAME;
+            Command_to_pub.Reference_State.yaw_ref = 999;
+            move_pub.publish(Command_to_pub);
+            Command_to_pub.Reference_State.yaw_ref = 0.0;
+        }
+
+        switch (Control_Mode)
+        {
+            case prometheus_msgs::ControlCommand::Idle:
+                Command_to_pub.header.stamp = ros::Time::now();
+                Command_to_pub.Mode = prometheus_msgs::ControlCommand::Idle;
+                Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+                Command_to_pub.source = NODE_NAME;
+                move_pub.publish(Command_to_pub);
+                break;
+
+            case prometheus_msgs::ControlCommand::Takeoff:
+                Command_to_pub.header.stamp = ros::Time::now();
+                Command_to_pub.Mode = prometheus_msgs::ControlCommand::Takeoff;
+                Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+                Command_to_pub.source = NODE_NAME;
+                move_pub.publish(Command_to_pub);
+                break;
+
+            case prometheus_msgs::ControlCommand::Hold:
+                Command_to_pub.header.stamp = ros::Time::now();
+                Command_to_pub.Mode = prometheus_msgs::ControlCommand::Hold;
+                Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+                Command_to_pub.source = NODE_NAME;
+                move_pub.publish(Command_to_pub);
+                break;
+    
+            case prometheus_msgs::ControlCommand::Land:
+                Command_to_pub.header.stamp = ros::Time::now();
+                Command_to_pub.Mode = prometheus_msgs::ControlCommand::Land;
+                Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+                Command_to_pub.source = NODE_NAME;
+                move_pub.publish(Command_to_pub);
+                break;
+
+            case prometheus_msgs::ControlCommand::Move:
+                if(Move_mode == prometheus_msgs::PositionReference::TRAJECTORY)
+                {
+                     time_trajectory = 0.0;
+
+                    while(time_trajectory < trajectory_total_time)
+                    {
+                        Command_to_pub.header.stamp = ros::Time::now();
+                        Command_to_pub.Mode = prometheus_msgs::ControlCommand::Move;
+                        Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+                        Command_to_pub.source = NODE_NAME;
+
+                        if(Trjectory_mode == 0)
+                        {
+                            Command_to_pub.Reference_State = Controller_Test.Circle_trajectory_generation(time_trajectory);
+                        }else if(Trjectory_mode == 1)
+                        {
+                            Command_to_pub.Reference_State = Controller_Test.Eight_trajectory_generation(time_trajectory);
+                        }else if(Trjectory_mode == 2)
+                        {
+                            Command_to_pub.Reference_State = Controller_Test.Step_trajectory_generation(time_trajectory);
+                        }else if(Trjectory_mode == 3)
+                        {
+                            Command_to_pub.Reference_State = Controller_Test.Line_trajectory_generation(time_trajectory);
+                        }
+
+                        move_pub.publish(Command_to_pub);
+                        time_trajectory = time_trajectory + 0.01;
+
+                        cout << "Trajectory tracking: "<< time_trajectory << " / " << trajectory_total_time  << " [ s ]" <<endl;
+
+                        Draw_in_rviz(Command_to_pub.Reference_State, true);
+
+                        ros::Duration(0.01).sleep();
+                    }
+                    
+                }else
+                {
+                    Command_to_pub.header.stamp = ros::Time::now();
+                    Command_to_pub.Mode = prometheus_msgs::ControlCommand::Move;
+                    Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+                    Command_to_pub.source = NODE_NAME;
+                    Command_to_pub.Reference_State.Move_mode  = Move_mode;
+                    Command_to_pub.Reference_State.Move_frame = Move_frame;
+                    Command_to_pub.Reference_State.time_from_start = -1;
+                    generate_com(Move_mode, state_desired);
+        
+                    move_pub.publish(Command_to_pub);
+                }
+                break;
+            
+            case prometheus_msgs::ControlCommand::Disarm:
+                Command_to_pub.header.stamp = ros::Time::now();
+                Command_to_pub.Mode = prometheus_msgs::ControlCommand::Disarm;
+                Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+                Command_to_pub.source = NODE_NAME;
+                move_pub.publish(Command_to_pub);
+                break;
+
+            case prometheus_msgs::ControlCommand::User_Mode1:
+                int sock;
+                struct sockaddr_in server;
+                char message[4096], server_reply[4096];
+
+                sock = socket(AF_INET, SOCK_STREAM, 0);
+                if(sock == -1){
+                    perror("Could not create socket");
+                    return 1;
+                }
+                puts("Socket created");
+
+                server.sin_addr.s_addr = inet_addr(HOST);
+                server.sin_family = AF_INET;
+                server.sin_port = htons(PORT);
+
+                if(connect(sock, (struct sockaddr *)&server, sizeof(server)) < 0){
+                    perror("connect failed. Error");
+                    return 1;
+                }
+                puts("Connected");
+
+                if(Move_mode == prometheus_msgs::PositionReference::TRAJECTORY)
+                {
+                    Command_to_pub.header.stamp = ros::Time::now();
+                    Command_to_pub.Mode = prometheus_msgs::ControlCommand::Move;
+                    Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+                    Command_to_pub.source = NODE_NAME;
+                    Command_to_pub.Reference_State.Move_mode  = Move_mode;
+                    Command_to_pub.Reference_State.Move_frame = Move_frame;
+                    Command_to_pub.Reference_State.time_from_start = -1;
+                    generate_com(Move_mode, state_desired);
+        
+                    move_pub.publish(Command_to_pub);
+                    sleep(20);
+                    time_trajectory = 0.0;
+                   
+
+                    while(time_trajectory < trajectory_total_time)
+                    {
+                        try{
+                            if(recv(sock, server_reply, 4096, 0) < 0){
+                                perror("recv failed");
+                                break;
+                            }
+                            std::string recvmsg(server_reply);
+                            Json::Value root;
+                            Json::Reader reader;
+                            bool parsingSuccessful = reader.parse(recvmsg, root);
+                            if(!parsingSuccessful){
+                                std::cout << "JSON parsing Error" << std::endl;
+                                break;
+                            }
+                            std::cout << recvmsg << std::endl;
+                            Json::Value target = root["target"];
+                            if (target.size()){
+                                if (!target.isNull() && target.isArray()) {
+                                    for (int i = 0; i < target.size(); i++) {
+                                        std::string target1 = target[i].asString();
+                                        std::cout << target1 << std::endl;
+                                    }
+                                }
+                                std::string message = std::to_string(state_desired[0]) + " " + std::to_string(state_desired[1]);
+                                if (send(sock, message.c_str(), message.length(), 0) == -1) {
+                                    std::cout << "Failed to send message" << std::endl;
+                                    return 1;
+                                }
+                                break;
+                            }
+                            
+                        }catch(Json::RuntimeError e){
+                            std::cout << "JSON runtime exception: " << e.what() << std::endl;
+                            break;
+                        } 
+
+
+                        Command_to_pub.header.stamp = ros::Time::now();
+                        Command_to_pub.Mode = prometheus_msgs::ControlCommand::Move;
+                        Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+                        Command_to_pub.source = NODE_NAME;
+
+
+                        //Command_to_pub.Reference_State = Controller_Test.Circle_trajectory_generation(time_trajectory);
+                        Command_to_pub.Reference_State.Move_mode  = Move_mode;
+                        Command_to_pub.Reference_State.Move_frame = Move_frame;
+                        Command_to_pub.Reference_State.time_from_start = -1;
+                        
+                        if (time_trajectory <= 25){
+                            state_desired[0] += time_trajectory/5;
+                        }
+                        else if (time_trajectory > 25 && time_trajectory <= 50){
+                            state_desired[1] += (time_trajectory-25)/5;
+                        }
+                        else if (time_trajectory > 50 && time_trajectory <= 75){
+                            state_desired[0] -= (time_trajectory-50)/5;
+                        }
+                        else if (time_trajectory > 75 && time_trajectory <= 100){
+                            state_desired[1] -= (time_trajectory-75)/5;
+                        }
+                        cout<<'x pos:   '<<state_desired[0]<<'                     '<<'y pos:     '<<state_desired[1]<<endl;
+                        generate_com(Move_mode, state_desired);
+            
+                        move_pub.publish(Command_to_pub);
+
+
+                        //move_pub.publish(Command_to_pub);
+                        time_trajectory = time_trajectory + 0.01;
+
+                        //cout << "Trajectory tracking: "<< time_trajectory << " / " << trajectory_total_time  << " [ s ]" <<endl;
+
+                        //Draw_in_rviz(Command_to_pub.Reference_State, true);
+
+                        ros::Duration(0.01).sleep();
+                    }
+                    
+
+
+                }
+                break;
+            
+            case prometheus_msgs::ControlCommand::User_Mode2:
+                Command_to_pub.header.stamp = ros::Time::now();
+                Command_to_pub.Mode = prometheus_msgs::ControlCommand::User_Mode2;
+                Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+                Command_to_pub.source = NODE_NAME;
+                move_pub.publish(Command_to_pub);
+                break;
+        }
+        
+        cout << "....................................................." <<endl;
+        
+        sleep(1.0);
+    }
+}
+
+void mainloop2()
+{
+    KeyboardEvent keyboardcontrol;
+    Controller_Test Controller_Test;
+
+    char key_now;
+    char key_last;
+
+    cout << ">>>>>>>>>>>>>>>> Welcome to use Prometheus Terminal Control <<<<<<<<<<<<<<<<"<< endl;
+    cout << "ENTER key to control the drone: " <<endl;
+    cout << "1 for Arm, Space for Takeoff, L for Land, H for Hold, 0 for Disarm, 8/9 for Trajectory tracking" <<endl;
+    cout << "Move mode is fixed (XYZ_VEL,BODY_FRAME): w/s for body_x, a/d for body_y, k/m for z, q/e for body_yaw" <<endl;
+    cout << "CTRL-C to quit." <<endl;
+
+    while (ros::ok())
+    {
+        keyboardcontrol.RosWhileLoopRun();
+        key_now = keyboardcontrol.GetPressedKey();
+        switch (key_now)
+        {
+
+        //悬停, 应当只发送一次, 不需要循环发送
+        case U_KEY_NONE:
+
+          if (key_last != U_KEY_NONE)
+          {
+              //to be continued.
+          }
+          sleep(0.5);
+
+          break;
+
+        // 数字1（非小键盘数字）：解锁及切换到OFFBOARD模式
+        case U_KEY_1:
+          cout << " " <<endl;
+          cout << "Arm and Switch to OFFBOARD." <<endl;
+      
+          Command_to_pub.header.stamp = ros::Time::now();
+          Command_to_pub.Mode = prometheus_msgs::ControlCommand::Idle;
+          Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+          Command_to_pub.source = NODE_NAME;
+          Command_to_pub.Reference_State.yaw_ref = 999;
+          move_pub.publish(Command_to_pub);
+          sleep(1.0);
+          break;
+
+        // 空格：起飞
+        case U_KEY_SPACE:
+          cout << " " <<endl;
+          cout << "Switch to Takeoff Mode." <<endl;
+
+          Command_to_pub.header.stamp = ros::Time::now();
+          Command_to_pub.Mode = prometheus_msgs::ControlCommand::Takeoff;
+          Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+          Command_to_pub.Reference_State.yaw_ref = 0.0;
+          Command_to_pub.source = NODE_NAME;
+          move_pub.publish(Command_to_pub);
+
+          sleep(1.0);
+
+          break;
+
+        // 键盘L：降落
+        case U_KEY_L:
+          cout << " " <<endl;
+          cout << "Switch to Land Mode." <<endl;
+      
+          Command_to_pub.header.stamp = ros::Time::now();
+          Command_to_pub.Mode = prometheus_msgs::ControlCommand::Land;
+          Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+          Command_to_pub.source = NODE_NAME;
+          move_pub.publish(Command_to_pub);
+
+          break;
+
+        // 键盘0（非小键盘数字）：紧急停止
+        case U_KEY_0:
+          cout << " " <<endl;
+          cout << "Switch to Disarm Mode." <<endl;
+      
+          Command_to_pub.header.stamp = ros::Time::now();
+          Command_to_pub.Mode = prometheus_msgs::ControlCommand::Disarm;
+          Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+          Command_to_pub.source = NODE_NAME;
+          move_pub.publish(Command_to_pub);
+
+          break;
+
+        //起飞要维持起飞的模式?
+        case U_KEY_T:
+          cout << " " <<endl;
+          cout << "Switch to Takeoff Mode." <<endl;
+
+          Command_to_pub.header.stamp = ros::Time::now();
+          Command_to_pub.Mode = prometheus_msgs::ControlCommand::Takeoff;
+          Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+          Command_to_pub.source = NODE_NAME;
+          move_pub.publish(Command_to_pub);
+
+          sleep(2.0);
+        
+          break;
+
+        //起飞要维持起飞的模式?
+        case U_KEY_H:
+          cout << " " <<endl;
+          cout << "Switch to Hold Mode." <<endl;
+
+          Command_to_pub.header.stamp = ros::Time::now();
+          Command_to_pub.Mode = prometheus_msgs::ControlCommand::Hold;
+          Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+          Command_to_pub.source = NODE_NAME;
+          Command_to_pub.Reference_State.position_ref[0]     = 0;
+          Command_to_pub.Reference_State.position_ref[1]     = 0;
+          Command_to_pub.Reference_State.position_ref[2]     = 0;
+          Command_to_pub.Reference_State.velocity_ref[0]     = 0;
+          Command_to_pub.Reference_State.velocity_ref[1]     = 0;
+          Command_to_pub.Reference_State.velocity_ref[2]     = 0;
+          Command_to_pub.Reference_State.acceleration_ref[0] = 0;
+          Command_to_pub.Reference_State.acceleration_ref[1] = 0;
+          Command_to_pub.Reference_State.acceleration_ref[2] = 0;
+          move_pub.publish(Command_to_pub);
+
+          sleep(1.0);
+        
+          break;
+
+        // 向前匀速运动
+        case U_KEY_W:
+
+          Command_to_pub.header.stamp = ros::Time::now();
+          Command_to_pub.Mode = prometheus_msgs::ControlCommand::Move;
+          Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+          Command_to_pub.source = NODE_NAME;
+          Command_to_pub.Reference_State.Move_mode       = prometheus_msgs::PositionReference::XYZ_VEL;
+          Command_to_pub.Reference_State.Move_frame      = prometheus_msgs::PositionReference::BODY_FRAME;
+          Command_to_pub.Reference_State.velocity_ref[0]     += VEL_XY_STEP_SIZE;
+          move_pub.publish(Command_to_pub);
+          
+          cout << " " <<endl;
+          cout << "Current Velocity [X Y Z]: " << Command_to_pub.Reference_State.velocity_ref[0] << " [m/s] " << Command_to_pub.Reference_State.velocity_ref[1] << " [m/s] " << Command_to_pub.Reference_State.velocity_ref[2] << " [m/s] "<<endl;
+
+          sleep(0.1);
+        
+          break;
+        
+        // 向后匀速运动
+        case U_KEY_S:
+        
+
+          Command_to_pub.header.stamp = ros::Time::now();
+          Command_to_pub.Mode = prometheus_msgs::ControlCommand::Move;
+          Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+          Command_to_pub.source = NODE_NAME;
+          Command_to_pub.Reference_State.Move_mode       = prometheus_msgs::PositionReference::XYZ_VEL;
+          Command_to_pub.Reference_State.Move_frame      = prometheus_msgs::PositionReference::BODY_FRAME;
+          Command_to_pub.Reference_State.velocity_ref[0]     -= VEL_XY_STEP_SIZE;
+          move_pub.publish(Command_to_pub);
+          cout << " " <<endl;
+          cout << "Current Velocity [X Y Z]: " << Command_to_pub.Reference_State.velocity_ref[0] << " [m/s] " << Command_to_pub.Reference_State.velocity_ref[1] << " [m/s] " << Command_to_pub.Reference_State.velocity_ref[2] << " [m/s] "<<endl;
+
+          sleep(0.1);
+
+          break;
+
+        // 向左匀速运动
+        case U_KEY_A:
+
+          Command_to_pub.header.stamp = ros::Time::now();
+          Command_to_pub.Mode = prometheus_msgs::ControlCommand::Move;
+          Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+          Command_to_pub.source = NODE_NAME;
+          Command_to_pub.Reference_State.Move_mode       = prometheus_msgs::PositionReference::XYZ_VEL;
+          Command_to_pub.Reference_State.Move_frame      = prometheus_msgs::PositionReference::BODY_FRAME;
+          Command_to_pub.Reference_State.velocity_ref[1]     += VEL_XY_STEP_SIZE;
+          move_pub.publish(Command_to_pub);
+        
+          cout << " " <<endl;
+          cout << "Current Velocity [X Y Z]: " << Command_to_pub.Reference_State.velocity_ref[0] << " [m/s] " << Command_to_pub.Reference_State.velocity_ref[1] << " [m/s] " << Command_to_pub.Reference_State.velocity_ref[2] << " [m/s] "<<endl;
+
+          sleep(0.1);
+
+          break;
+
+        // 向右匀速运动
+        case U_KEY_D:
+        
+          Command_to_pub.header.stamp = ros::Time::now();
+          Command_to_pub.Mode = prometheus_msgs::ControlCommand::Move;
+          Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+          Command_to_pub.source = NODE_NAME;
+          Command_to_pub.Reference_State.Move_mode       = prometheus_msgs::PositionReference::XYZ_VEL;
+          Command_to_pub.Reference_State.Move_frame      = prometheus_msgs::PositionReference::BODY_FRAME;
+          Command_to_pub.Reference_State.velocity_ref[1]     -= VEL_XY_STEP_SIZE;
+          move_pub.publish(Command_to_pub);
+
+          cout << " " <<endl;
+          cout << "Current Velocity [X Y Z]: " << Command_to_pub.Reference_State.velocity_ref[0] << " [m/s] " << Command_to_pub.Reference_State.velocity_ref[1] << " [m/s] " << Command_to_pub.Reference_State.velocity_ref[2] << " [m/s] "<<endl;
+
+          sleep(0.1);
+
+          break;
+
+        // 向上运动
+        case U_KEY_K:
+
+
+          Command_to_pub.header.stamp = ros::Time::now();
+          Command_to_pub.Mode = prometheus_msgs::ControlCommand::Move;
+          Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+          Command_to_pub.source = NODE_NAME;
+          Command_to_pub.Reference_State.Move_mode       = prometheus_msgs::PositionReference::XYZ_VEL;
+          Command_to_pub.Reference_State.Move_frame      = prometheus_msgs::PositionReference::BODY_FRAME;
+          Command_to_pub.Reference_State.velocity_ref[2]     += VEL_Z_STEP_SIZE;
+          move_pub.publish(Command_to_pub);
+
+          cout << " " <<endl;
+          cout << "Current Velocity [X Y Z]: " << Command_to_pub.Reference_State.velocity_ref[0] << " [m/s] " << Command_to_pub.Reference_State.velocity_ref[1] << " [m/s] " << Command_to_pub.Reference_State.velocity_ref[2] << " [m/s] "<<endl;
+
+          sleep(0.1);
+
+          break;
+
+        // 向下运动
+        case U_KEY_M:
+
+
+          Command_to_pub.header.stamp = ros::Time::now();
+          Command_to_pub.Mode = prometheus_msgs::ControlCommand::Move;
+          Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+          Command_to_pub.source = NODE_NAME;
+          Command_to_pub.Reference_State.Move_mode       = prometheus_msgs::PositionReference::XYZ_VEL;
+          Command_to_pub.Reference_State.Move_frame      = prometheus_msgs::PositionReference::BODY_FRAME;
+          Command_to_pub.Reference_State.velocity_ref[2]     -= VEL_Z_STEP_SIZE;
+          move_pub.publish(Command_to_pub);
+
+          cout << " " <<endl;
+          cout << "Current Velocity [X Y Z]: " << Command_to_pub.Reference_State.velocity_ref[0] << " [m/s] " << Command_to_pub.Reference_State.velocity_ref[1] << " [m/s] " << Command_to_pub.Reference_State.velocity_ref[2] << " [m/s] "<<endl;
+
+          sleep(0.1);
+        
+          break;
+
+        // 偏航运动，左转 （这个里偏航控制的是位置 不是速度）
+        case U_KEY_Q:
+
+
+          Command_to_pub.header.stamp = ros::Time::now();
+          Command_to_pub.Mode = prometheus_msgs::ControlCommand::Move;
+          Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+          Command_to_pub.source = NODE_NAME;
+          Command_to_pub.Reference_State.Move_mode       = prometheus_msgs::PositionReference::XYZ_VEL;
+          Command_to_pub.Reference_State.Move_frame      = prometheus_msgs::PositionReference::BODY_FRAME;
+          Command_to_pub.Reference_State.yaw_ref             = YAW_STEP_SIZE;
+          move_pub.publish(Command_to_pub);
+          
+          cout << " " <<endl;
+          cout << "Increase the Yaw angle." <<endl;
+
+          sleep(0.1);
+        
+          break;
+
+        // 偏航运动，右转
+        case U_KEY_E:
+
+
+          Command_to_pub.header.stamp = ros::Time::now();
+          Command_to_pub.Mode = prometheus_msgs::ControlCommand::Move;
+          Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+          Command_to_pub.source = NODE_NAME;
+          Command_to_pub.Reference_State.Move_mode       = prometheus_msgs::PositionReference::XYZ_POS;
+          Command_to_pub.Reference_State.Move_frame      = prometheus_msgs::PositionReference::BODY_FRAME;
+          Command_to_pub.Reference_State.yaw_ref             = YAW_STEP_SIZE;
+          move_pub.publish(Command_to_pub);
+          
+          cout << " " <<endl;
+          cout << "Decrease the Yaw angle." <<endl;
+
+          sleep(0.1);
+        
+          break;
+        
+        // 圆形追踪
+        case U_KEY_9:
+          time_trajectory = 0.0;
+          trajectory_total_time = 50.0;
+          // 需要设置
+          while(time_trajectory < trajectory_total_time)
+          {
+              Command_to_pub.header.stamp = ros::Time::now();
+              Command_to_pub.Mode = prometheus_msgs::ControlCommand::Move;
+              Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+              Command_to_pub.source = NODE_NAME;
+
+              Command_to_pub.Reference_State = Controller_Test.Circle_trajectory_generation(time_trajectory);
+
+              move_pub.publish(Command_to_pub);
+              time_trajectory = time_trajectory + 0.01;
+
+              cout << "Trajectory tracking: "<< time_trajectory << " / " << trajectory_total_time  << " [ s ]" <<endl;
+
+              Draw_in_rviz(Command_to_pub.Reference_State, true);
+
+              ros::Duration(0.01).sleep();
+          }
+          break;
+
+        // 8字追踪
+        case U_KEY_8:
+          time_trajectory = 0.0;
+          trajectory_total_time = 50.0;
+          // 需要设置
+          while(time_trajectory < trajectory_total_time)
+          {
+              Command_to_pub.header.stamp = ros::Time::now();
+              Command_to_pub.Mode = prometheus_msgs::ControlCommand::Move;
+              Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
+              Command_to_pub.source = NODE_NAME;
+
+              Command_to_pub.Reference_State = Controller_Test.Eight_trajectory_generation(time_trajectory);
+
+              move_pub.publish(Command_to_pub);
+              time_trajectory = time_trajectory + 0.01;
+
+              cout << "Trajectory tracking: "<< time_trajectory << " / " << trajectory_total_time  << " [ s ]" <<endl;
+
+              Draw_in_rviz(Command_to_pub.Reference_State, true);
+
+              ros::Duration(0.01).sleep();
+          }
+          break;
+        }
+
+        key_last = key_now;
+        ros::spinOnce();
+        sleep(0.1);
+    }
+}
+
+
+void generate_com(int Move_mode, float state_desired[4])
+{
+    //# Move_mode 2-bit value:
+    //# 0 for position, 1 for vel, 1st for xy, 2nd for z.
+    //#                   xy position     xy velocity
+    //# z position       	0b00(0)       0b10(2)
+    //# z velocity		0b01(1)       0b11(3)
+
+    if(Move_mode == prometheus_msgs::PositionReference::XYZ_ACC)
+    {
+        cout << "ACC control not support yet." <<endl;
+    }
+
+    if((Move_mode & 0b10) == 0) //xy channel
+    {
+        Command_to_pub.Reference_State.position_ref[0] = state_desired[0];
+        Command_to_pub.Reference_State.position_ref[1] = state_desired[1];
+        Command_to_pub.Reference_State.velocity_ref[0] = 0;
+        Command_to_pub.Reference_State.velocity_ref[1] = 0;
+    }
+    else
+    {
+        Command_to_pub.Reference_State.position_ref[0] = 0;
+        Command_to_pub.Reference_State.position_ref[1] = 0;
+        Command_to_pub.Reference_State.velocity_ref[0] = state_desired[0];
+        Command_to_pub.Reference_State.velocity_ref[1] = state_desired[1];
+    }
+
+    if((Move_mode & 0b01) == 0) //z channel
+    {
+        Command_to_pub.Reference_State.position_ref[2] = state_desired[2];
+        Command_to_pub.Reference_State.velocity_ref[2] = 0;
+    }
+    else
+    {
+        Command_to_pub.Reference_State.position_ref[2] = 0;
+        Command_to_pub.Reference_State.velocity_ref[2] = state_desired[2];
+    }
+
+    Command_to_pub.Reference_State.acceleration_ref[0] = 0;
+    Command_to_pub.Reference_State.acceleration_ref[1] = 0;
+    Command_to_pub.Reference_State.acceleration_ref[2] = 0;
+
+
+    Command_to_pub.Reference_State.yaw_ref = state_desired[3]/180.0*M_PI;
+}
+
+void Draw_in_rviz(const prometheus_msgs::PositionReference& pos_ref, bool draw_trajectory)
+{
+    geometry_msgs::PoseStamped reference_pose;
+
+    reference_pose.header.stamp = ros::Time::now();
+    reference_pose.header.frame_id = "world";
+
+    reference_pose.pose.position.x = pos_ref.position_ref[0];
+    reference_pose.pose.position.y = pos_ref.position_ref[1];
+    reference_pose.pose.position.z = pos_ref.position_ref[2];
+
+    if(draw_trajectory)
+    {
+        posehistory_vector_.insert(posehistory_vector_.begin(), reference_pose);
+        if(posehistory_vector_.size() > TRA_WINDOW){
+            posehistory_vector_.pop_back();
+        }
+        
+        nav_msgs::Path reference_trajectory;
+        reference_trajectory.header.stamp = ros::Time::now();
+        reference_trajectory.header.frame_id = "world";
+        reference_trajectory.poses = posehistory_vector_;
+        ref_trajectory_pub.publish(reference_trajectory);
+    }else
+    {
+        posehistory_vector_.clear();
+        
+        nav_msgs::Path reference_trajectory;
+        reference_trajectory.header.stamp = ros::Time::now();
+        reference_trajectory.header.frame_id = "world";
+        reference_trajectory.poses = posehistory_vector_;
+        ref_trajectory_pub.publish(reference_trajectory);
+    }
+}
\ No newline at end of file