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。