目錄

摘要：本文件主要記錄是實現基於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流程圖