地面站设置航点后,是要写入飞控的e2prom,然后执行的时候,从eeprom中取一个执行一个。读和写函数分别是:read_cmd_from_storage,write_cmd_to_storage。另外包括add_cmd
等函数都在ap_mission.cpp文件中。下面我们来看看apm中auto模式执行wp的流程:
mission任务执行代码流程是:run_nav_updates(50hz,在arducopter.cpp中98行定义)-->run_autopilot(navigation.cpp65行,这里有限制mission任务执行的条件,必须是auto模式才执行mission任务)->mission.update。在>mission.update中,每次执行时,或者读取新wp执行,或者检查中间执行过程,或者任务执行完以后的操作,如果是执行完毕,则首先进入loiter模式,如果进入loiter模式出现异常,则进入land模式,
即在最后执行的位置或者执行loiter模式,或者执行land模式。执行mission任务的开始,中间检查,结束三种类型的操作函数在commands_logic.cpp文件中,分别是start_command,verify_command_callback和exit_mission。这三个函数在是copter.cpp文件中copter这个类执行构造函数初始化时,调用mission构造时设置的。在copter.cpp大概27行。
三、除了任务添加和执行以外,还有一个需要注意的地方,那就是mission任务执行过程的进入条件必须是auto模式,那我们还得看看进入auto模式都执行了什么。在auto模式的进入需要执行set_mode函数,而这个函数的执行大概有3个入口:
1 mavlink,即通过gcs控制,具体在arducopter/gcs_mavlink.cpp文件
2 switches,通过遥控器控制,具体在arducopter/switches.cpp文件
3 events,各种故障保护策略,具体在arducopter/events.cpp文件
由于执行wp任务。必须在auto模式,所以我们来看由上面三个入口开始的设置auto模式是怎么执行的。设置auto模式以后,先执行auto_init,
在这个函数,检查进入auto模式的条件,然后执行start_or_resume->resume->start->advance_current_nav_cmd->Copter::start_command。start_command则根据实际cmd的id来执行是take_off,还是rtl,还是labd或者loiter等具体动作模式。例如,我们是auto控制模式,第一个cmd是take_off命令,则执行do_takeoff,在这个函数里设置auto_mode变量为auto_takeoff,设置一些takeoff模式需要的初始变量。
没了,就到这。
auto模式才刚刚开始呢,还需要循环调用啊!!!
fast_loop->update_flight_mode->auto_run,这个函数是循环执行auto模式具体哪个字模式的过程。如我们第一个任务是takeoff,并且在auto_takeoff函数中设置auto_mode=auto_takeoff,这是在auto_run函数会根据这个auto_mode执行auto_takeoff_run。这个函数设置电机输出,最后分别调用航点控制器(wp_nav.update_wpnav),Z轴控制器(pos_control.update_z_controller),姿态控制( auto_takeoff_attitude_run),循环,一直执行。这个run函数调用频率>=100hz. 那这个takeoff任务啥时候结束呢啊?看AP_Mission::update函数,函数说明说这个update调用频率是>=10Hz,在这个循环里检查开始执行的任务是否结束,并且看是否还有cmd要执行。调用流程是这样的:update->verify_command_callback->verify_command->verify_takeoff,这里则根据检查一个标志看飞机飞行那个高度是否达到taktoff设定高度而设置一个标志。而这个标志的循环检测是在auto_takeoff_run函数的航点控制器(wp_nav.update_wpnav->advance_wp_target_along_track函数)中实时更新的。advance_wp_target_along_track函数在ardupilot/libraries/ac_wpnav/ac_wpnav.cpp文件大概690行左右。
在ap_mission::update中如果检查到takeoff执行完毕,即飞行高度达到设定高度。或者取下一个cmd执行,或者执行complete。如果执行下一个航点,例如是waypoint的cmd,则重复mission的执行过程。如果是任务执行完毕,的话,那我们看AP_Mission::complete函数。流程是这样的:
AP_Mission::complete->Copter::exit_mission,这个函数在arducopter/commands_logic.cpp文件中。在这个函数,先尝试执行设置loiter模式,如果失败,则当前位置降落,并且直到检测到落地,disarm电机。这就是大结局。
APM的代码。我怎么感觉应用层代码都是顺序执行,就和单片机里的while循环一样,底层的传感器数据获取类似单片机的中断后台一样。应用层基本上可以认为是有一个大全局变量,在循环里不断更新,而跟新后的全局变量值决定下一步执行究竟干什么,怎么干。