pixhawk commander分析

2019-10-30

commander.cpp

commander.cpp主要是通过指令改变当前的飞行模式

指令分为：
* 遥控器的指令
* 上位机控制台的指令
* mavlink指令

飞行模式分为
* manual
* altctl
* posctl
* mission
* rtl
* loiter
* acro
* offboard
* stabilized
* rattitude
* takeoff
* land

 基本流程为订阅数据，处理数据，发布数据

遥控器指令

arm/disarm

上锁逻辑：
因为对于上锁解锁这种条件，不需要改动，也不能改动，安全最为重要，所以按照它的逻辑必定是可以上锁解锁。

首先判断是不是要进入上锁模式，即在已经解锁状态，遥控器油门低且yaw轴摇杆小于0.9×1000

 /* DISARM
 * check if left stick is in lower left position or arm button is pushed or arm switch has transition from arm to disarm
 * and we are in MANUAL, Rattitude, or AUTO_READY mode or (ASSIST mode and landed)
 * do it only for rotary wings in manual mode or fixed wing if landed */
const bool stick_in_lower_left = sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f;
const bool arm_switch_to_disarm_transition =  arm_switch_is_button == 0 &&
        _last_sp_man_arm_switch == manual_control_setpoint_s::SWITCH_POS_ON &&
        sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF;
		
if (in_armed_state &&
    status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF &&
    (status.is_rotary_wing || (!status.is_rotary_wing && land_detector.landed)) &&
    (stick_in_lower_left || arm_button_pressed || arm_switch_to_disarm_transition) ) {

再判断是不是手动的模式（MAIN_STATE_MANUAL，MAIN_STATE_ACRO，MAIN_STATE_STAB,MAIN_STATE_RATTITUDE）且不是正在降落

if (internal_state.main_state != commander_state_s::MAIN_STATE_MANUAL &&
                                         internal_state.main_state != commander_state_s::MAIN_STATE_ACRO &&
                                         internal_state.main_state != commander_state_s::MAIN_STATE_STAB &&
                                         internal_state.main_state != commander_state_s::MAIN_STATE_RATTITUDE &&
                                         !land_detector.landed) {
                                     print_reject_arm("NOT DISARMING: Not in manual mode or landed yet.");
		
                                 }

不满足上面条件的话，就上锁

arming_ret = arming_state_transition(&status,
                                     &battery,
                                     &safety,
                                     new_arming_state,
                                     &armed,
                                     true /* fRunPreArmChecks */,
                                     &mavlink_log_pub,
                                     &status_flags,
                                     avionics_power_rail_voltage,
                                     arm_requirements,
                                     hrt_elapsed_time(&commander_boot_timestamp));

解锁

逻辑大致与上锁相同，只是摇杆yaw轴要大于0.9*1000

模式切换

只有在遥控器（rc）有输入的时候才能切换
1
2
if (!status_flags.rc_input_blocked && sp_man.timestamp != 0 &&
(hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f))) {
主要函数main_res = set_main_state(&status, &global_position, &local_position, &status_changed)

主要输入飞行器状态，位置信息，internal_state

返回TRANSITION_CHANGED/TRANSITION_NOT_CHANGED与internal_state->main_state.

控制台指令

分为两种情况：直接切换模式，发送cmd指令切换数据

直接切换数据，就只能切换模式。（所有的模式都能直接切换）
发送cmd切换数据，不仅能切换模式，还能在切换模式之后发送固定数据。（只有三种模式可以这么切换，takeoff，land，transition，且必须要有位置数据）

直接切换模式

在控制台发送commander mode auto：(misison)进入TRANSITION_DENIED == main_state_transition(&status, new_main_state, main_state_prev, &status_flags, &internal_state)

发送cmd指令切换数据

在控制台输入commander takeoff commander land，它数据都已经写好了，所以要想改变数据的话，就要改动代码

/* see if we got a home position */
    if (status_flags.condition_local_position_valid) {
	
        if (TRANSITION_DENIED != arm_disarm(true, &mavlink_log_pub, "command line")) {
	
            struct vehicle_command_s cmd = {
                .timestamp = hrt_absolute_time(),
                .param5 = NAN,
                .param6 = NAN,
                /* minimum pitch */
                .param1 = NAN,
                .param2 = NAN,
                .param3 = NAN,
                .param4 = NAN,
                .param7 = NAN,
                .command = vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF,
                .target_system = status.system_id,
                .target_component = status.component_id
            };
	
            orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
            (void)orb_unadvertise(h);
	
        } else {
            warnx("arming failed");
        }

orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH)这句话将cmd topic发送出去

在while中处理cmd消息
cmdtopic更新的话会进入handle_command()函数
主要输入status cmd

判断cmd的模式然后进入main_state_transition(&status, new_main_state, main_state_prev, &status_flags, &internal_state)
切换模式

改变飞行模式

主要函数set_nav_state()

输入:通过之前设置的internal_state->main_state与相应的条件，来确定飞行器到底能不能改变模式
输出：status->nav_state
例如：position_ctrl只需要rc连接，解锁，有位置信息

 case commander_state_s::MAIN_STATE_POSCTL: {

    if (rc_lost && is_armed) {
        enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);

        set_rc_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act);

        /* As long as there is RC, we can fallback to ALTCTL, or STAB. */
        /* A local position estimate is enough for POSCTL for multirotors,
         * this enables POSCTL using e.g. flow.
         * For fixedwing, a global position is needed. */

    } else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, !(posctl_nav_loss_act == 1), !status->is_rotary_wing)) {
        // nothing to do - everything done in check_invalid_pos_nav_state

    } else {
        status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
    }
}
break;

set_control_mode()

根据之前的status.nav_state来设置control_mode。

switch (status.nav_state) {
case vehicle_status_s::NAVIGATION_STATE_MANUAL:
	control_mode.flag_control_manual_enabled = true;
	control_mode.flag_control_auto_enabled = false;
	control_mode.flag_control_rates_enabled = stabilization_required();
	control_mode.flag_control_attitude_enabled = stabilization_required();
	control_mode.flag_control_rattitude_enabled = false;
	control_mode.flag_control_altitude_enabled = false;
	control_mode.flag_control_climb_rate_enabled = false;
	control_mode.flag_control_position_enabled = false;
	control_mode.flag_control_velocity_enabled = false;
	control_mode.flag_control_acceleration_enabled = false;
	control_mode.flag_control_termination_enabled = false;
	break;
	
case vehicle_status_s::NAVIGATION_STATE_STAB:
	control_mode.flag_control_manual_enabled = true;
	control_mode.flag_control_auto_enabled = false;
	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 = false;
	control_mode.flag_control_climb_rate_enabled = false;
	control_mode.flag_control_position_enabled = false;
	control_mode.flag_control_velocity_enabled = false;
	control_mode.flag_control_acceleration_enabled = false;
	control_mode.flag_control_termination_enabled = false;
	/* override is not ok in stabilized mode */
	control_mode.flag_external_manual_override_ok = false;
	break;
	...

公布数据

control_mode.timestamp = now;
orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);

status.timestamp = now;
orb_publish(ORB_ID(vehicle_status), status_pub, &status);