Pixhawk原生固件PX4之TAKEOFF的启动流程

固件PX4之TAKEOFF的启动流程"/>

Pixhawk原生固件PX4之TAKEOFF的启动流程

欢迎交流~ 个人 Gitter 交流平台，点击直达：

以TAKEOFF为例说明PX4中一个飞行模式的启动流程

• 众所周知由遥控器或者地面站决定Main state作为用户期望到达的飞行模式

• 然后有commander进行条件判断后输出一个Navigation state作为飞机最终的飞行模式

• 进而由navigator模块中的函数作为具体的飞行模式实现方法

• 具体的实现还得看位置控制

一、 首先从commander.cpp选中TAKEOFF开始

if (!strcmp(argv[1], "takeoff"))

这里需要注意的是

cmdmand = 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 (cmdmand == 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 / handMission Param #1    Minimum pitch (if airspeed sensor present), desired pitch without sensorMission Param #2    EmptyMission Param #3    EmptyMission Param #4    Yaw angle (if magnetometer present), ignored without magnetometerMission Param #5    LatitudeMission Param #6    LongitudeMission 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 bothrep->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