Ardupilot之传感器mpu6000流程学习（mpu6000）

目录

摘要：本文档主要记录是实现基于ardupilot飞控代码，学习传感器数据获取的文档，欢迎批评指正

1.原理图学习

---

2.程序流程

---

**跟着思路往下走，主要围绕这几点学习？
（1）mpu6000传感器怎么实现初始化？
（2）mpu6000传感器数据怎么更新？
（3）mpu6000传感器数据怎么被调用？

---

（1）mpu6000传感器怎么实现初始化？

进入init_ardupilot()函数


这里重点是看ins.init(scheduler.get_loop_rate_hz())函数

**然后是重点到
void AP_InertialSensor::detect_backends(void)函数
backend[i]->start()函数**
其中void AP_InertialSensor::detect_backe**重点内容**nds(void)函数主要用来区分选择哪个mpu,比如你可以选择mpu6000,mpu6500,icm20602之类


----------


接着看重点函数
/****************************************************************************************************************
*函数原型：void AP_InertialSensor_Invensense::start()
*函数功能：开始数据读取
*修改日期：2018-5-26
*备   注：
*****************************************************************************************************************/
void AP_InertialSensor_Invensense::start()
{
    printf("CCC\r\n");
    if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER))
    {
        return;
    }

    // initially run the bus at low speed
    _dev->set_speed(AP_HAL::Device::SPEED_LOW);

    // only used for wake-up in accelerometer only low power mode
    _register_write(MPUREG_PWR_MGMT_2, 0x00); //允许用户配置加速度计仅在低功耗模式下的唤醒频率
    hal.scheduler->delay(1);                  //延迟1ms

    // always use FIFO
    _fifo_reset();

    // grab the used instances
    enum DevTypes gdev, adev;
    switch (_mpu_type)
    {
    case Invensense_MPU9250:
        gdev = DEVTYPE_GYR_MPU9250;
        adev = DEVTYPE_ACC_MPU9250;
        break;
    case Invensense_MPU6000:
    case Invensense_MPU6500:
    case Invensense_ICM20608:
    case Invensense_ICM20602:
    default:
        gdev = DEVTYPE_GYR_MPU6000;
        adev = DEVTYPE_ACC_MPU6000;
        break;
    }

    /*
      setup temperature sensitivity and offset. This varies
      considerably between parts
     */
    switch (_mpu_type)
    {
    case Invensense_MPU9250:
        temp_zero = 21;
        temp_sensitivity = 1.0/340;
        break;

    case Invensense_MPU6000:
    case Invensense_MPU6500:
        temp_zero = 36.53;
        temp_sensitivity = 1.0/340;
        break;

    case Invensense_ICM20608:
    case Invensense_ICM20602:
        temp_zero = 25;
        temp_sensitivity = 1.0/326.8; 
        break;
    }

    _gyro_instance = _imu.register_gyro(1000, _dev->get_bus_id_devtype(gdev));
    _accel_instance = _imu.register_accel(1000, _dev->get_bus_id_devtype(adev));

    // setup ODR and on-sensor filtering
    _set_filter_register();

    // set sample rate to 1000Hz and apply a software filter
    // In this configuration, the gyro sample rate is 8kHz
    _register_write(MPUREG_SMPLRT_DIV, 0, true);
    hal.scheduler->delay(1);

    // Gyro scale 2000º/s
    _register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS, true);
    hal.scheduler->delay(1);

    // read the product ID rev c has 1/2 the sensitivity of rev d
    uint8_t product_id = _register_read(MPUREG_PRODUCT_ID);

    if (_mpu_type == Invensense_MPU6000 &&
        ((product_id == MPU6000ES_REV_C4) ||
         (product_id == MPU6000ES_REV_C5) ||
         (product_id == MPU6000_REV_C4)   ||
         (product_id == MPU6000_REV_C5))) {
        // Accel scale 8g (4096 LSB/g)
        // Rev C has different scaling than rev D
        _register_write(MPUREG_ACCEL_CONFIG,1<<3, true);
        _accel_scale = GRAVITY_MSS / 4096.f;
    } else {
        // Accel scale 16g (2048 LSB/g)
        _register_write(MPUREG_ACCEL_CONFIG,3<<3, true);
        _accel_scale = GRAVITY_MSS / 2048.f;
    }
    hal.scheduler->delay(1);

    if (_mpu_type == Invensense_ICM20608 ||
        _mpu_type == Invensense_ICM20602)
    {
        // this avoids a sensor bug, see description above
        _register_write(MPUREG_ICM_UNDOC1, MPUREG_ICM_UNDOC1_VALUE, true);
    }

    // configure interrupt to fire when new data arrives
    _register_write(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN);
    hal.scheduler->delay(1);

    // clear interrupt on any read, and hold the data ready pin high
    // until we clear the interrupt
    _register_write(MPUREG_INT_PIN_CFG, _register_read(MPUREG_INT_PIN_CFG) | BIT_INT_RD_CLEAR | BIT_LATCH_INT_EN);

    // now that we have initialised, we set the bus speed to high
    _dev->set_speed(AP_HAL::Device::SPEED_HIGH);

    _dev->get_semaphore()->give();

    // setup sensor rotations from probe()
    set_gyro_orientation(_gyro_instance, _rotation);
    set_accel_orientation(_accel_instance, _rotation);

    // allocate fifo buffer
    _fifo_buffer = (uint8_t *)hal.util->dma_allocate(MPU_FIFO_BUFFER_LEN * MPU_SAMPLE_SIZE);
    if (_fifo_buffer == nullptr) {
        AP_HAL::panic("Invensense: Unable to allocate FIFO buffer");
    }
    printf("DDD\r\n");
/////////////////////////////////////////////////////////////////////////////////////////////////////////
    // 启动1ms定时器采样数据更新-------start the timer process to read samples
    _dev->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_Invensense::_poll_data, void));
/////////////////////////////////////////////////////////////////////////////////////////////////////////
}

代码之间的跳转为什么到这里，后面会用visio图说明

1ms的定时器回调函数

进入函数内部

---

/****************************************************************************************************************
*函数原型：void AP_InertialSensor_Invensense::_read_fifo()
*函数功能：imu数据读取
*修改日期：2018-5-26
*备   注：publish any pending data
*****************************************************************************************************************/
void AP_InertialSensor_Invensense::_read_fifo()
{
    uint8_t n_samples;
    uint16_t bytes_read;
    uint8_t *rx = _fifo_buffer;
    bool need_reset = false;

    if (!_block_read(MPUREG_FIFO_COUNTH, rx, 2))
    {
        goto check_registers;
    }

    bytes_read = uint16_val(rx, 0);
    n_samples = bytes_read / MPU_SAMPLE_SIZE;

    if (n_samples == 0) {
        /* Not enough data in FIFO */
        goto check_registers;
    }

    /*
      testing has shown that if we have more than 32 samples in the
      FIFO then some of those samples will be corrupt. It always is
      the ones at the end of the FIFO, so clear those with a reset
      once we've read the first 24. Reading 24 gives us the normal
      number of samples for fast sampling at 400Hz
     */
    if (n_samples > 32) {
        need_reset = true;
        n_samples = 24;
    }

    while (n_samples > 0)
    {
//      printf("FFF\r\n");
        uint8_t n = MIN(n_samples, MPU_FIFO_BUFFER_LEN);
        if (!_dev->set_chip_select(true)) {
            if (!_block_read(MPUREG_FIFO_R_W, rx, n * MPU_SAMPLE_SIZE)) {
                goto check_registers;
            }
        } else {
            // this ensures we keep things nicely setup for DMA
            uint8_t reg = MPUREG_FIFO_R_W | 0x80;
            if (!_dev->transfer(&reg, 1, nullptr, 0)) {
                _dev->set_chip_select(false);
                goto check_registers;
            }
            memset(rx, 0, n * MPU_SAMPLE_SIZE);
            if (!_dev->transfer(rx, n * MPU_SAMPLE_SIZE, rx, n * MPU_SAMPLE_SIZE)) {
                hal.console->printf("MPU60x0: error in fifo read %u bytes\n", n * MPU_SAMPLE_SIZE);
                _dev->set_chip_select(false);
                goto check_registers;
            }
            _dev->set_chip_select(false);
        }
//        printf("_fast_sampling=%d\r\n",_fast_sampling);//_fast_sampling=0
        if (_fast_sampling)
        {
            if (!_accumulate_fast_sampling(rx, n))
            {
                debug("stop at %u of %u", n_samples, bytes_read/MPU_SAMPLE_SIZE);
                break;
            }
        } else
        {
            if (!_accumulate(rx, n)) //计算传感器数据
            {
                break;
            }
        }
        n_samples -= n;
    }

    if (need_reset)
    {
        //debug("fifo reset n_samples %u", bytes_read/MPU_SAMPLE_SIZE);
        _fifo_reset();
    }

check_registers:
    // check next register value for correctness
    _dev->set_speed(AP_HAL::Device::SPEED_LOW);
    if (!_dev->check_next_register()) {
        _inc_gyro_error_count(_gyro_instance);
        _inc_accel_error_count(_accel_instance);
    }
    _dev->set_speed(AP_HAL::Device::SPEED_HIGH);
}

我们重点看这个函数

/****************************************************************************************************************
*函数原型：bool AP_InertialSensor_Invensense::_accumulate(uint8_t *samples, uint8_t n_samples)
*函数功能：imu数据更新---计算加速度数据
*修改日期：2018-5-26
*备   注： 默认情况下： int16_val(data, 0)--------X--------(0)
*                    int16_val(data, 1)-------Y-------(0)
*                    int16_val(data, 2)-------Z-------(-2048)
*****************************************************************************************************************/
int16_t  my_data[8]={0};
bool AP_InertialSensor_Invensense::_accumulate(uint8_t *samples, uint8_t n_samples)
{
//  printf("NNN\r\n");
    for (uint8_t i = 0; i < n_samples; i++)
    {
        const uint8_t *data = samples + MPU_SAMPLE_SIZE * i;
        Vector3f accel, gyro;
        bool fsync_set = false;

#if INVENSENSE_EXT_SYNC_ENABLE
        fsync_set = (int16_val(data, 2) & 1U) != 0;
#endif

        accel = Vector3f(int16_val(data, 1),
                         int16_val(data, 0),
                         -int16_val(data, 2));

//        my_data[0]=int16_val(data, 1);
//        my_data[1]=int16_val(data, 0);
//        my_data[2]=int16_val(data, 2);
//        my_data[7]=-20;
//        printf("**********************************************\r\n");
//        printf("my_data[0]=%d\r\n",my_data[0]);
//        printf("my_data[1]=%d\r\n",my_data[1]);
//        printf("my_data[2]=%d\r\n",my_data[2]);
//        printf("my_data[7]=%d\r\n",my_data[7]);
//        printf("**********************************************\r\n");
        accel *= _accel_scale;

        int16_t t2 = int16_val(data, 3);
        if (!_check_raw_temp(t2))
        {
            debug("temp reset %d %d", _raw_temp, t2);
            _fifo_reset();
            return false;
        }
        float temp = t2 * temp_sensitivity + temp_zero;

        gyro = Vector3f(int16_val(data, 5),
                        int16_val(data, 4),
                        -int16_val(data, 6));
        my_data[3]=int16_val(data, 5);
        my_data[4]=int16_val(data, 4);
        my_data[5]=int16_val(data, 6);
//        printf("**********************************************\r\n");
//        printf("my_data[3]=%d\r\n",my_data[3]);
//        printf("my_data[4]=%d\r\n",my_data[4]);
//        printf("my_data[5]=%d\r\n",my_data[5]);
//        printf("**********************************************\r\n");
        gyro *= GYRO_SCALE; //量程转换，变成弧度

         //旋转更正加速度数据，陀螺仪数据
//        printf("_accel_instance=%d\r\n",_accel_instance); //_accel_instance=0;
//        printf("_gyro_instance=%d\r\n",_gyro_instance);   //_gyro_instance=0;

        _rotate_and_correct_accel(_accel_instance, accel); //把加速度数据转换成重力加速度数据，坐标系是NED
        _rotate_and_correct_gyro(_gyro_instance, gyro);    //获取陀螺仪旋转后数据

        _notify_new_accel_raw_sample(_accel_instance, accel, 0, fsync_set); //发布数据
        _notify_new_gyro_raw_sample(_gyro_instance, gyro);                  //发布数据

        _temp_filtered = _temp_filter.apply(temp);
    }

    return true;
}

