以TAKEOFF为例说明PX4中一个飞行模式的启动流程
众所周知由遥控器或者地面站决定Main state作为用户期望到达的飞行模式
然后有commander进行条件判断后输出一个Navigation state作为飞机最终的飞行模式
进而由navigator模块中的函数作为具体的飞行模式实现方法
具体的实现还得看位置控制
一、 首先从commander.cpp选中TAKEOFF开始
if (!strcmp(argv[1], "takeoff"))
这里需要注意的是
cmd.command = vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF;
此结构体vehicle_command_s cmd
被赋值,用户希望进入TAKEOFF模式。
这里省略对是否能切换到该模式的条件判断,直接进入TAKEOFF导航模式set_control_mode()
void
set_control_mode()
{
/* set vehicle_control_mode according to set_navigation_state */
...
switch (status.nav_state) {
...
case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = true;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_rattitude_enabled = false;
control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = !status.in_transition_mode;
control_mode.flag_control_velocity_enabled = !status.in_transition_mode;
control_mode.flag_control_acceleration_enabled = false;
control_mode.flag_control_termination_enabled = false;
break;
...
}
二、 进入navigator_main函数
① 在主函数中完成对vehicle_command
话题的检查更新后,根据commander中的模式选择为当前的结构体rep
赋值
if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) {
/ TAKEOFF 起飞
struct position_setpoint_triplet_s *rep = get_takeoff_triplet(); // takeoff_triplet
// store current position as previous position and goal as next
// 将现在的位置作为之前的位置存起来,将目标位置作为下一个航点
rep->previous.yaw = get_global_position()->yaw;
rep->previous.lat = get_global_position()->lat;
rep->previous.lon = get_global_position()->lon;
rep->previous.alt = get_global_position()->alt;
rep->current.loiter_radius = get_loiter_radius();
rep->current.loiter_direction = 1; // 顺时针
rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
rep->current.yaw = cmd.param4; // 偏航角(if magnetometer present), ignored without magnetometer
/****
MAV_CMD_NAV_TAKEOFF Takeoff from ground / hand
Mission Param #1 Minimum pitch (if airspeed sensor present), desired pitch without sensor
Mission Param #2 Empty
Mission Param #3 Empty
Mission Param #4 Yaw angle (if magnetometer present), ignored without magnetometer
Mission Param #5 Latitude
Mission Param #6 Longitude
Mission Param #7 Altitude
*****/
if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7;
rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7;
} else {
// If one of them is non-finite, reset both
rep->current.lat = NAN;
rep->current.lon = NAN;
}
rep->current.alt = cmd.param7;
rep->previous.valid = true;
rep->current.valid = true;
rep->next.valid = false;
}
② 根据commander设定的导航状态进入相应导航模式
switch (_vstatus.nav_state) {
...
case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_takeoff; // takeoff模式
break;
...
}
③ 遍历导航模式,并为每个模式设置active/inactive
/* iterate through navigation modes and set active/inactive for each */
// 遍历导航模式,并为每个模式设置active/inactive
for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
_navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);
}
由②已经知道_navigation_mode = &_takeoff
,所以这里run
激活的是takeoff飞行模式。其中
_navigation_mode_array[0] = &_mission;
_navigation_mode_array[1] = &_loiter;
_navigation_mode_array[2] = &_rtl;
_navigation_mode_array[3] = &_dataLinkLoss;
_navigation_mode_array[4] = &_engineFailure;
_navigation_mode_array[5] = &_gpsFailure;
_navigation_mode_array[6] = &_rcLoss;
_navigation_mode_array[7] = &_takeoff;
_navigation_mode_array[8] = &_land;
_navigation_mode_array[9] = &_follow_target;
注意: navigator模块中每一种模式都对应有on_inactive 、on_activation和on_active三个模式,由NavigatorMode::run(bool active)选择
void
NavigatorMode::run(bool active)
{
if (active) {
if (_first_run) { // 首次运行
/* first run */
_first_run = false;
/* Reset stay in failsafe flag */
_navigator->get_mission_result()->stay_in_failsafe = false;
_navigator->set_mission_result_updated();
/* 当模式激活时此函数会被调用一次,pos_sp_triplet必须在此初始化 */
on_activation(); // 运行我
} else {
/* 模式已激活时调用此函数 */
on_active(); // 实际功能实现
}
} else {
/* 模式未激活时调用此函数 */
_first_run = true;
//
on_inactive();
}
}
由此可见一个模式的启用在navigator中是先经过on_activation()
初始化,然后在on_active
完成功能实现。
三、 takeoff过程
TAKEOFF其实没有什么好说的,必须有GPS才可以。
首先初始化home点的航点信息set_takeoff_position()
,然后起飞MIS_TAKEOFF_ALT
高度后悬停_navigator->set_can_loiter_at_sp(true)
By Fantasy