Hi! I’m using a PIC18F2680 instead of arduino with picbasic code.
Gyroscope response (offset subraction): (excellent)
Accelerometer: (fail)
Magnetometer: (excellent)
Here there are my register settings:
'****************************************************************
'*** ALIASES ***
'****************************************************************
' SLAVE addresses
Symbol MEMS_MagnetAccel_R = %00111011 'SA0 High
Symbol MEMS_Gyro_R = %11010111 'SA0 High
Symbol MEMS_MagnetAccel_W = %00111010 'SA0 High
Symbol MEMS_Gyro_W = %11010110 'SA0 High
' Generic symbols
Symbol GX = 0
Symbol GY = 1
Symbol GZ = 2
Symbol WHO_AM_I = $0F
' gyro sub-addresses
Symbol GYRO_CTRL1 = $20
Symbol GYRO_CTRL2 = $21
Symbol GYRO_CTRL3 = $22
Symbol GYRO_CTRL4 = $23
Symbol GYRO_CTRL5 = $24
Symbol GYRO_OUT_X_L = $28
Symbol GYRO_OUT_X_H = $29
Symbol GYRO_OUT_Y_L = $2A
Symbol GYRO_OUT_Y_H = $2B
Symbol GYRO_OUT_Z_L = $2C
Symbol GYRO_OUT_Z_H = $2D
Symbol GYRO_LOW_ODR = $39
' accel and magnetometer sub-addresses
Symbol ACCEL_CTRL0 = $1F
Symbol ACCEL_CTRL1 = $20
Symbol ACCEL_CTRL2 = $21
Symbol ACCEL_CTRL3 = $22
Symbol ACCEL_CTRL4 = $23
Symbol ACCEL_CTRL5 = $24
Symbol ACCEL_CTRL6 = $25
Symbol ACCEL_CTRL7 = $26
Symbol ACCEL_OUT_X_L = $28
Symbol ACCEL_OUT_X_H = $29
Symbol ACCEL_OUT_Y_L = $2A
Symbol ACCEL_OUT_Y_H = $2B
Symbol ACCEL_OUT_Z_L = $2C
Symbol ACCEL_OUT_Z_H = $2D
Symbol MAGN_OUT_X_L = $08
Symbol MAGN_OUT_X_H = $09
Symbol MAGN_OUT_Y_L = $0A
Symbol MAGN_OUT_Y_H = $0B
Symbol MAGN_OUT_Z_L = $0C
Symbol MAGN_OUT_Z_H = $0D
'gyro configuration
Symbol GYRO_SENSITIVITY = 70 '0.07 dps/s/LSB
Symbol GYRO_SENS_DIVIDER = 1 '0.07 dps/s/LSB
Symbol GYRO_2SCOMPLEMENT_MAX = $FF00
Symbol GYRO_CTRL1_VALUE = %00001111 'DR1-DR0 = 00/BW1-BW0 = 00/PD = 1/
'Zen = 1/Yen = 1/Zen = 1 , data rate 100Hz
Symbol GYRO_CTRL2_VALUE = %00100011 'EXTRen = 0/LVRen = 0/HPM1-HPM0 = 00/ 'high pass filter normal
'HPCF3-HPCF0 = 0011 ' 1 Hz High pass filter cut off freq
Symbol GYRO_CTRL3_VALUE = %00000000 'INT1-IG = 0/INT1-BOOT = 0/H_LACTIVE = 0/PP_OD = 1
'INT2_DRDY = 0/INT2_FTH = 0/INT2_ORun = 0/INT2_Empty = 0
Symbol GYRO_CTRL4_VALUE = %00100000 'BDU = 0/BLE = 0/FS1-FS0 = 10/IMPen = 0/ST2-ST1 = 00/ SIM = 0
'2000 dps full scale
Symbol GYRO_CTRL5_VALUE = %10010000 'BOOT = 1/FIFO_EN = 0/StopOnFTH = 0/HPen = 1/IG_Sel1-IGSel0 = 00
'OUTG_Sel1-OUTGSel0 = 00
Symbol GYRO_LOW_ODR_VALUE = %00000000 '-/-/DRDY_HL = 0/-/I2C_dis = 0/SW_RES = 0/LOW_ODR = 0
'accel & magnetometer configuration
Symbol ACCEL_SENSITIVITY = 122 '0.122 mG/LSB
Symbol ACCEL_2SCOMPLEMENT_MAX = $FF00
Symbol MAGN_2SCOMPLEMENT_MAX = $FF00
symbol MAGN_SENSITIVITY = 160 ' 0.160 mgauss / LSB
Symbol ACCEL_CTRL0_VALUE = %00000000 'BOOT = 1/FIFO_EN = 0/FTH_EN = 0/0/0/HP_Click = 0/HPIS1 = 0/HPIS2 = 0
Symbol ACCEL_CTRL1_VALUE = %01100111 'ADDR3-ADDR0 = 0110/DBU = 0/AZEN = 1/AYEN = 1/AXEN = 1
' 100 Hz data rate, all channels elabled
Symbol ACCEL_CTRL2_VALUE = %11001000 'ABW1-ABW0 = 11/AFS2-AFS0 = 001/0/AST = 0/SIM = 0
'50 Hz filter bandwith , 4g sensitivity
Symbol ACCEL_CTRL3_VALUE = %00000000 'INT1_BOOT = 0/INT1_Click = 0/INT1_IG1 = 0/INT1_IG2 = 0/INT1_IGM = 0
'INT1_DRDY_M = 0/INT1_DRDY_M = 0/INT1_EMPTY = 0
Symbol ACCEL_CTRL4_VALUE = %00000000 'All interrupts disabled
Symbol ACCEL_CTRL5_VALUE = %11110100 'TEMP_EN = 1/M_RES1-M_RES0 = 11/M_ODR2-M_ODR0 = 101/ IRQ non latched
'100Hz magnetometer data rate
Symbol ACCEL_CTRL6_VALUE = %00100000 'MFS1-MFS= 10 magnetic full scale set to 4gauss
Symbol ACCEL_CTRL7_VALUE = %10000000 'AHPM1-AHPM0 = 00/AFDS = 0/T_ONLY = 0/MLP = 0/MD1-MD0 = 00
' pin assignements
SYMBOL I2C_DATAPIN = PORTB.1 ' data assigned to PORTB.1
SYMBOL I2C_CLOCKPIN = PORTB.0 ' clock assigned to PORTB.0
'****************************************************************
'*** I/O PORT ASSIGNMENTS ***
'****************************************************************
TRISB.1 = 0
TRISB.0 = 0
This is the part that reads and translates the signals from 2’s complement to fixed point
******************************************************************************
'********** M_READGYRO_RAW -> vector M_Gyro_RAW[WORD]
'********** this subroutine reads the on-board IMU gyroscope
'**********
'******************************************************************************
M_READGYRO_RAW:
I2CREAD I2C_DATAPIN,I2C_CLOCKPIN,MEMS_Gyro_R,(GYRO_OUT_X_L+$80),[STR M_b\6]
for M_s1 = 0 to 4 step 2
M_Gyro_RAW[M_s1/2] = M_b[M_s1] + M_b[M_s1+1] * $FF
next
return
'******************************************************************************
'********** M_READGYRO -> vector M_Gyro[LONG] , angle rate in mdegrees/s
'********** this subroutine reads the on-board IMU gyroscope
'**********
'******************************************************************************
M_READGYRO:
I2CREAD I2C_DATAPIN,I2C_CLOCKPIN,MEMS_Gyro_R,WHO_AM_I,[M_s1]
gosub M_READGYRO_RAW:
for M_s1 = 0 to 2
if M_Gyro_RAW[M_s1] > 32768 then ' sign negative 2's complement value
M_Gyro[M_s1] = (-(GYRO_2SCOMPLEMENT_MAX - M_Gyro_RAW[M_s1]) - M_Gyro_RAW_OFFSET[M_s1]) * GYRO_SENSITIVITY / GYRO_SENS_DIVIDER
else
M_Gyro[M_s1] = (M_Gyro_RAW[M_s1] - M_Gyro_RAW_OFFSET[M_s1]) * GYRO_SENSITIVITY / GYRO_SENS_DIVIDER
endif
next
return
'******************************************************************************
'********** M_CONFIGGYRO
'********** configurates the Gyroscope device
'**********
'******************************************************************************
M_CONFIGGYRO:
I2CWRITE I2C_DATAPIN,I2C_CLOCKPIN,MEMS_Gyro_W,(GYRO_CTRL1),[GYRO_CTRL1_VALUE]
I2CWRITE I2C_DATAPIN,I2C_CLOCKPIN,MEMS_Gyro_W,(GYRO_CTRL2),[GYRO_CTRL2_VALUE]
I2CWRITE I2C_DATAPIN,I2C_CLOCKPIN,MEMS_Gyro_W,(GYRO_CTRL3),[GYRO_CTRL3_VALUE]
I2CWRITE I2C_DATAPIN,I2C_CLOCKPIN,MEMS_Gyro_W,(GYRO_CTRL4),[GYRO_CTRL4_VALUE]
I2CWRITE I2C_DATAPIN,I2C_CLOCKPIN,MEMS_Gyro_W,(GYRO_CTRL5),[GYRO_CTRL5_VALUE]
I2CWRITE I2C_DATAPIN,I2C_CLOCKPIN,MEMS_Gyro_W,(GYRO_LOW_ODR),[GYRO_LOW_ODR_VALUE]
gosub M_READGYRO:
return
'******************************************************************************
'********** M_CONFIGGYRO
'********** calculates the gyroscope offset
'**********
'******************************************************************************
M_CONFIGGYROOFFSET:
' offset calibration
for M_s0 = 0 to 2
M_Gyro_RAW_OFFSET[M_s0] = 0
for M_s2 = 0 to 255
gosub M_READGYRO_RAW:
M_Gyro_RAW_OFFSET[M_s0] = M_Gyro_RAW_OFFSET[M_s0] + M_Gyro_RAW[M_s0]
pause 2
next M_s2
M_Gyro_RAW_OFFSET[M_s0] = M_Gyro_RAW_OFFSET[M_s0] / 256
if M_Gyro_RAW_OFFSET[M_s0] > 32768 then ' sign negative 2's complement value
M_Gyro_RAW_OFFSET[M_s0] = -(GYRO_2SCOMPLEMENT_MAX - M_Gyro_RAW[M_s0])
else
M_Gyro_RAW_OFFSET[M_s0] = (M_Gyro_RAW[M_s0] )
endif
next M_s0
return
'******************************************************************************
'********** M_READACCEL_RAW -> vector M_Accel_RAW[WORD]
'********** this subroutine reads the on-board IMU accelerometer
'**********
'******************************************************************************
M_READACCEL_RAW:
I2CREAD I2C_DATAPIN,I2C_CLOCKPIN,MEMS_Magnetaccel_R,(ACCEL_OUT_X_L+$80),[STR M_b\6]
for M_s1 = 0 to 4 step 2
M_Accel_RAW[M_s1/2] = M_b[M_s1] + M_b[M_s1+1] * $FF
next
return
'******************************************************************************
'********** M_READACCEL -> vector M_Accel[LONG] , acceleration in g
'********** this subroutine reads the on-board IMU accelerometer
'**********
'******************************************************************************
M_READACCEL:
I2CREAD I2C_DATAPIN,I2C_CLOCKPIN,MEMS_MagnetAccel_R,WHO_AM_I,[M_s1]
gosub M_READACCEL_RAW:
for M_s1 = 0 to 2
if M_Accel_RAW[M_s1] > 32768 then ' sign negative 2's complement value
M_Accel[M_s1] = -(ACCEL_2SCOMPLEMENT_MAX - M_Accel_RAW[M_s1]) * ACCEL_SENSITIVITY
else
M_Accel[M_s1] = M_Accel_RAW[M_s1] * ACCEL_SENSITIVITY
endif
next
return
'******************************************************************************
'********** M_READMAGN_RAW -> vector M_Magn_RAW[WORD]
'********** this subroutine reads the on-board IMU accelerometer
'**********
'******************************************************************************
M_READMAGN_RAW:
I2CREAD I2C_DATAPIN,I2C_CLOCKPIN,MEMS_Magnetaccel_R,(MAGN_OUT_X_L+$80),[str M_b\6]
for M_s1 = 0 to 4 step 2
M_MAgn_RAW[M_s1/2] = M_b[M_s1] + M_b[M_s1+1] * $FF
next
return
'******************************************************************************
'********** M_READMAGN -> vector M_Magn[LONG] , magnetic axes in gauss
'********** this subroutine reads the on-board IMU accelerometer
'**********
'******************************************************************************
M_READMAGN:
I2CREAD I2C_DATAPIN,I2C_CLOCKPIN,MEMS_MagnetAccel_R,WHO_AM_I,[M_s1]
gosub M_READMAGN_RAW:
for M_s1 = 0 to 2
if M_Magn_RAW[M_s1] > 32768 then ' sign negative 2's complement value
M_Magn[M_s1] = -(MAGN_2SCOMPLEMENT_MAX - M_MAGN_RAW[M_s1]) * MAGN_SENSITIVITY
else
M_Magn[M_s1] = M_Magn_RAW[M_s1] * MAGN_SENSITIVITY
endif
next
return
'******************************************************************************
'********** M_CONFIGACCELMAGN
'********** configurates the Accelerometer & Magnetometer device
'**********
'******************************************************************************
M_CONFIGACCELMAGN:
I2CWRITE I2C_DATAPIN,I2C_CLOCKPIN,MEMS_MagnetAccel_W,ACCEL_CTRL0,[ACCEL_CTRL0_VALUE]
I2CWRITE I2C_DATAPIN,I2C_CLOCKPIN,MEMS_MagnetAccel_W,ACCEL_CTRL1,[ACCEL_CTRL1_VALUE]
I2CWRITE I2C_DATAPIN,I2C_CLOCKPIN,MEMS_MagnetAccel_W,ACCEL_CTRL2,[ACCEL_CTRL2_VALUE]
I2CWRITE I2C_DATAPIN,I2C_CLOCKPIN,MEMS_MagnetAccel_W,ACCEL_CTRL3,[ACCEL_CTRL3_VALUE]
I2CWRITE I2C_DATAPIN,I2C_CLOCKPIN,MEMS_MagnetAccel_W,ACCEL_CTRL4,[ACCEL_CTRL4_VALUE]
I2CWRITE I2C_DATAPIN,I2C_CLOCKPIN,MEMS_MagnetAccel_W,ACCEL_CTRL5,[ACCEL_CTRL5_VALUE]
I2CWRITE I2C_DATAPIN,I2C_CLOCKPIN,MEMS_MagnetAccel_W,ACCEL_CTRL6,[ACCEL_CTRL6_VALUE]
I2CWRITE I2C_DATAPIN,I2C_CLOCKPIN,MEMS_MagnetAccel_W,ACCEL_CTRL7,[ACCEL_CTRL7_VALUE]
return