【2022_10_17】PX4学习_px4盘旋逻辑-优快云博客

本文深入解析了PX4飞行控制系统中Commander模块的custom_command函数，该函数处理不同命令，如takeoff，并通过send_vehicle_command发送指令。navigator模块则根据任务输出导航轨迹。讨论了如何修改navigator策略以适应移动目标的跟踪。同时，分析了由QGC生成的飞行计划文件，包括航点信息、速度设置等。重点关注了FlightTaskAutoFollowTarget.cpp在自动跟随目标任务中的作用。

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

commander.cpp内
int Commander::custom_command(int argc, char *argv[])
221行
该函数接受所有commander输入的参数，strcmp比较后调用不同的函数
strcmp返回值是0表示相同字符串

比如

if (!strcmp(argv[0], "takeoff")) {
		// switch to takeoff mode and arm
		send_vehicle_command(vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF);
		send_vehicle_command(vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM,
				     static_cast<float>(vehicle_command_s::ARMING_ACTION_ARM),
				     0.f);
		return 0;
	}

这一段对应的是takeoff起飞的conmander命令
调用的所有函数都是send_vehicle_command

static bool send_vehicle_command(const uint32_t cmd, const float param1 = NAN, const float param2 = NAN,
				 const float param3 = NAN,  const float param4 = NAN, const double param5 = static_cast<double>(NAN),
				 const double param6 = static_cast<double>(NAN), const float param7 = NAN)
{
	vehicle_command_s vcmd{};
	vcmd.command = cmd;
	vcmd.param1 = param1;
	vcmd.param2 = param2;
	vcmd.param3 = param3;
	vcmd.param4 = param4;
	vcmd.param5 = param5;
	vcmd.param6 = param6;
	vcmd.param7 = param7;

	uORB::SubscriptionData<vehicle_status_s> vehicle_status_sub{ORB_ID(vehicle_status)};
	vcmd.source_system = vehicle_status_sub.get().system_id;
	vcmd.target_system = vehicle_status_sub.get().system_id;
	vcmd.source_component = vehicle_status_sub.get().component_id;
	vcmd.target_component = vehicle_status_sub.get().component_id;

	uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
	vcmd.timestamp = hrt_absolute_time();
	return vcmd_pub.publish(vcmd);
}

调用该函数之后，我们知道飞控的控制分为commander负责事件的处理，由navigator pos_control att_control完成层级的控制，因此在这里commander处理完takeoff这个事件的命令之后应该将一些信息发布给navigator去做

观察这个函数，定义了一个结构体vehicle_command_s vcmd
这是一个uorb msg的实例结构体
它的位置是PX4-Autopilot/build/px4_sitl_default/uORB/topics/vehicle_command.h
有一些参数

函数在对这个msg结构体进行了完整的赋值之后，打了一个时间戳，然后直接发布

发布后该消息会被订阅，之后由谁来处理？

分析导航模块navigator

https://blog.youkuaiyun.com/mbdong/article/details/123029120
根据这一篇文章，navigator - 导航模块，根据指定的任务输出导航轨迹
因此我们要实现跟踪，要做的是commander模块不断接收指令，navigator模块不断输出导航轨迹
同时navigator模块只负责输出轨迹，不考虑姿态的问题
要做的

读通顺commander模块，实现读取实时信号，自动输入commander命令
修改部分navigator策略，实现对于移动的且移动速度不快的目标不要傻傻地直奔target位置和盘旋

分析盘旋的逻辑
https://blog.youkuaiyun.com/shenxingxin/article/details/107703110?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522166601474416782414953227%2522%252C%2522scm%2522%253A%252220140713.130102334…%2522%257D&request_id=166601474416782414953227&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2_allsobaiduend~default-1-107703110-null-null.142^v58pc_rank_34_2,201^v3control_1&utm_term=px4loiter&spm=1018.2226.3001.4187

分析commander具体有哪些命令和含义
https://blog.youkuaiyun.com/borbz/article/details/78007453?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522166601463116782414991890%2522%252C%2522scm%2522%253A%252220140713.130102334…%2522%257D&request_id=166601463116782414991890&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2_allsobaiduend~default-2-78007453-null-null.142^v58pc_rank_34_2,201^v3control_1&utm_term=posctl&spm=1018.2226.3001.4187

分析由QGC生成的传入飞控的plan文件
重点是mission后面
cruiseSpeed 巡航速度
firmwareType固件类型
hoverspeed悬停速度

然后是一堆items，就是各个航点的基本信息
每一个航点分为两个{}，由两部分结构体提供一个航点的信息

params部分最后三个参数是
经纬度和高度

比如第一个航点,command命令是22，对应是takeoff

{
    "fileType": "Plan",
    "geoFence": {
        "circles": [
        ],
        "polygons": [
        ],
        "version": 2
    },
    "groundStation": "QGroundControl",
    "mission": {
        "cruiseSpeed": 15,
        "firmwareType": 12,
        "hoverSpeed": 5,
        "items": [
            {
                "AMSLAltAboveTerrain": 50,
                "Altitude": 50,
                "AltitudeMode": 0,
                "autoContinue": true,
                "command": 22,
                "doJumpId": 1,
                "frame": 3,
                "params": [
                    0,
                    0,
                    0,
                    null,
                    47.397757116360204,
                    8.54802567938998,
                    50
                ],
                "type": "SimpleItem"
            },
            {
                "AMSLAltAboveTerrain": 50,
                "Altitude": 50,
                "AltitudeMode": 0,
                "autoContinue": true,
                "command": 16,
                "doJumpId": 2,
                "frame": 3,
                "params": [
                    0,
                    0,
                    0,
                    null,
                    47.395834920202326,
                    8.548141558196932,
                    50
                ],
                "type": "SimpleItem"
            },
            {
                "autoContinue": true,
                "command": 189,
                "doJumpId": 3,
                "frame": 2,
                "params": [
                    0,
                    0,
                    0,
                    0,
                    0,
                    0,
                    0
                ],
                "type": "SimpleItem"
            },
            {
                "AMSLAltAboveTerrain": 25,
                "Altitude": 25,
                "AltitudeMode": 0,
                "autoContinue": true,
                "command": 31,
                "doJumpId": 4,
                "frame": 3,
                "params": [
                    1,
                    50,
                    0,
                    1,
                    47.39580995751514,
                    8.545613891100459,
                    25
                ],
                "type": "SimpleItem"
            },
            {
                "AMSLAltAboveTerrain": 0,
                "Altitude": 0,
                "AltitudeMode": 0,
                "autoContinue": true,
                "command": 21,
                "doJumpId": 5,
                "frame": 3,
                "params": [
                    25,
                    0,
                    0,
                    null,
                    47.39757125904106,
                    8.545589014690734,
                    0
                ],
                "type": "SimpleItem"
            }
        ],
        "plannedHomePosition": [
            47.397742,
            8.545594,
            489
        ],
        "vehicleType": 1,
        "version": 2
    },
    "rallyPoints": {//召集点
        "points": [
        ],
        "version": 2
    },
    "version": 1
}

PX4-Autopilot/src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp