PX4 navigator-TAKEOFF

                                                                              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。