PX4模式切换流程图commander.cpp.pdf
需积分: 50 167 浏览量
2020-04-26
22:51:27
上传
评论
收藏 256KB PDF 举报
commander流程图初级版
commander_main(int argc, char *argv[])
模块入口函数
daemon_task = px4_task_spawn_cmd("commander"…
启动commander_thread_main主线程
strcmp(argv[1], "stop")
strcmp(argv[1], "calibrate")
strcmp(argv[1], "check")
strcmp(argv[1], "arm")
strcmp(argv[1], "disarm")
strcmp(argv[1], "takeoff")
strcmp(argv[1], "land")
strcmp(argv[1], "transition")
strcmp(argv[1], "mode")
模块带参数指令函数，在nsh终端和软件仿真环境可
以运行可以运行，这些command指令，只是公告，没
有发布，在主线程里面发布，从而改变控制器的模式
状态。
Set parameters(这个模块会更新参数)
1 Led_init()；//主灯初始化
2 Buzzer_init()；//蜂鸣器初始化
3 Status_flags(系统状态标志位，用来标志系统的home_position，
local_position，gps_failure...是否可用)；
5 系统状态标志位初始化；
6 初始化消息；
7 dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s);//
读取mission列表
8 订阅消息orb_subscribe(ORB_ID(sensor_combined));
主线程的while循环
1 更新系统参数
2 检查消息更新orb_copy(ORB_ID(manual_control_setpoint)
orb_copy(ORB_ID(offboard_control_mode)//离线模式在线检查，影响status_flags标志位
orb_copy(ORB_ID(telemetry_status)//数传数据链路检查
orb_copy(ORB_ID(sensor_combined)//气压计数据检查，影响status_flags标志位
orb_copy(ORB_ID(safety)//安全开关解锁检查
orb_check(global_position_sub, &updated);//GPS定位因子检查，定位因子过大表示GPS数
据不行，不能正确的切换到定点,任务等模式。
orb_check(battery_sub, &updated);//电池状态检查，如果电量不足，进入自动降落模式
orb_check(geofence_result_sub, &updated);//地理围栏检查，如果超越地理围栏，执行降
落，或者返回，
if (!status_flags.rc_input_blocked；//遥控器信号，遥控器上锁，遥控器解锁
handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position,&attitude,
&home_pub, &command_ack_pub, &command_ack, &_roi, &roi_pub))
解析vehicle_command消息，并且根据Status，safety等等标志位和main_state_transition函数来做是否允许的
模式切换
main_state_transition(struct vehicle_status_s *status, main_state_t
new_main_state, uint8_t &main_state_prev,status_flags_s *status_flags, struct
commander_state_s *internal_state)
{
switch (new_main_state) {
case commander_state_s::MAIN_STATE_MANUAL:
case commander_state_s::MAIN_STATE_STAB:
}利用switch结构来结合status_flags标志位来判断能不能切换到相应的模式，
如果能切换返回值为 TRANSITION_CHANGED
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);订阅vehicle_command消息。px4io.cpp 遥控器IO模
块和Mavlink_recever.cpp 地面站发来的mavlink指令接收模块，可以发布这个vehicle_command，也就是遥控
器和地面站可以切换模式指令。
commander_set_home_position
设置家的位置，在GPS信号第一次达到最好的时候和每一次解锁(GPS信号
良好)的时候就会设置家的位置。
orb_publish(ORB_ID(home_position)
发布家的位置消息
set_control_mode();
设置control_modef的标志量，通过这些标志量，
在位置控制模块中判断这些标志量，来让飞机进
入不同的状态
orb_publish(ORB_ID(vehicle_control_mode),
control_mode_pub, &control_mode);
发布vehicle_control_mode，位置控制模块订
阅该数据，如果满足需求，模式切换顺利完
成。
bool nav_state_changed = set_nav_state()
根据internal _state>main_state和飞行器状
态status_flags(传感器等硬件正常否)确定
能否完成internal _state> main_state所指定
的模式，若飞行器状态不行，则将模式跟
新为status->nav_state。
commander_low_prio_loop(指令应答线程)
orb_publish(ORB_ID(vehicle_command_ack)
发布指令应答消息，相应的主板灯根据模式
状态发生变化。
资源评论
