该部分实现VTOL机型的姿态控制部分。
该部分接收来自固定翼以及旋翼的姿态控制部分的数据,并对该数据进行数据。
在数据处理时针对飞机的状态:旋翼,FW还是切换状态分别进行处理。
最后发布电机控制的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,其他状态都为false
if (!_attc->is_fixed_wing_requested()) {//非固定翼请求模式,也就是旋翼模式
switch (_vtol_schedule.flight_mode) { // user switchig to MC mode
case 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 mode
if (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 mode
if ((_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]; // throttle
break;
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 setpoint
matrix::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 speed
if (_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));
}