ADIS16405与stm32通信

# ADIS16465 IMU Driver in Python
# SPI Master is using Raspberry Pi
# spidev is the Raspberry Pi spi communication library
# Date: 2019.4.29

import sys
import spidev
import time

class ADIS16465():
   
    #Configuration Constant based on ADIS16475 datasheet
   
    DIAG_STS        =0x02
    X_GYRO_LOW      =0x04
    X_GYRO_OUT      =0x06
    Y_GYRO_LOW      =0x08
    Y_GYRO_OUT      =0x0A
    Z_GYRO_LOW      =0x0C
    Z_GYRO_OUT      =0x0E
    X_ACCL_LOW      =0x10
    X_ACCL_OUT      =0x12
    Y_ACCL_LOW      =0x14
    Y_ACCL_OUT      =0x16
    Z_ACCL_LOW      =0x18
    Z_ACCL_OUT      =0x1A
    TEMP_OUT        =0x1C
    DATA_CNTR       =0x22
    X_DELTANG_LOW   =0x24  
    X_DELTANG_OUT   =0x26
    Y_DELTANG_LOW   =0x28  
    Y_DELTANG_OUT   =0x2A
    Z_DELTANG_LOW   =0x2C  
    Z_DELTANG_OUT   =0x2E
       
    X_DELTVEL_LOW   =0x30  
    X_DELTVEL_OUT   =0x32     
    Y_DELTVEL_LOW   =0x34  
    Y_DELTVEL_OUT   =0x36     
    Z_DELTVEL_LOW   =0x38  
    Z_DELTVEL_OUT   =0x3A

    XG_BIAS_LOW     =0x40
    XG_BIAS_HIGH    =0x42
    YG_BIAS_LOW     =0x44
    YG_BIAS_HIGH    =0x46
    ZG_BIAS_LOW     =0x48
    ZG_BIAS_HIGH    =0x4A   
    XA_BIAS_LOW     =0x4C
    XA_BIAS_HIGH    =0x4E
    YA_BIAS_LOW     =0x50
    YA_BIAS_HIGH    =0x52
    ZA_BIAS_LOW     =0x54
    ZA_BIAS_HIGH    =0x56

    FILT_CTRL       =0x5C
    RANG_MDL        =0x5E
    MSC_CTRL        =0x60
    UP_SCALE        =0x62
    DEC_RATE        =0x64
    NULL_CFG        =0x66
    GLOB_CMD        =0x68
    FIRM_REV        =0x6C
    FIRM_DM         =0x6E
    FIRM_Y          =0x70
    PROD_ID         =0X72
    SERIAL_NUM      =0x74
    USER_SCR_1      =0x76
    USER_SCR_2      =0x78
    USER_SCR_3      =0x7A
    FLSHCNT_LOW     =0x7C
    FLSHCNT_HIGH    =0x7E

    def __init__(self, device=0, ce_pin=0):
        """
        device: the SPI device (often 0)
        ce_pin: pass 0 for CE0, 1 for CE1, etc.
        device and ce_pin map to device file /dev/spidev{device}.{ce_pin}
        """
        self.spi_init(device, ce_pin)

    def spi_init(self,device,ce_pin):
        """
        Using "pinout" command to check the raspberry spi definition.
        device: SPI0 (0), SPI1 (1)
        ce_pin: CE0 (0), CE1 (1)
       
        SPI0 definition
        CE0 :  GPIO8 (0)  PIN24
        CE1 :  GPIO7 (1)  PIN26
        MOSI:  GPIO10     PIN19
        MISO:  GPIO9      PIN21
        SCLK:  GPIO11     PIN23

        SPI1 definition
        CE0 :  GPIO18 (0) PIN12
        CE1 :  GPIO17 (1) PIN11
        CE2 :  GPIO16 (2) PIN36
        MOSI:  GPIO20     PIN38
        MISO:  GPIO19     PIN35
        SCLK:  GPIO21     PIN40  
        """
        # init spi for communication
        self.spi = spidev.SpiDev()
        self.spi.open(device, ce_pin) # (x,0) == CE0, (x,1) == CE1

        # Set 4-line spi mode
        self.spi.threewire   =False

        # Set spi msb first
        self.spi.lsbfirst    =False

        # Set clock phase and polarity to default
        # SPI_CPOL=0, SPI_CPHA=0
        self.spi.mode       =3

        # Set SPI CS Active Low
        self.spi.cshigh     =False

        # Set SPI clock rate: 7.8125MHz, cdiv=32, fsclk=250MHz/cdiv, cdiv = [2,4,8,16,32,...32768]
        # self.spi.max_speed_hz=7812500
        # self.spi.max_speed_hz=1953125
        self.spi.max_speed_hz=976562
 
       
   
    def spi_write(self, address, value):
        # Write value (1 byte) to address
        # Send instruction (command bit7=1 for write), address[6:0], and value
        address=address+0x80
        self.spi.xfer([address, value])
       
    def spi_read(self, address):
        # Read
        # Send instruction (command bit7=0 for read), address bit6:0
        #rb=self.spi.xfer([address,0x00,0x00,0x00])check_spi_rd
        #return hex( rb[0]*256 + rb[1] )
        rb=self.spi.xfer([address,0x00])
        return  rb[0]*256 + rb[1]
    
       

    def twos_comp(self,val, bits):
        # two's complement of value in bits
        if val&(1<<(bits-1)) != 0:
            val = val - (1<<bits)
        return val

    def spi_write_word(self,address,value_16b):
        value_l=value_16b & 0xFF
        value_h=value_16b>>8
        self.spi_write(address,value_l)
        self.spi_write(address+1,value_h)

    def check_spi_rd(self):
        #Readback Device ID to check the SPI Read
        self.get_prod_id()
        rb=self.get_prod_id()
        print ("--------------")
        print ("product_id readback:%s"%(rb))
        if (rb == 16465) :
            print ("spi Read OK, Find DUT=ADIS16465")
        else:
            print ("spi Read Failure, Please check SPI timing and hardware connections")

    def software_reset(self):
        self.spi_read(self.GLOB_CMD)
        rb=self.spi_read(self.GLOB_CMD)
        rb=rb | 0x80
        self.spi_write_word(self.GLOB_CMD,rb)
        #wait 74ms
        time.sleep(0.5)

    def get_diag_sts(self):
        self.spi_read(self.DIAG_STS)
        rb=self.spi_read(self.DIAG_STS)
        return hex(rb)

    def get_gyro_out_once(self):
        # The first readback should be discarded
        temp      =self.spi_read(self.X_GYRO_OUT)
        x_gyro_out=self.spi_read(self.Y_GYRO_OUT)
        y_gyro_out=self.spi_read(self.Z_GYRO_OUT)
        z_gyro_out=self.spi_read(self.Z_GYRO_OUT)
        return self.twos_comp(x_gyro_out,16),self.twos_comp(y_gyro_out,16),self.twos_comp(z_gyro_out,16)

    def get_accl_out_once(self):
        # The first readback should be discarded
        temp     =self.spi_read(self.X_ACCL_OUT)
        x_accl_out=self.spi_read(self.Y_ACCL_OUT)
        y_accl_out=self.spi_read(self.Z_ACCL_OUT)
        z_accl_out=self.spi_read(self.Z_ACCL_OUT)
        return self.twos_comp(x_accl_out,16),self.twos_comp(y_accl_out,16),self.twos_comp(z_accl_out,16)

    def get_gyro_accl_out_32b_once(self):
        # The first readback should be discarded
        temp0     =self.spi_read(self.X_GYRO_LOW)
        x_gyro_low=self.spi_read(self.X_GYRO_OUT)
        x_gyro_out=self.spi_read(self.Y_GYRO_LOW)
        y_gyro_low=self.spi_read(self.Y_GYRO_OUT)
        y_gyro_out=self.spi_read(self.Z_GYRO_LOW)
        z_gyro_low=self.spi_read(self.Z_GYRO_OUT)
        z_gyro_out=self.spi_read(self.X_ACCL_LOW)
        x_accl_low =self.spi_read(self.X_ACCL_OUT)     
        x_accl_out =self.spi_read(self.Y_ACCL_LOW)
        y_accl_low =self.spi_read(self.Y_ACCL_OUT)
        y_accl_out =self.spi_read(self.Z_ACCL_LOW)
        z_accl_low =self.spi_read(self.Z_ACCL_OUT)
        z_accl_out =self.spi_read(self.Z_ACCL_OUT)
        x_gyro=x_gyro_out*65536+x_gyro_low
        y_gyro=y_gyro_out*65536+y_gyro_low
        z_gyro=z_gyro_out*65536+z_gyro_low
        x_accl=x_accl_out*65536+x_accl_low
        y_accl=y_accl_out*65536+y_accl_low
        z_accl=z_accl_out*65536+z_accl_low
        return self.twos_comp(x_gyro,32),self.twos_comp(y_gyro,32),self.twos_comp(z_gyro,32), \
               self.twos_comp(x_accl,32),self.twos_comp(y_accl,32),self.twos_comp(z_accl,32)

    def get_gyro_accl_out_repeat(self):
        z_accl_out=self.spi_read(self.X_GYRO_OUT)
        x_gyro_out=self.spi_read(self.Y_GYRO_OUT)
        y_gyro_out=self.spi_read(self.Z_GYRO_OUT)
        z_gyro_out=self.spi_read(self.X_ACCL_OUT)
        x_accl_out=self.spi_read(self.Y_ACCL_OUT)
        y_accl_out=self.spi_read(self.Z_ACCL_OUT)
        return self.twos_comp(x_gyro_out,16),self.twos_comp(y_gyro_out,16),self.twos_comp(z_gyro_out,16),\
               self.twos_comp(x_accl_out,16),self.twos_comp(y_accl_out,16),self.twos_comp(z_accl_out,16)

    def get_gyro_accl_out_32b_repeat(self):
        x_gyro_low=self.spi_read(self.X_GYRO_OUT)
        x_gyro_out=self.spi_read(self.Y_GYRO_LOW)
        y_gyro_low=self.spi_read(self.Y_GYRO_OUT)
        y_gyro_out=self.spi_read(self.Z_GYRO_LOW)
        z_gyro_low=self.spi_read(self.Z_GYRO_OUT)
        z_gyro_out=self.spi_read(self.X_ACCL_LOW)
        x_accl_low =self.spi_read(self.X_ACCL_OUT)     
        x_accl_out =self.spi_read(self.Y_ACCL_LOW)
        y_accl_low =self.spi_read(self.Y_ACCL_OUT)
        y_accl_out =self.spi_read(self.Z_ACCL_LOW)
        z_accl_low =self.spi_read(self.Z_ACCL_OUT)
        z_accl_out =self.spi_read(self.X_GYRO_LOW)
        x_gyro=x_gyro_out*65536+x_gyro_low
        y_gyro=y_gyro_out*65536+y_gyro_low
        z_gyro=z_gyro_out*65536+z_gyro_low
        x_accl=x_accl_out*65536+x_accl_low
        y_accl=y_accl_out*65536+y_accl_low
        z_accl=z_accl_out*65536+z_accl_low
        return self.twos_comp(x_gyro,32),self.twos_comp(y_gyro,32),self.twos_comp(z_gyro,32), \
               self.twos_comp(x_accl,32),self.twos_comp(y_accl,32),self.twos_comp(z_accl,32)

    def get_temp_out(self):
        #return unit:C
        self.spi_read(self.TEMP_OUT)
        rb=self.spi_read(self.TEMP_OUT)
        return self.twos_comp(rb,16)*0.00565+25
   

   
    def get_prod_id(self):
        self.spi_read(self.PROD_ID)
        rb=self.spi_read(self.PROD_ID)
        return rb

    def spi_read_burst(self):
        # Read
        # Send instruction (command bit7=0 for read), address bit6:0
        #rb=self.spi.xfer([address,0x00,0x00,0x00])check_spi_rd
        #return hex( rb[0]*256 + rb[1] )
        rb=self.spi.xfer([0x68,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00])
        diag_stat=rb[2]*256+rb[3]
        xgyro_out=rb[4]*256+rb[5]
        ygyro_out=rb[6]*256+rb[7]
        zgyro_out=rb[8]*256+rb[9]
        xaccl_out=rb[10]*256+rb[11]
        yaccl_out=rb[12]*256+rb[13]
        zaccl_out=rb[14]*256+rb[15]
        temp_out =rb[16]*256+rb[17]
        data_cntr=rb[18]*256+rb[19]
        check_sum=rb[20]*256+rb[21]
        print ("diag_stat:%s"%(diag_stat))
        print ("xgyro_out:%s"%(xgyro_out))
        print ("ygyro_out:%s"%(ygyro_out))
        print ("zgyro_out:%s"%(zgyro_out))
        print ("xaccl_out:%s"%(xaccl_out))
        print ("yaccl_out:%s"%(yaccl_out))
        print ("zaccl_out:%s"%(zaccl_out))
        print ("temp_out:%s"%(temp_out))
        print ("data_cntr:%s"%(data_cntr))
        print ("check_sum:%s"%(check_sum))       
        return  rb  

       

if __name__ == "__main__":
    try:
        if 1:
            time.sleep(1)
    except KeyboardInterrupt:
        spi.close()
        sys.exit(0)