PX4模块设计之三十八：Navigator模块

Posted 2022-09-16 lida2003

PX4模块设计之三十八：Navigator模块

• 1. Navigator模块简介
• 2. 模块入口函数
• • 2.1 主入口navigator_main
  • 2.2 自定义子命令custom_command
• 3. Navigator模块重要函数
• • 3.1 task_spawn
  • 3.2 instantiate
  • 3.3 init
  • 3.4 Run
• 4. 总结
• 5. 参考资料

1. Navigator模块简介

### Description
Module that is responsible for autonomous flight modes. This includes missions (read from dataman),
takeoff and RTL.
It is also responsible for geofence violation checking.

### Implementation
The different internal modes are implemented as separate classes that inherit from a common base class `NavigatorMode`.
The member `_navigation_mode` contains the current active mode.

Navigator publishes position setpoint triplets (`position_setpoint_triplet_s`), which are then used by the position
controller.

navigator <command> [arguments...]
 Commands:
   start

   fencefile     load a geofence file from SD card, stored at etc/geofence.txt

   fake_traffic  publishes 4 fake transponder_report_s uORB messages

   stop

   status        print status info

注：print_usage函数是具体对应实现。

class Navigator : public ModuleBase<Navigator>, public ModuleParams

注：Navigator模块采用了ModuleBase, 但没有使用WorkQueue设计，因此在实现上需要实现instantiate方法。

2. 模块入口函数

2.1 主入口navigator_main

同样继承了ModuleBase，由ModuleBase的main来完成模块入口。

navigator_main
 └──> return Navigator::main(argc, argv)

2.2 自定义子命令custom_command

模块除支持start/stop/status命令，自定义命令支持以下子命令：

• fencefile: load a geofence file from SD card, stored at etc/geofence.txt
• fake_traffic: publishes 4 fake transponder_report_s uORB messages

Navigator::custom_command
 ├──> <!is_running()>
 │   ├──> print_usage("not running")
 │   └──> return 1
 ├──> <!strcmp(argv[0], "fencefile")>
 │   ├──> get_instance()->load_fence_from_file(GEOFENCE_FILENAME)
 │   └──> return 0
 ├──> <!strcmp(argv[0], "fake_traffic")>
 │   ├──> get_instance()->fake_traffic("LX007", 500, 1.0f, -1.0f, 100.0f, 90.0f, 0.001f, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT)
 │   ├──> get_instance()->fake_traffic("LX55", 1000, 0, 0, 100.0f, 90.0f, 0.001f, transponder_report_s::ADSB_EMITTER_TYPE_SMALL)
 │   ├──> get_instance()->fake_traffic("LX20", 15000, 1.0f, -1.0f, 280.0f, 90.0f, 0.001f, transponder_report_s::ADSB_EMITTER_TYPE_LARGE)
 │   ├──> get_instance()->fake_traffic("UAV", 10, 1.0f, -2.0f, 10.0f, 10.0f, 0.01f, transponder_report_s::ADSB_EMITTER_TYPE_UAV)
 │   └──> return 0
 └──> return print_usage("unknown command")

3. Navigator模块重要函数

3.1 task_spawn

这里直接使用px4_task_spawn_cmd创建任务。

Navigator::task_spawn
 ├──> _task_id = px4_task_spawn_cmd("navigator",SCHED_DEFAULT,SCHED_PRIORITY_NAVIGATION,PX4_STACK_ADJUSTED(1952),(px4_main_t)&run_trampoline,(char *const *)argv)
 ├──> <_task_id < 0>
 │   ├──> _task_id = -1
 │   └──> return -errno
 └──> return 0

注：鉴于ModuleBase::run_trampoline会直接调用instantiate初始化任务，Navigator模块就必须对Navigator::instantiate方法进行重载实现。

3.2 instantiate

初始化Navigator类实例。

Navigator::instantiate
 ├──> Navigator *instance = new Navigator()
 ├──> <instance == nullptr>
 │   └──> PX4_ERR("alloc failed")
 └──> return instance

3.3 init

注：鉴于该模块采用任务，而之前大量的模块使用了WorkQueue的设计方法，在task_spawn里会调用init这个方法，为了对比，保留这个方法。

3.4 Run

根据输入参数及业务逻辑计算输出量，并发布消息。

Navigator::Run
 ├──> [Try to load the geofence: /fs/microsd/etc/geofence.txt]
 ├──> params_update
 ├──> [wakeup source(s) setup]
 │   ├──> fds[0].fd = _local_pos_sub;
 │   ├──> fds[0].events = POLLIN;
 │   ├──> fds[1].fd = _vehicle_status_sub;
 │   ├──> fds[1].events = POLLIN;
 │   ├──> fds[2].fd = _mission_sub;
 │   ├──> fds[2].events = POLLIN;
 │   └──> orb_set_interval(_local_pos_sub, 50);  // rate-limit position subscription to 20 Hz / 50 ms
 └──> <!should_exit()>
     ├──> int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000);  // wait for up to 1000ms for data
     ├──> <pret < 0>  // this is undesirable but not much we can do - might want to flag unhappy status
     │   ├──> PX4_ERR("poll error %d, %d", pret, errno);
     │   ├──> px4_usleep(10000);
     │   └──> continue;
     ├──> [update vehicle_local_position]
     ├──> [update vehicle_status]
     ├──> [update mission]
     ├──> [update vehicle_gps_position]
     ├──> [update vehicle_global_position]
     ├──> [update parameter_update]
     ├──> [update vehicle_land_detected]
     ├──> [update position_controller_status]
     ├──> [update home_position]
     ├──> <_vehicle_command_sub.updated()>
     │   └──> [execute commands]  【发布vehicle_roi/vehicle_command/vehicle_command_ack】
     ├──> check_traffic()
     ├──> geofence_breach_check(have_geofence_position_data)  【发布geofence_result消息】
     ├──> [judge navigation new mode]  // Do not execute any state machine while we are disarmed
     ├──> [reset triplet, when we have a new navigation mode]
     ├──> [iterate through navigation modes and set active/inactive for each]
     ├──> <_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once> // if nothing is running, set position setpoint triplet invalid once
     │   ├──> _pos_sp_triplet_published_invalid_once = true;
     │   └──> reset_triplets();
     ├──> <_pos_sp_triplet_updated>
     │   └──> publish_position_setpoint_triplet();  【发布position_setpoint_triplet消息】
     └──> <_mission_result_updated>
         └──> publish_mission_result();  【发布mission_result消息】

4. 总结

具体逻辑业务后续再做深入，从模块代码角度:

• 输入

uORB::SubscriptionData<position_controller_status_s>	_position_controller_status_subORB_ID(position_controller_status);

uORB::SubscriptionInterval _parameter_update_subORB_ID(parameter_update), 1_s;

uORB::Subscription _global_pos_subORB_ID(vehicle_global_position);	/**< global position subscription */
uORB::Subscription _gps_pos_subORB_ID(vehicle_gps_position);		/**< gps position subscription */
uORB::Subscription _home_pos_subORB_ID(home_position);		/**< home position subscription */
uORB::Subscription _land_detected_subORB_ID(vehicle_land_detected);	/**< vehicle land detected subscription */
uORB::Subscription _pos_ctrl_landing_status_subORB_ID(position_controller_landing_status);	/**< position controller landing status subscription */
uORB::Subscription _traffic_subORB_ID(transponder_report);		/**< traffic subscription */
uORB::Subscription _vehicle_command_subORB_ID(vehicle_command);	/**< vehicle commands (onboard and offboard) */

• 输出

uORB::Publication<geofence_result_s>		_geofence_result_pubORB_ID(geofence_result);
uORB::Publication<mission_result_s>		_mission_result_pubORB_ID(mission_result);
uORB::Publication<position_setpoint_triplet_s>	_pos_sp_triplet_pubORB_ID(position_setpoint_triplet);
uORB::Publication<vehicle_command_ack_s>	_vehicle_cmd_ack_pubORB_ID(vehicle_command_ack);
uORB::Publication<vehicle_command_s>		_vehicle_cmd_pubORB_ID(vehicle_command);
uORB::Publication<vehicle_roi_s>		_vehicle_roi_pubORB_ID(vehicle_roi);

5. 参考资料

【1】PX4开源软件框架简明简介
【2】PX4模块设计之十一：Built-In框架
【3】PX4模块设计之十二：High Resolution Timer设计
【4】PX4模块设计之十三：WorkQueue设计
【5】PX4模块设计之十七：ModuleBase模块
【6】PX4模块设计之三十：Hysteresis类
【7】PX4 modules_main