mc_att_control源码解析

目录

之前写了博客分析了一下旋翼姿态控制的基础知识:mc_att_control基础知识,这次就对照代码将整个旋翼姿态控制过程呈现一遍.先看一下整个程序的框图:

从图中可以看到,实际上整个控制分成内外两个环进行控制,外环的输入是位置控制产生的姿态角设定值 ,输出的是角速度设定值,内环根据输入的角速度设定值和陀螺仪测得的角速度值进行控制,产生最终给混控器的量.这个串级PID的控制流程如下图:
\

---

源码分析

我们直接进入任务主进程函数task_mian(),首先是数据订阅:

/*
     * do subscriptions
     */
    _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));//飞机姿态
    _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));//姿态设定值
    _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
    _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));
    _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
    _motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits));//电机转子的限制(判断是否饱和)
    _battery_status_sub = orb_subscribe(ORB_ID(battery_status));

    _gyro_count = math::min(orb_group_count(ORB_ID(sensor_gyro)), MAX_GYRO_COUNT);//需要订阅最多三组陀螺仪的数据
    //orb_group_count():获取主题组的已发布实例的数量

这里需要注意的是订阅了三组的陀螺仪的数据,然后选择偏差最小的陀螺仪.代码如下:

for (unsigned s = 0; s < _gyro_count; s++) {
        _sensor_gyro_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
    }

    _sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));
    _sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias));

    /* initialize parameters cache */
    parameters_update();

    /* wakeup source: gyro data from sensor selected by the sensor app *///所选择的陀螺仪数据变化,则唤醒进程
    //陀螺仪的选择在sensor_correction.msg中有定义,选择误差小的陀螺仪数据

我们看一下sensor_correction.msg:

uint8 selected_gyro_instance    # gyro uORB topic instance for the voted sensor 选择的陀螺仪
uint8 selected_accel_instance   # accelerometer uORB topic instance for the voted sensor
uint8 selected_baro_instance    # barometric pressure uORB topic instance for the voted sensor

后面就是轮询,开始我们的控制任务,这里我们主要分析外环和内环控制实现的两个函数:

1. control_attitude(dt):先得到各轴的角度偏差e_R,将angle error经过PF控制器得到_rates_sp(角速度设定值.

2. control_attitude_rates(dt):通过选择数据最好的陀螺仪得到当前角速度信息,再和_rates_sp比较,通过PID+前馈控制,得到_att_control:即给混控器的四个u1,u2,u3,u4

   内环的控制参考论文:High Performance Full Attitude Control of a Quadrotor on SO(3)
   其主要思想是将整个旋转分成两部分:

   Re=Rtorsion