程序员经验分享-hwhale

vtol的姿态控制部分vtol_att_control_main

2023-05-16

该部分实现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，其他状态都为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));
}
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

vtol的姿态控制部分vtol_att_control_main 的相关文章

  • 北醒激光雷达模组 资料汇总

    目录 1 文档说明1 1 北醒单点系列雷达激光模组相关资料1 2 北醒面阵系列雷达激光模组相关资料1 2 1 产品基本介绍1 2 2 Benewake 北醒 短距 TF LC02 2m资料整理1 2 3 Benewake 北醒 短距 TF

  • TFmini Plus在开源飞控PX4上的应用

    TFmini Plus在开源飞控PX4上的应用 PX4有着自己独特的优势 xff0c 受到广大爱好者的喜爱 TFmini Plus是北醒公司推出的性价比极高的激光雷达 xff0c 受到广大爱好者的追捧 本文介绍TFmini Plus和PX4

  • Benewake TFmini-S\TFmimi Plus\TFluna\TF02-Pro 串口版本雷达在STM32的例程

    目录 文档说明北醒串口标准通讯协议硬件接线Lidar通讯代码1 初始化USART1 2 开启USART1的空闲中断 3 USART2 IRQHandler增加中断判断4 中断处理函数 xff0c 用于接收雷达数据 协议处理注 xff1a 换

  • 使用CH341 I2C连接北醒TF系列I2C模式 Python例程

    目录 硬件接线 xff1a 源码结果输出 本文介绍了北醒单点系列雷达IIC模式下使用CH341芯片转接板读取雷达数据的例程 例程下载 xff1a 链接 https pan baidu com s 1KVJ fINxUgKZny2Gdi8T2

  • 蓝牙nrf51822程序的分析(一)

    蓝牙nrf51822程序的分析 一 最近继续用NRF51822开发一个东西 无奈之前没接触过蓝牙 连蓝牙串口模块也没有 所以对蓝牙的基础知识不够 xff0c 后面看了之后接着补充 花了2天时间把提供的NRF51822的程序大致看明白了 xf

  • 常用Arduino板介绍

    目录 NANO板介绍烧录说明 UNO板介绍烧录说明 Pro mini板介绍烧录说明 DUE板介绍烧录说明 NANO板介绍 概述 xff1a Arduino Nano是一款基于ATMega328P xff08 Arduino Nano 3 x

  • Modbus设备在Modbus scan上面的使用方法

    操作教程 参数 xff1a DeviceID xff1a 485从站 寄存器地址 xff1a 查询设备地址表 北醒雷达Dist在0x0000开始 读取寄存器长度 xff1a 雷达数据长度值 格式 xff1a MODBUS RTU 串口协议

  • Raspberry Pi Pico C/C++语言在Windows环境下开发环境搭建 Raspberry Pi Pico C/C++ SDK

    目录 前言Raspberry Pi Pico介绍需要支持的软件软件安装配置及注意事项ARM GCC compiler的安装CMake的安装Git 安装Visual Studio 2019的安装Visual Studio Code的配置Pyt

  • 【LoRa32U4II】介绍以及基于Arduino IDE编译环境搭建及测试

    目录 LoRa 模块LoRa32u4 II介绍LoRa32u4 II 资料下载LoRa32u4 II 规格介绍LoRa32u4 II 脚位说明 编译环境介绍电脑系统编译软件Arduino需求库 编译环境搭建及测试LoRa32u4 II 测试

  • 【Benewake(北醒) 】短距 TF-LC02 2m资料整理

    目录 1 TF LC02简要说明1 1 性能参数1 2产品图片及尺寸 2 运用2 1 在开源板Arduino上的运用2 2 在Python上的应用 1 TF LC02简要说明 1 1 性能参数 1 2产品图片及尺寸 2 运用 2 1 在开源

  • 【Arduino】Benewake(北醒) TF-LC02(TTL)基于Arduino 开发板运用说明

    目录 前言Benewake 北醒 TF LC02产品简要说明Arduino开发板介绍Benewake 北醒 TF LC02 接口及通讯协议说明接口定义串口协议说明通讯协议说明功能码说明 接线示意图例程说明配置软硬串口定义获取TOF数据的结构

  • 【Benewake(北醒) 】中距 TF02-i 40m工业版本CAN/485介绍以及资料整理

    目录 1 前言2 产品介绍3 产品快速测试3 1 产品规格书及使用说明书3 2 通用指令串口助手使用说明3 3 产品快速测试说明 4 基于开源硬件的运用整理4 1 在开源飞控上的运用 5 基于其他的运用整理5 1 在PLC上的运用说明5 2

  • 【ESP32 DEVKIT_V1】基于Arduino IDE环境搭建

    目录 一 前言二 板子介绍三 环境搭建1 Arduino IDE的安装2 在Arduino IDE上添加外包链接3 添加好外包链接后就可以下载对应的板子库文件 测试1 先把开发板接到电脑 xff0c 并在Arduino IDE上选择对应的开

  • 【ESP32 DEVKIT_V1】北醒TF系列雷达在ESP32 DEVKIT_V1开发板上的运用

    目录 前言一 硬件准备二 硬件接线说明串口接线示意图 xff1a I2C接先示意图 三 软件搭建及测试1 使用Arduino IDE编译教程2 使用vsCode 43 Arduino教程2 1 在vsCode上使用Arduino的环境搭建2

  • 【vsCode + Arduino】在Visual Studio Code编译Arduino项目

    目录 前言一 参考文档二 操作步骤2 1 安装Arduino IDE2 2 在vsCode里安装Arduino插件2 3 配置arduino的安装路径2 4 配置好后打开一个Arduino的项目文件夹进行相应的配置 三 目前已知问题 前言

  • 蓝牙：GATT，属性，特性，服务

    接着上一篇 通用属性配置文件 xff08 Generic Attribute Profile xff09 1 GATT简介 通用属性配置文件Generic Attribute Profile简称GATT GATT定义了属性类型并规定了如何使

  • RS232 RS422 RS485详细介绍

    1 RS 232 C RS 232 C是美国电子工业协会EIA xff08 Electronic Industry Association xff09 制定的一种串行物理接口标准 RS是英文 推荐标准 的缩写 xff0c 232为标识号 x

  • stm32串口使用以及串口中断接收不定长度字符串

    开始使用cubemx配置PA9 PA10分别为TX RX端 xff0c 在使能串口中断 之后其余值直接使用默认的就可以了 点击生成代码即可 span class token class name uint8 t span rx buff s

  • STM32-串口通信printf重定向

    前言 xff1a 平时我们进行c语言编程的时候会经常用到printf函数进行打印输出 xff0c 来调试代码 可是这个printf函数C库已经帮我们实现好了 xff0c 通常只需要直接调用即可 xff0c 但是如果在一个新的开发平台 xff

  • FMCW毫米波雷达原理

    Radar系列文章 传感器融合是将多个传感器采集的数据进行融合处理 xff0c 以更好感知周围环境 xff1b 这里首先介绍毫米波雷达的相关内容 xff0c 包括毫米波雷达基本介绍 xff0c 毫米波雷达数据处理方法 xff08 测距测速测

随机推荐

热门标签