成就更好的自己

经过本系列上一篇文章关于PX4_command飞行控制功能包的分析，相信大家对于飞整个流程有个大概的了解，本章将在此基础上详细讲解一下应用级算法构建的思路与操作方法。

关于PX4_command飞行控制功能包的分析见链接：

https://blog.csdn.net/qq_36098477/article/details/115496830

目录

px4_command核心文件修改

px4_vel_controller.cpp

px4_pos_estimator.cpp

上层控制程序控制思路

px4_command核心文件修改

让我们详细看一下px4_vel_controller.cpp和px4_pos_estimator.cpp

px4_vel_controller.cpp

//相应的命令分别为 移动(惯性系ENU)，移动(机体系)，悬停，降落，上锁，紧急降落，待机

enum Command

{

    Move_ENU,

    Move_Body,

    Hold,

    Takeoff,

    Land,

    Arm,

    Disarm,

    Failsafe_land,

    Idle

};

上面枚举了command命令所支持的所有命令，具体使用方法如下：

Arm：通过解锁命令使得自驾仪状态由Disarm变为Arm，然后自驾仪进入Idle待机状态，四个电机怠速旋转；

Command_now.command = Arm;

command_pub.publish(Command_now);

Disarm：通过锁定命令使得自驾仪状态变为Disarm，然后自驾仪进入锁定状态，四个电机锁定；

Command_now.command = Disarm;

command_pub.publish(Command_now);

Takeoff：初始化所有起飞数据，将起飞地面点设为世界坐标系零点，起飞至一定高度并悬停，起飞高度由参数服务器中的fly_height决定，在文件Parameter_for_control.yaml中修改；

Command_now.command = Takeoff;

command_pub.publish(Command_now);

Land：使无人机降落，降落原理是当距离地面小于某个距离时发布命令使自驾仪锁定，即进入Disarm状态，电机锁转，降落距离在文件command_to_mavros.h中修改，修改后需要重新catkin_make；

Command_now.command = Land;

command_pub.publish(Command_now);

Hold：在某一位置悬停，悬停位置即当时无人机所处位置；

Command_now.command = Hold;

Command_now.sub_mode = 0x00;

command_pub.publish(Command_now);

Idle：电机怠速旋转模式，可以用来作为临时降落；

注：因为ldle怠速降落与land降落的区别，怠速降落只是使无人机失去推力，不会改变自驾仪锁定状态等其他属性，世界坐标系与无人机相对坐标不变，再次起飞时不需要解锁与take_off指令，直接使用Move_ENU或Move_Body即可起飞，相当于继续保留降落前的状态；但是land指令执行后直接使自驾仪锁定Disarm，再次起飞必须要重新Arm，然后使用take_off指令，但是这样会使世界坐标系重新定位，各种参数也会重置；因此在需要多次降落的任务中，可以考虑ldle怠速降落的方法。

Command_now.command = Idle;

command_pub.publish(Command_now);

Move_ENU和Move_Body：是无人机移动指令，这两模式的区别是，前者是在世界坐标系下移动，所给出的航点是参照世界坐标系原点的；后者是在机体系下移动，每条指令所给出的航点始终以自己为坐标系原点。此外，两种模式均有子模式，分别是按照速度控制模式与位置控制模式，使用时按照所需参数赋值即可。

Command_now.command = Move_ENU;     //机体系下移动

Command_now.comid = comid;

comid++;

Command_now.sub_mode = 2; // xy 速度控制模式 z 位置控制模式

Command_now.vel_sp[0] =  vel_sp_ENU[0];

Command_now.vel_sp[1] =  vel_sp_ENU[1];  //ENU frame

Command_now.pos_sp[2] =  0.5;  //航点高度

Command_now.yaw_sp = 0 ;

px4_vel_controller.cpp文件的任务就是对每一条命令进行处理的程序，想了解与MAVROS关系的可以从这里下手。

px4_pos_estimator.cpp

这个文件的内容跟数据融合有关，然而大多数都不要修改，只有一个地方可以根据实际情况修改，就是主要传感器数据的来源设置；

void pose_pub_timer_cb(const ros::TimerEvent& TE)

{

    //source -from cartographer

    if (pose_source == 0)  

    {

        pos_drone_laser[2] = pos_drone_fcu[2];

        vision.pose.position.x = pos_drone_laser[0];

        vision.pose.position.y = pos_drone_laser[1];

        vision.pose.position.z = pos_drone_laser[2];



        vision.pose.orientation.x = q_laser.x();

        vision.pose.orientation.y = q_laser.y();

        vision.pose.orientation.z = q_laser.z();

        vision.pose.orientation.w = q_laser.w();

        vision.header.stamp = TE.current_real;

    }

    //source -from t265

    if (pose_source == 1)

    {

        //pos_drone_t265[2] = pos_drone_fcu[2];

        vision.pose.position.x = pos_drone_t265[0];

        vision.pose.position.y = pos_drone_t265[1];

        vision.pose.position.z = pos_drone_t265[2];



        vision.pose.orientation.x = q_t265.x();

        vision.pose.orientation.y = q_t265.y();

        vision.pose.orientation.z = q_t265.z();

        vision.pose.orientation.w = q_t265.w();

        vision.header.stamp = TE.current_real;

    }

    //source -from uwb

    if (pose_source == 2)

    {

        pos_drone_uwb[2] = pos_drone_fcu[2];

        vision.pose.position.x = pos_drone_uwb[0];

        vision.pose.position.y = pos_drone_uwb[1];

        vision.pose.position.z = pos_drone_uwb[2];



        vision.pose.orientation.x = q_uwb.x();

        vision.pose.orientation.y = q_uwb.y();

        vision.pose.orientation.z = q_uwb.z();

        vision.pose.orientation.w = q_uwb.w();

        vision.header.stamp = TE.current_real;

    }

    ready_for_pub = true;

}

