1

我现在正在为我的硕士论文使用 mpu9250，但我对硬件设备还是很陌生。我的目标是以采样率收集数据并通过艾伦偏差对这些数据进行后处理。所以我只想从 512 字节的 fifo 中获取数据，但是当我试图读出数据时，它总是会溢出，这让我很烦恼。

我知道方法是将读取速度设置为快于fifo写入速度，所以我删除了循环中的时间睡眠，但它似乎比以前更快溢出。我有点困惑，因为读取速度取决于写入速度。

#!/usr/bin/env python3

import os
import time
import sys
import struct
import csv
from datetime import datetime
from mpu9250_FIFO import MPU9250
from navio.leds import Led


def main():

    # Initialize MPU9250
    mpu = MPU9250()

    mpu.bus_open(max_speed_hz=10000000)  # Hz

    sampleRate = 50
    sampleRateDiv = int(1000 / sampleRate - 1)
    mpu.initialize(sample_rate_div=sampleRateDiv, low_pass_filter_gt=0x01, low_pass_filter_a=0x01)

    mpu.set_acc_scale(0x08)       # +/-4G
    mpu.set_gyro_scale(0x08)      # +/-500dps

    # Enable FIFO to collect data
    mpu.enableFIFO(True)


    # Test connection:
    if mpu.testConnection():
        print("Connection working.")
    else:
        print("Connection to one of the sensors is faulty.")


    now = datetime.now().strftime('%d-%m-%Y_%H:%M:%S')
    fname = '/home/pi/Python/meas_data/mpu9250_data_' + now + r'.txt'

    with open(fname, 'w', 1) as data_imu:
        # write headers to file
        data_imu.write('Sample Rate: %d Hz\n' % sampleRate)
        data_imu.write('mpu_accel_1, mpu_accel_2, mpu_accel_3, mpu_gyro_1, mpu_gyro_2, mpu_gyro_3, temp\n')
        #print("----1----")
        # Main loop
        while True:
            mpudata_a, mpudata_g, mpudata_temp = mpu.getFIFOData()
            #print("----2----")
            data = mpudata_a + mpudata_g + mpudata_temp
            # print(data)
            data_imu.write(str(data).replace("[", "").replace("]", "") + "\n")


#                               FIFO READ AND WRITE
#   FIFO_R_W REGISTER:
#   This register is used to read and write data from the FIFO buffer. Data is written to the FIFO
#   in order of register number (from lowest to highest). If all the FIFO enable flags are enabled
#   and all External Sensor Data registers (Registers 73 to 96) are associated with a Slave device,
#   the contents of registers 59 through 96 will be written in order that the Sample Rate.

#   The contents of the sensor data registers (Registers 59 to 96) are written into the FIFO buffer
#   when their corresponding FIFO enable flags are set to 1 in FIFO_EN (Register 35). An additional
#   flag for the sensor data registers associated with I2C Slave 3 can be found in I2C_MST_CTRL
#   （Register 46).

#   If the FIFO buffer has overflowed, the status bit FIFO_OFLOW_INT is automatically set to 1.
#   This bit is located in INT_STATUS (Register 58). When the FIFO buffer has overflowed, the
#   oldest data will be lost and new data will be written to the FIFO.

#   If the FIFO buffer is empty, reading this register will return the last byte that was previously
#   read from the FIFO until new data is available. The user should check FIFO_COUNT to ensure that
#   the FIFO is not read when empty.
# -----------------------------------------------------------------------------------------------

    def getFIFOData(self):
        while True:
            mpu_int_status = self.getIntStatus()
            INT_FIFO_OFLOW_BIT = 0x10  # the 4th bit is set to 1 when FIFO buffer is overflowed    
            fifoCount = self.getFIFOCount()
             #print("fifoCount:%d" % fifoCount)

            if fifoCount < 14:
                continue
            # check for overflow (this shouldn't happen)
            elif (mpu_int_status & 0x10 == 0x10) or fifoCount>=512:
                print("fifoCount is: %d" % fifoCount)
                print("FIFO is overflow!")
                # print("Reset FIFO...")
               # self.resetFIFO()
                break

            else:
                packet_count = int(fifoCount/14)
            #    print("packet count:%d" % packet_count)
                for i in range(0, packet_count):
                    response = self.ReadRegs(self.__MPUREG_FIFO_R_W, 14)
                    # print(response)
                    # Get Accelerometer values
                    for i in range(0, 3):
                        data = self.byte_to_float(response[i * 2:i * 2 + 2])
                        self.accelerometer_data[i] = self.G_SI * data / self.acc_divider
                   # print(self.accelerometer_data)
                    # Get temperature
                    i = 3
                    temp = self.byte_to_float(response[i * 2:i * 2 + 2])
                    self.temperature[i - 3] = (temp / 333.87) + 21.0
                    # print(self.temperature)
                    # Get gyroscope values
                    for i in range(4, 7):
                        data = self.byte_to_float(response[i * 2:i * 2 + 2])
                        self.gyroscope_data[i - 4] = (self.PI / 180) * data / self.gyro_divider
                   # print(self.gyroscope_data)

                return self.accelerometer_data, self.gyroscope_data, self.temperature

    def getIntStatus(self):
        return self.ReadReg(self.__MPUREG_INT_STATUS)

    def getFIFOCount(self):
        response = self.ReadRegs(self.__MPUREG_FIFO_COUNTH, 2)
        fifoCount = self.byte_to_float(response)
        return fifoCount

    def resetFIFO(self):
        self.WriteReg(self.__MPUREG_USER_CTRL, 0x00)
        self.WriteReg(self.__MPUREG_USER_CTRL, 0x04)    # reset fifo mode
        self.WriteReg(self.__MPUREG_USER_CTRL, 0x40)    # FIFO_EN
        print('fifo is reset!')

    def enableFIFO(self, flag):
        self.WriteReg(self.__MPUREG_FIFO_EN, 0x00)
        if flag:
            self.resetFIFO()
            self.WriteReg(self.__MPUREG_FIFO_EN, 0xF8)
        print('fifo is enabled!')

我希望获得一个恒定的 fifo 读数而不会溢出。

4

0 回答 0