---

按照这个图Z轴向上放置，我们的pixhawk飞控你会看到，Z轴是正值（int16_val(data, 2),=2048），当飞控反过来数据就，当你按照pixhawk的放置读取的int16_val(data, 2)=-2048；并且你会看到程序读取把X与Y进行了对调，并且Z轴加上了负号，为啥？？？？？这里就是转换成北东地坐标系的，所以后面还有很多需要说明，看下面那个坐标系，你按照轴的思想验证X与Y你会发现，到这里并没有转换成北东地坐标系，为什么？？？自己用串口打印出来就知道，从后面的代码也可以看出来，不信就往下看代码

---

**到这里我们已经得到了原始的加速度和陀螺仪数据
accel和gyro,注意这里可以表示成
accel.x,accel.y,accel.z,
gyro.x,gyro.y,gyro.z,
下一步我们关心的就是旋转和滤波处理；**



这里就说明上面说的为啥没有转换成北东地坐标系，从代码我们可以看到？
accel.rotate(_imu._accel_orientation[instance]);//这个函数进行了坐标旋转，_imu._accel_orientation[instance]=8，我们进入函数看下
template void Vector3::rotate(enum Rotation);

---

到这里一切还没有结束，不过确实已经把我们的机体坐标系转换成了北东地；还有一种情况要考虑，假如你的飞控板不是跟pixhawk一样放置，怎么办？看这个函数，有个板层选择函数 accel.rotate(_imu._board_orientation);我们能想到的，巨人们总是领先了我们。只能感叹！！！！！，陀螺仪我这里不想多说，需要注意的就是，无论你怎么旋转，一定要保证沿着哪个轴，逆时针旋转一定读取的是正值，这里我折腾过很久，才搞明白，以后无论移植哪款飞控，这里都需要注意。更要注意数据手册中的一些细节！！！

---

（2）mpu6000传感器数据怎么更新？
**_notify_new_accel_raw_sample(_accel_instance, accel, 0, fsync_set);
_notify_new_gyro_raw_sample(_gyro_instance, gyro); 布**