pose_source决定的是无人机位姿数据来源，0是激光雷达，1是T265相机，2是无人机自驾仪数据。因为本项目采用的是T265获取位姿，因此主要数据来源就采用了T265；如果在后期开发使用了其他位姿传感器，则可以再次修改主要数据来源。pose_source是ROS参数服务器中的一个参数，可以在px4_pos_estimator.launch文件中进行修改。

上层控制程序控制思路

分析一下如何调用px4_command命令控制飞机运动；

例如，运行input_point.sh脚本启动输入航点控制无人机飞行程序，C++源文件是src中的input_point.cpp；

//【订阅】无人机当前位置 坐标系 NED系

//ros::Subscriber position_sub = nh.subscribe<geometry_msgs::Pose>("/drone/pos", 100, pos_cb);

ros::Subscriber position_sub = nh.subscribe<geometry_msgs::PoseStamped>("/mavros/local_position/pose", 100, pos_cb);  //dyx

// 【发布】发送给position_control.cpp的命令

ros::Publisher command_pub = nh.advertise<px4_command::command>("/px4/command", 10);

//【订阅】Lidar数据

ros::Subscriber lidar_sub = nh.subscribe<sensor_msgs::LaserScan>("/scan", 1000, lidar_cb);//dyx

订阅无人机当前位姿带入算法，数据是由px4_pos_estimator节点发布到/mavros/local_position/pose话题上的；订阅激光雷达信息，判断周围环境和障碍物信息；发布控制指令到/px4/command话题上，由px4_vel_controller节点订阅接收并处理。

//输入1,继续，其他，退出程序

    cout << "Please check the parameter and setting，1 for go on， else for quit: "<<endl;

    cin >> check_flag;

    if(check_flag != 1) return -1;



    //check arm

    int Arm_flag;

    cout<<"Whether choose to Arm? 1 for Arm, 0 for quit"<<endl;

    cin >> Arm_flag;

    if(Arm_flag == 1)

    {

        Command_now.command = Arm;

        command_pub.publish(Command_now);

    }

    else return -1;



    //check takeoff

    int Take_off_flag;

    cout<<"Whether choose to Takeoff? 1 for Takeoff, 0 for quit"<<endl;

    cin >> Take_off_flag;

    if(Take_off_flag == 1)

    {

        Command_now.command = Takeoff;

        command_pub.publish(Command_now);

    }

else return -1;

经过起飞前检查，解锁，起飞后，飞机已经进入悬停模式了，并悬停在take_off指定的高度。

cout << "按右手系坐标输入航点坐标:x  y  z,坐标以空格隔开,最大4m" << endl;

            scanf("%f %f %f",&target_x,&target_y,&target_z);

            if(abs(target_x) <= 4.0 &&abs(target_y) <= 4.0 && target_z > 0 && target_z <= 3)

            {

                while(arrive_flag == false)

                {

                    ros::spinOnce();

                    collision_avoidance(target_x,target_y);

                    horizontal_distance = sqrt((pos_drone.pose.position.x - target_x) * (pos_drone.pose.position.x - target_x) + (pos_drone.pose.position.y - target_y) * (pos_drone.pose.position.y - target_y));

                    vertical_distance = abs(pos_drone.pose.position.z - target_z);

                    cout<<"horizontal_distance:  "<<horizontal_distance<<endl;

                    Command_now.command = Move_ENU;     //机体系下移动

                    Command_now.comid = comid;

                    comid++;

                    Command_now.sub_mode = 2; // xy 速度控制模式 z 位置控制模式

                    Command_now.vel_sp[0] =  vel_sp_ENU[0];

                    Command_now.vel_sp[1] =  vel_sp_ENU[1];  //ENU frame

                    Command_now.pos_sp[2] = target_z;

                    Command_now.yaw_sp = 0 ;

                    command_pub.publish(Command_now);

                    printf();

                    rate.sleep();

                    if(horizontal_distance < 0.03 && vertical_distance < 0.03)

                    {

                        arrive_flag = true;

                        for (int i=0; i<5; i++)

                        {

                            Command_now.command = Hold;

                            Command_now.sub_mode = 0x00;

                            command_pub.publish(Command_now);

                            rate.sleep();

                        }

                    }

                }

输入航点，collision_avoidance(target_x,target_y)函数根据激光雷达算出障碍物位置与距离进而计算水平移动速度与避障方案，最后得出x和y轴向的正交分解速度，z轴坐标给出直接使用，然后通过Move_ENU命令将算出的速度传给MAVROS，进而传给自驾仪控制电机；当3轴向位置达到预定位置后，使无人机悬停到当前位置，准备接受下一个航点指令。

同样道理，如果想要开发高级算法，思路也是使用/mavros/local_position/pose的无人机位姿数据带入算法，输入或计算出目标航点，然后将目标航点通过px4_command指令发布在/px4/command话题上控制飞行即可。