vtol的姿态控制部分vtol_att_control_main

该部分实现VTOL机型的姿态控制部分。

1. 该部分接收来自固定翼以及旋翼的姿态控制部分的数据，并对该数据进行数据。

2. 在数据处理时针对飞机的状态：旋翼,FW还是切换状态分别进行处理。

3. 最后发布电机控制的topic,期望姿态topic。

在具体分析之前，需要明确vtol飞机的四种模式：
a. 切换到旋翼模式 TRANSITION_TO_MC
b. 切换到固定翼模式 TRANSITION_TO_FW
c. 旋翼模式 ROTARY_WING
d. 固定翼模式 FIXED_WING

下面对main函数进行详细分析。

==首先是一系列数据的订阅，以及对默认参数的获取：==

    _v_att_sp_sub          = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));_mc_virtual_att_sp_sub = orb_subscribe(ORB_ID(mc_virtual_attitude_setpoint));_fw_virtual_att_sp_sub = orb_subscribe(ORB_ID(fw_virtual_attitude_setpoint));_mc_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(mc_virtual_rates_setpoint));_fw_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(fw_virtual_rates_setpoint));_v_att_sub             = orb_subscribe(ORB_ID(vehicle_attitude));_v_control_mode_sub    = orb_subscribe(ORB_ID(vehicle_control_mode));_params_sub            = orb_subscribe(ORB_ID(parameter_update));_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));_local_pos_sub         = orb_subscribe(ORB_ID(vehicle_local_position));_local_pos_sp_sub         = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));_pos_sp_triplet_sub    = orb_subscribe(ORB_ID(position_setpoint_triplet));_airspeed_sub          = orb_subscribe(ORB_ID(airspeed));_vehicle_cmd_sub       = orb_subscribe(ORB_ID(vehicle_command));_tecs_status_sub = orb_subscribe(ORB_ID(tecs_status));_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));_actuator_inputs_mc    = orb_subscribe(ORB_ID(actuator_controls_virtual_mc));_actuator_inputs_fw    = orb_subscribe(ORB_ID(actuator_controls_virtual_fw));parameters_update();

==接下来是设置默认mc模式下，默认的pwm的输出。由函数==

_vtol_type->set_idle_mc();

==实现。具体代码逻辑如下：==

void VtolType::set_idle_mc()
{const char *dev = PWM_OUTPUT0_DEVICE_PATH;int fd = px4_open(dev, 0); //获得fd句柄if (fd < 0) {PX4_WARN("can't open %s", dev);}unsigned servo_count;int ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);//获得servo的数目unsigned pwm_value = _params->idle_pwm_mc;struct pwm_output_values pwm_values;memset(&pwm_values, 0, sizeof(pwm_values));
/*设置默认的pwm值，默认为900，最小值*/for (int i = 0; i < _params->vtol_motor_count; i++) {pwm_values.values[i] = pwm_value;pwm_values.channel_count++;}ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);if (ret != OK) {PX4_WARN("failed setting min values");}px4_close(fd);flag_idle_mc = true;
}

==下面进入while循环部分。在while循环里阻塞等待的是电机输入：==

int pret = px4_poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 100);

==在while之前以及在while里可以看到如下代码：==

==while之前==

    px4_pollfd_struct_t fds[1] = {};fds[0].fd     = _actuator_inputs_mc;fds[0].events = POLLIN;

==while循环中。在该部分可以看出来，在转换模式以及旋翼模式下，一直获取的是旋翼的的电机输入，只有在固定翼模式下才获取固定翼输入。猜测在着前面状态下，飞机以旋翼机的形式进行处理等操作。==

        switch (_vtol_type->get_mode()) {case TRANSITION_TO_FW:case TRANSITION_TO_MC:case ROTARY_WING:fds[0].fd = _actuator_inputs_mc;break;case FIXED_WING:fds[0].fd = _actuator_inputs_fw;break;}

==###### 接下来是对飞机模式的判读。确认飞机是在那种模式下，下面指针对尾坐式的飞机进行解读：==

_vtol_type->update_vtol_state();

==具体代码为如下部分。在注释部分可以看到，当发出切换到固定翼的请求后，飞机会开始倾斜并且获得一个向前的速度，当速度足够高并且pitch足够大后，飞机开始进入固定翼模式；同理，当发出切换到旋翼请求后，pitch会在旋翼模式下进行控制，当pitch满足后，进入旋翼模式。==

==在这部分需要明确两个概念。一个是vtol模式，以及schedule模式。vtol模式前面已经说明。schedule模式，感觉目的就是为了pitch进行转换，具体分为五种：MC_MODE，FW_MODE，TRANSITION_FRONT_P1，TRANSITION_FRONT_P2，TRANSITION_BACK，后边三种都是过渡状态。具体对应可以看这段代码的最后部分。==

void Tailsitter::update_vtol_state()
{matrix::Eulerf euler = matrix::Quatf(_v_att->q);float pitch = euler.theta(); //获得当前的pitch角度//_attc->is_fixed_wing_requested()这句话理解为是否发出固定翼请求，也就是说开关切换到固定翼选项后，返回true，其他状态都为falseif (!_attc->is_fixed_wing_requested()) {//非固定翼请求模式，也就是旋翼模式switch (_vtol_schedule.flight_mode) { // user switchig to MC modecase MC_MODE:break;case FW_MODE://这部分可以看出来，固定翼模式下切换，先切换到TRANSITION_BACK模式。_vtol_schedule.flight_mode  = TRANSITION_BACK;_vtol_schedule.transition_start = hrt_absolute_time();break;case TRANSITION_FRONT_P1:// failsafe into multicopter mode_vtol_schedule.flight_mode = MC_MODE;break;case TRANSITION_FRONT_P2:// NOT USED// failsafe into multicopter mode//_vtol_schedule.flight_mode = MC_MODE;break;case TRANSITION_BACK://这部分可以看出来，当pitch角度足够了，再切换到旋翼模式。// check if we have reached pitch angle to switch to MC modeif (pitch >= PITCH_TRANSITION_BACK) {_vtol_schedule.flight_mode = MC_MODE;}break;}} else {  // user switchig to FW mode
//这部分是旋翼切换到固定翼模式switch (_vtol_schedule.flight_mode) {case MC_MODE://在旋翼模式下，先切换到TRANSITION_FRONT_P1模式。// initialise a front transition_vtol_schedule.flight_mode  = TRANSITION_FRONT_P1;_vtol_schedule.transition_start = hrt_absolute_time();break;case FW_MODE:break;case TRANSITION_FRONT_P1:
//在该模式下，当空速足够并且pithchz足够后，再切换到固定翼模式。或者当前在地面，可以直接切换。// check if we have reached airspeed  and pitch angle to switch to TRANSITION P2 modeif ((_airspeed->indicated_airspeed_m_s >= _params_tailsitter.airspeed_trans&& pitch <= PITCH_TRANSITION_FRONT_P1) || can_transition_on_ground()) {_vtol_schedule.flight_mode = FW_MODE;//_vtol_schedule.transition_start = hrt_absolute_time();}break;case TRANSITION_FRONT_P2:case TRANSITION_BACK:// failsafe into fixed wing mode_vtol_schedule.flight_mode = FW_MODE;/*  **LATER***  if pitch is closer to mc (-45>)*   go to transition P1*/break;}}// map tailsitter specific control phases to simple control modes//下面部分就是讲shedule飞行模式映射到具体的vtol的模式中去。switch (_vtol_schedule.flight_mode) {case MC_MODE:_vtol_mode = ROTARY_WING;_vtol_vehicle_status->vtol_in_trans_mode = false;_flag_was_in_trans_mode = false;break;case FW_MODE:_vtol_mode = FIXED_WING;_vtol_vehicle_status->vtol_in_trans_mode = false;_flag_was_in_trans_mode = false;break;case TRANSITION_FRONT_P1:_vtol_mode = TRANSITION_TO_FW;_vtol_vehicle_status->vtol_in_trans_mode = true;break;case TRANSITION_FRONT_P2:_vtol_mode = TRANSITION_TO_FW;_vtol_vehicle_status->vtol_in_trans_mode = true;break;case TRANSITION_BACK:_vtol_mode = TRANSITION_TO_MC;_vtol_vehicle_status->vtol_in_trans_mode = true;break;}
}

TODO

==回到vtol的姿态控制部分的代码接着看。下面的部分是在手动模式下，根据当前的飞机模式选择_transition_command。这部分还不清楚这个变量的作用。。。。。。做个标记==

        if (_v_control_mode.flag_control_manual_enabled) {if (_vtol_type->get_mode() == ROTARY_WING) {_transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;} else if (_vtol_type->get_mode() == FIXED_WING) {_transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW;} else if (_vtol_type->get_mode() == TRANSITION_TO_MC) {/* We want to make sure that a mode change (manual>auto) during the back transition* doesn't result in an unsafe state. This prevents the instant fall back to* fixed-wing on the switch from manual to auto */_transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;}}

==接下来的部分就是根据飞机的模式，进行设置。在这部分中，通过获取当前期望的姿态以及飞机的模式调整对应的期望姿态。==

在旋翼模式以及固定翼模式下，直接将获取的期望姿态复制到_v_att_sp中，最后Pubslish出去。

if (_vtol_type->get_mode() == ROTARY_WING) {mc_virtual_att_sp_poll();//获取_mc_virtual_att_sp，也就是mc的期望姿态。还没看到在哪里publish出来的。// vehicle is in rotary wing mode_vtol_vehicle_status.vtol_in_rw_mode = true;_vtol_vehicle_status.vtol_in_trans_mode = false;// got data from mc attitude controller_vtol_type->update_mc_state();//把上面获取的virtual姿态设置为真正的姿态_v_att_sp，//memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s))fill_mc_att_rates_sp();//填充发布期待的姿态rate信息。} else if (_vtol_type->get_mode() == FIXED_WING) {fw_virtual_att_sp_poll();//获取当前固定翼的期望姿态// vehicle is in fw mode_vtol_vehicle_status.vtol_in_rw_mode = false;_vtol_vehicle_status.vtol_in_trans_mode = false;_vtol_type->update_fw_state();//填充期望姿态：memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s))。在这个函数里有函数check_quadchute_condition，主要是检测不满足条件转换失败fill_fw_att_rates_sp();} else if (_vtol_type->get_mode() == TRANSITION_TO_MC || _vtol_type->get_mode() == TRANSITION_TO_FW) {mc_virtual_att_sp_poll();//获取旋翼的期望姿态fw_virtual_att_sp_poll();//获取固定翼期望姿态// vehicle is doing a transition_vtol_vehicle_status.vtol_in_trans_mode = true;_vtol_vehicle_status.vtol_in_rw_mode = true; //making mc attitude controller work during transition_vtol_vehicle_status.in_transition_to_fw = (_vtol_type->get_mode() == TRANSITION_TO_FW);_vtol_type->update_transition_state();//这是过渡阶段的期望姿态，后边做详细分析fill_mc_att_rates_sp();//发布旋翼的姿态速度信息}

==接着就是根据飞机的状态，填充电机信息==

_vtol_type->fill_actuator_outputs();

上面的具体实现形式为：

void Tailsitter::fill_actuator_outputs()
{switch (_vtol_mode) {case ROTARY_WING://在旋翼模式下，直接将获取的旋翼的电机信息填充到电机actuators_out_0中取_actuators_out_0->timestamp = _actuators_mc_in->timestamp;_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL];_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] =_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH];_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW];_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];_actuators_out_1->timestamp = _actuators_mc_in->timestamp;if (_params->elevons_mc_lock == 1) {_actuators_out_1->control[0] = 0;_actuators_out_1->control[1] = 0;} else {// NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation!//在这里需要注意，旋翼模式下，旋翼的yaw相当于固定翼的roll；旋翼的pitch相当于固定翼的pitch。旋翼的roll不需要考虑_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] =_actuators_mc_in->control[actuator_controls_s::INDEX_YAW];  //roll elevon_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH];    //pitch elevon}break;case FIXED_WING:// in fixed wing mode we use engines only for providing thrust, no moments are generated//这个状态下，旋翼的yaw.roll.pitch不需要考虑，只保持推力就OK。_actuators_out_0->timestamp = _actuators_fw_in->timestamp;_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = 0;_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = 0;_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = 0;_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] =-_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL];    // roll elevon_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim;   // pitch elevon_actuators_out_1->control[actuator_controls_s::INDEX_YAW] =_actuators_fw_in->control[actuator_controls_s::INDEX_YAW];  // yaw_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] =_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttlebreak;case TRANSITION_TO_FW:case TRANSITION_TO_MC:// in transition engines are mixed by weight (BACK TRANSITION ONLY)//在这个状态下，需要把固定翼以及旋翼的 actuators都赋值，并且两个有不同的权重。_mc_roll_weight，_mc_pitch_weight，_mc_yaw_weight均在更新期望姿态信息时计算得出。_actuators_out_0->timestamp = _actuators_mc_in->timestamp;_actuators_out_1->timestamp = _actuators_mc_in->timestamp;_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]* _mc_roll_weight;_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] =_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] *_mc_yaw_weight;_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];// NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation!_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]* (1 - _mc_yaw_weight);//这个对值进行取反操作，不知道为什么....TODO_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;// **LATER** + (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight);_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] =_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];break;}
}

==在最后发布期望姿态信息以及电机信息。==

==现在对转换过程中，计算期望姿态函数进行分析：
在分析之前需要明确，转换过程其实就是pitch的变化过程。需要明确知道下面三个概念：==
1. 开始的pitch姿态
2. 转换后的pitch姿态
3. 转换时间
也就是说需要在确认的转换时间内姿态转换。

void Tailsitter::update_transition_state()
{if (!_flag_was_in_trans_mode) {//这部分获得初始的期望pitch，以及推力thrust// save desired heading for transition and last thrust value_yaw_transition = _v_att_sp->yaw_body;//transition should start from current attitude instead of current setpointmatrix::Eulerf euler = matrix::Quatf(_v_att->q);_pitch_transition_start = euler.theta();_thrust_transition_start = _v_att_sp->thrust;_flag_was_in_trans_mode = true;}if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) {/** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/_v_att_sp->pitch_body = _pitch_transition_start - (fabsf(PITCH_TRANSITION_FRONT_P1 - _pitch_transition_start) *(float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur * 1000000.0f));//这部分就时对期望姿态做调整，在固定时间内完成_v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body, PITCH_TRANSITION_FRONT_P1 - 0.2f,_pitch_transition_start);/** create time dependant throttle signal higher than  in MC and growing untill  P2 switch speed reached *///当空速不够时，需要慢慢加大推力if (_airspeed->indicated_airspeed_m_s <= _params_tailsitter.airspeed_trans) {_thrust_transition = _thrust_transition_start + (fabsf(THROTTLE_TRANSITION_MAX * _thrust_transition_start) *(float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur * 1000000.0f));_thrust_transition = math::constrain(_thrust_transition, _thrust_transition_start,(1.0f + THROTTLE_TRANSITION_MAX) * _thrust_transition_start);_v_att_sp->thrust = _thrust_transition;}// disable mc yaw control once the plane has picked up speedif (_airspeed->indicated_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) {_mc_yaw_weight = 0.0f;} else {_mc_yaw_weight = 1.0f;}_mc_roll_weight = 1.0f;_mc_pitch_weight = 1.0f;} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) {//这部分没用到，暂时不管// the plane is ready to go into fixed wing mode, smoothly switch the actuator controls, keep pitching down/** no motor  switching */if (flag_idle_mc) {set_idle_fw();flag_idle_mc = false;}/** create time dependant pitch angle set point  + 0.2 rad overlap over the switch value*/if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_FRONT_P2 - 0.2f)) {_v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P1 -(fabsf(PITCH_TRANSITION_FRONT_P2 - PITCH_TRANSITION_FRONT_P1) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f));if (_v_att_sp->pitch_body <= (PITCH_TRANSITION_FRONT_P2 - 0.2f)) {_v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P2 - 0.2f;}}_v_att_sp->thrust = _thrust_transition;/** start blending MC and FW controls from pitch -45 to pitch -70 for smooth control takeover*///_mc_roll_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f));//_mc_pitch_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f));_mc_roll_weight = 0.0f;_mc_pitch_weight = 0.0f;/** keep yaw disabled */_mc_yaw_weight = 0.0f;} else if (_vtol_schedule.flight_mode == TRANSITION_BACK) {if (!flag_idle_mc) {set_idle_mc();flag_idle_mc = true;}/** create time dependant pitch angle set point stating at -pi/2 + 0.2 rad overlap over the switch value*///这部分是从-90到-14度的转换，但是对这里的_pitch_transition_start初始值想不太明白。_v_att_sp->pitch_body = M_PI_2_F + _pitch_transition_start + fabsf(PITCH_TRANSITION_BACK + 1.57f) *(float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.back_trans_dur * 1000000.0f);_v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body, -2.0f, PITCH_TRANSITION_BACK + 0.2f);//  throttle value is decreesed_v_att_sp->thrust = _thrust_transition_start * 0.9f;/** keep yaw disabled */_mc_yaw_weight = 0.0f;/** smoothly move control weight to MC *///mc的权重不断变大_mc_roll_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) /(_params_tailsitter.back_trans_dur * 1000000.0f);_mc_pitch_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) /(_params_tailsitter.back_trans_dur * 1000000.0f);}_mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f);_mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f);_mc_pitch_weight = math::constrain(_mc_pitch_weight, 0.0f, 1.0f);// compute desired attitude and thrust setpoint for the transition_v_att_sp->timestamp = hrt_absolute_time();_v_att_sp->roll_body = 0.0f;_v_att_sp->yaw_body = _yaw_transition;math::Quaternion q_sp;q_sp.from_euler(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body);memcpy(&_v_att_sp->q_d[0], &q_sp.data[0], sizeof(_v_att_sp->q_d));
}

本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！