volans的控制px4画圆思路

volans是一个开源的ros+px4仿真工程，在编写ros控制px4时，可以提供许多的参考思路与代码。其中画圆的部分可以用于参考在offboard模式下控制飞机。

launch启动文件

launch用于加载各个模型以及脚本文件，画圆的主 launch 文件在 src/simulation/launch/Demo/px4_quadrotor/ 目录下的 circular_px4.launch

可以看一下其中的内容：

包含四个部分：运行世界模型，飞机模型，画圆启动程序，键盘控制脚本。

其他的都不必说，只是画圆启动程序需要再看一下，顺着提示，可以找到其路径：

在 src/module/px4_control/launch 中的 circular.launch

查看其内容：

可以看到在这里加载了 ros 的节点包，并且配置了参数，之后就会调用 ros 相关的节点程序。节点程序一般也是在同一个 pkg 中，也就是在 px4_control 同目录下的 src 文件夹中的 circular.cpp

节点程序

ros节点程序的主要步骤为：

1）定义一个ros节点，频率

2）订阅相关消息并指定相关的回调函数

3）发布相关消息

4）设置服务消息

5）主循环任务

订阅消息

/mavros/local_position/pose 消息用于反应无人机的位置信息

/mavros/state 消息用于反应无人机的模式状态信息

发布信息

/mavros/setpoint_raw/local 消息用于发布无人机的目标位置信息

服务消息

设置模式的服务消息，之后mavros就会将此模式消息发送给px4

// 【服务】修改系统模式
set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("/mavros/set_mode");
nh.param<float>("desire_z", desire_z, 1.0); //期望高度
nh.param<float>("desire_Radius", desire_Radius, 1.0); //期望圆半径

可以利用 rostopic 命令来查看相关的主题消息：

rostopic echo /mavros/local_position/pose

主流程

主流程中，设置了一个状态机，先等待 offborad 模式，切换到 offboard 模式后，切换其他状态，一步一步发布圆形的轨迹

//状态机更新
void FlyState_update(void)
{
    switch(FlyState)
    {
        case WAITING:
            if(current_state.mode != "OFFBOARD")//等待offboard模式
            {
                pos_target[0] = pos_drone[0];
                pos_target[1] = pos_drone[1];
                pos_target[2] = pos_drone[2];
                temp_pos_drone[0] = pos_drone[0];
                temp_pos_drone[1] = pos_drone[1];
                temp_pos_drone[2] = pos_drone[2];
                send_pos_setpoint(pos_target, 0);
            }
            else
            {
                pos_target[0] = temp_pos_drone[0];
                pos_target[1] = temp_pos_drone[1];
                pos_target[2] = temp_pos_drone[2];
                send_pos_setpoint(pos_target, 0);
                FlyState = CHECKING;
            }
            //cout << "WAITING" <<endl;
            break;
        case CHECKING:
            if(pos_drone[0] == 0 && pos_drone[1] == 0)          //没有位置信息则执行降落模式
            {
                cout << "Check error, make sure have local location" <<endl;
                mode_cmd.request.custom_mode = "AUTO.LAND";
                set_mode_client.call(mode_cmd);
                FlyState = WAITING; 
            }
            else
            {
                FlyState = PREPARE;
                MoveTimeCnt = 0;
            }
            //cout << "CHECKING" <<endl;
            break;
        case PREPARE:                                           //起飞到圆轨迹的第一个点,起点在X负半轴
            temp_pos_target[0] = temp_pos_drone[0] - desire_Radius;
            temp_pos_target[1] = temp_pos_drone[1];
            temp_pos_target[2] = desire_z;
            MoveTimeCnt +=2;
            if(MoveTimeCnt >=500)
            {
                FlyState = REST;
                MoveTimeCnt = 0;
            }
            pos_target[0]=temp_pos_drone[0]+(temp_pos_target[0]-temp_pos_drone[0])*(MoveTimeCnt/500);
            pos_target[1]=temp_pos_drone[1]+(temp_pos_target[1]-temp_pos_drone[1])*(MoveTimeCnt/500);
            pos_target[2]=temp_pos_drone[2]+(temp_pos_target[2]-temp_pos_drone[2])*(MoveTimeCnt/500);
            send_pos_setpoint(pos_target, 0);                   
            if(current_state.mode != "OFFBOARD")                //如果在准备中途中切换到onboard，则跳到WAITING
            {
                FlyState = WAITING;
            }
            //cout << "PREPARE" <<endl;
            break;
        case REST:  
            pos_target[0] = temp_pos_drone[0] - desire_Radius;
            pos_target[1] = temp_pos_drone[1] ;
            pos_target[2] = desire_z;
            send_pos_setpoint(pos_target, 0);
            MoveTimeCnt +=1;
            if(MoveTimeCnt >= 100)
            {
                MoveTimeCnt = 0;
                FlyState = FLY;
            }
            if(current_state.mode != "OFFBOARD")                //如果在REST途中切换到onboard，则跳到WAITING
            {
                FlyState = WAITING;
            }
            break;
        case FLY:
            {
                float phase = 3.1415926;                        //起点在X负半轴
                float Omega = 2.0*3.14159*MoveTimeCnt / priod;  //0~2pi
                MoveTimeCnt += 3;                               //调此数值可改变飞圆形的速度
                if(MoveTimeCnt >=priod)                         //走一个圆形周期
                {
                    FlyState = FLYOVER;
                }
                pos_target[0] = temp_pos_drone[0]+desire_Radius*cos(Omega+phase);            
                pos_target[1] = temp_pos_drone[1]+desire_Radius*sin(Omega+phase); 
                pos_target[2] = desire_z;
                send_pos_setpoint(pos_target, 0);
                if(current_state.mode != "OFFBOARD")            //如果在飞圆形中途中切换到onboard，则跳到WAITING
                {
                    FlyState = WAITING;
                }
            }
            //cout << "FLY" <<endl;
            break;
        case FLYOVER:
            {
                mode_cmd.request.custom_mode = "AUTO.LAND";
                set_mode_client.call(mode_cmd);
                FlyState = WAITING;
            }
            //cout << "FLYOVER" <<endl;
            break;
        default:
            cout << "error" <<endl;
    }   
}