**下面主要分析加速度数据，陀螺仪不在分析**
/****************************************************************************************************************
*函数原型：void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
                                                             const Vector3f &accel,
                                                             uint64_t sample_us,
                                                             bool fsync_set)
*函数功能：通知数据到前端
*修改日期：2018-5-30
*备   注：
*****************************************************************************************************************/
void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
                                                             const Vector3f &accel,
                                                             uint64_t sample_us,
                                                             bool fsync_set)
{
    float dt;

    _update_sensor_rate(_imu._sample_accel_count[instance], _imu._sample_accel_start_us[instance],
                        _imu._accel_raw_sample_rates[instance]);

    /*
      we have two classes of sensors. FIFO based sensors produce data
      at a very predictable overall rate, but the data comes in
      bunches, so we use the provided sample rate for deltaT. Non-FIFO
      sensors don't bunch up samples, but also tend to vary in actual
      rate, so we use the provided sample_us to get the deltaT. The
      difference between the two is whether sample_us is provided.
     */
    if (sample_us != 0 && _imu._accel_last_sample_us[instance] != 0)
    {
        dt = (sample_us - _imu._accel_last_sample_us[instance]) * 1.0e-6;
    } else
    {
        // don't accept below 100Hz
        if (_imu._accel_raw_sample_rates[instance] < 100)
        {
            return;
        }

        dt = 1.0f / _imu._accel_raw_sample_rates[instance];
    }
    _imu._accel_last_sample_us[instance] = sample_us;

    // call accel_sample hook if any
    AP_Module::call_hook_accel_sample(instance, dt, accel, fsync_set);

    _imu.calc_vibration_and_clipping(instance, accel, dt);

    if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER))
    {
        // delta velocity
        _imu._delta_velocity_acc[instance] += accel * dt;
        _imu._delta_velocity_acc_dt[instance] += dt;

        _imu._accel_filtered[instance] = _imu._accel_filter[instance].apply(accel);
//
//        printf("=============================================\r\n");
//                printf("_imu._accel_filtered[0].x=%6.3f\r\n",_imu._accel_filtered[0].x);
//                printf("_imu._accel_filtered[0].y=%6.3f\r\n",_imu._accel_filtered[0].y);
//                printf("_imu._accel_filtered[0].z=%6.3f\r\n",_imu._accel_filtered[0].z);
//        printf("=============================================\r\n");
        if (_imu._accel_filtered[instance].is_nan() || _imu._accel_filtered[instance].is_inf())
        {
            _imu._accel_filter[instance].reset();
        }

        _imu.set_accel_peak_hold(instance, _imu._accel_filtered[instance]); //得到滤波后的数据

        _imu._new_accel_data[instance] = true;
        _sem->give();  //信号发布出去
    }

    DataFlash_Class *dataflash = get_dataflash();
    if (dataflash != nullptr)
    {
        uint64_t now = AP_HAL::micros64();
        struct log_ACCEL pkt =
        {
            LOG_PACKET_HEADER_INIT((uint8_t)(LOG_ACC1_MSG+instance)),
            time_us   : now,
            sample_us : sample_us?sample_us:now,
            AccX      : accel.x,
            AccY      : accel.y,
            AccZ      : accel.z
        };
        dataflash->WriteBlock(&pkt, sizeof(pkt));
    }
}

到这里我们的传感器初始化任务已经完成，就是不停的获取原始数据，进行滤波处理，最后把数据发送到数据更新任务，周期是1ms

---

**_imu._accel_filtered[0].x
_imu._accel_filtered[0].y
_imu._accel_filtered[0].z**

---

(3)mpu6000传感器数据怎么被调用？

下面两个箭头说明下：第一个箭头说的更新是指姿态更新，传感器数据更新就在这里面

我们重点看下下面两个函数：
第一个重要传感器更新函数：

这里我们得到要发布的更新数据，

---

看下面的关系
AP_InertialSensor &_imu; //引用到前端数据
Vector3f _accel[INS_MAX_INSTANCES];

---

_imu.accel[instance]就是最后得到的数据，这个数据将会被用在下面的姿态结算，ekf处理，*到这里后面就是要被调用的数据；怎么传递过去的，这里其实还是要的耐心的分析*

先说下姿态更新用的数据是下面两个，怎么跟上面的数据对应在一起？
get_accel(uint8_t i)
get_gyro(i)

---

const Vector3f &get_gyro(uint8_t i) const { return _gyro[i]; }
const Vector3f &get_accel(uint8_t i) const { return _accel[i]; }

---

**这里就用到了C++中的类引用，仔细分析下就可以得出下面的关系：
get_accel()可以获取_imu.accel
get_gyro()可以获取_imu.gyro**

到这里数据传递基本结束，有一些细节写错的，后面再修改！！！

第二个重要void Copter::read_AHRS(void)函数主要是姿态解算部分，现在先不分析，后面继续

3.visio流程图