2017.9.22 --edited by sonwpang
1.TAKE OFF 构造函数
确定起飞高度 MIS_TAKEOFF_ALT定义private
control::BlockParamFloat _param_min_alt;
调用Block::updateParams()进行参数更新
2.流程
由地面战或者RC端发送MAVLINK_MSG_ID_COMMAND_LONG 76消息
在中间层有commander.cpp中的command_handle()函数进行处理。
case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF:
由函数main_state_transition()进行状态更新，
这里做一下细节解析：
transition_result_t
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) 函数返回转换状态，
返回结果如下:TRANSITION_DENIED = -1,TRANSITION_NOT_CHANGED = 0,TRANSITION_CHANGED
vehicle_status_s:飞行器状态，区别是旋翼还是固定翼等等;
main_state_t:主状态，当前设定的状态 eg:commander_state_s::MAIN_STATE_AUTO_TAKEOFF;
status_flags_s:状态条件
commander_state_s:当前的飞行模式
在此判断各种模式
case commander_state_s::MAIN_STATE_AUTO_TAKEOFF:
/* need global position and home position */
if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
需要有定位，才可以执行TAKEOFF。
if (ret == TRANSITION_CHANGED) {
if (internal_state->main_state != new_main_state) {
main_state_prev = internal_state->main_state;
internal_state->main_state = new_main_state;
internal_state->timestamp = hrt_absolute_time();
} else {
ret = TRANSITION_NOT_CHANGED;
}
}
void answer_command(struct vehicle_command_s &cmd, unsigned result,
orb_advert_t &command_ack_pub, vehicle_command_ack_s &command_ack)
做command指令响应，并发布。
并通过orb_publish(ORB_ID(vehicle_status), status_pub, &status);将用户模式指令切换发布出去。
然后在navigator_main.cpp中订阅，通过switch (_vstatus.nav_state)进行任务分配。
case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_takeoff;
break;
迭代判断哪个模式激活，哪个模式失能，
for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
_navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);
}
例如是TAKEOFF模式，根据下面数组即可知道当前模式设定，其他模式均失能。
/* Create a list of our possible navigation types */
_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] = &_descend;
_navigation_mode_array[10] = &_follow_target;
接下来是run这个函数，在NavigatorMode.cpp中，对各个任务进行激活，激活之前先进行一次
任务初始化on_activation()，然后on_activation()。比如在TAKEOFF模式时，先设定起飞位置，
在on_activation()中，设定起飞位置set_takeoff_position()，在这个函数中，根据设定起飞高度，求出
绝对高度min_abs_altitude = _navigator->get_home_position()->alt + _param_min_alt.get();设置结构体_mission_item中的高度为abs_altitude，然后进行任务状态更新，并且限制起飞高度，更新pos_sp_triplet->current结构体，主要是global—>alt更新高度,lat\lon\yaw为当前值，然后memset(rep, 0, sizeof(*rep));这个很重要，pos_sp_triplet只设定一次。并将当前sp的vaild变量置位，最后将状态_pos_sp_triplet_updated更新为true。
在on_activation()中，判断is_mission_item_reached()，判断高度是否到达，yaw设定是否完成，完成后设定为悬停模式，更新悬停triplet_sp。