Read MinIMU-9 V2 MinIMU-9 v2 Gyro, Accelerometer, and Compass (L3GD20 and LSM303DLHC)

J

jaybeetoo

New Member
Code: 
#PICAXE 28X2
#no_table

#rem
#
# This program reads the raw gyro, magnetometer and accelerometer values from a
# MinIMU-9 V2 MinIMU-9 v2 Gyro, Accelerometer, and Compass (L3GD20 and LSM303DLHC Carrier)
# http://www.pololu.com/catalog/product/1268
#
# jaybeetoo May 2013
#
#endrem

;
; MinIMU-9 V2 - addresses
;
	symbol gyroID		= %11010110	; L3GD20 3-axis gyro
	symbol accelID		= %00110010	; LSM303DLHC 3-axis accelerometer
	symbol magID		= %00111100	; LSM303DLHC 3-axis magnetometer

;
; MinIMU-9 V2 registers
;
	; L3GD20 - three-axis digital output gyroscope
	symbol L3G_WHO_AM_I		= 0x0F	; r
	symbol L3G_CTRL_REG1		= 0x20	; rw
	symbol L3G_CTRL_REG2		= 0x21	; rw
	symbol L3G_CTRL_REG3		= 0x22	; rw
	symbol L3G_CTRL_REG4		= 0x23	; rw
	symbol L3G_CTRL_REG5		= 0x24	; rw
	symbol L3G_OUT_TEMP		= 0x26	; rw
	symbol L3G_STATUS_REG		= 0x27	; r
	symbol L3G_OUT_X_L		= 0x28	; r
	symbol L3G_OUT_X_H		= 0x29	; r
	symbol L3G_OUT_Y_L		= 0x2A	; r
	symbol L3G_OUT_Y_H		= 0x2B	; r
	symbol L3G_OUT_Z_L		= 0x2C	; r
	symbol L3G_OUT_Z_H		= 0x2D	; r
	symbol L3G_FIFO_CTRL_REG	= 0x2E	; rw
	symbol L3G_FIFO_SRC_REG		= 0x2F	; r
	symbol L3G_INT1_CFG		= 0x30	; rw
	symbol L3G_INT1_SRC		= 0x31	; r
	symbol L3G_INT1_TSH_XH		= 0x32	; rw
	symbol L3G_INT1_TSH_XL		= 0x33	; rw
	symbol L3G_INT1_TSH_YH		= 0x34	; rw
	symbol L3G_INT1_TSH_YL		= 0x35	; rw
	symbol L3G_INT1_TSH_ZH		= 0x36	; rw
	symbol L3G_INT1_TSH_ZL		= 0x37	; rw
	symbol L3G_INT1_DURATION	= 0x38	; rw
	; LSM303DLHC - 3D accelerometer and 3D magnetometer module
	symbol LSMa_CTRL_REG1_A		= 0x20	; rw
	symbol LSMa_CTRL_REG2_A		= 0x21	; rw
	symbol LSMa_CTRL_REG3_A		= 0x22	; rw
	symbol LSMa_CTRL_REG4_A		= 0x23	; rw
	symbol LSMa_CTRL_REG5_A		= 0x24	; rw
	symbol LSMa_CTRL_REG6_A		= 0x25	; rw
	symbol LSMa_REFERENCE_A		= 0x26	; rw
	symbol LSMa_STATUS_REG_A	= 0x27	; r
	symbol LSMa_OUT_X_L_A		= 0x28	; r
	symbol LSMa_OUT_X_H_A		= 0x29	; r
	symbol LSMa_OUT_Y_L_A		= 0x2A	; r
	symbol LSMa_OUT_Y_H_A		= 0x2B	; r
	symbol LSMa_OUT_Z_L_A		= 0x2C	; r
	symbol LSMa_OUT_Z_H_A		= 0x2D	; r
	symbol LSMa_FIFO_CTRL_REG_A	= 0x2E	; rw
	symbol LSMa_FIFO_SRC_REG_A	= 0x2F	; r
	symbol LSMa_INT1_CFG_A		= 0x30	; rw
	symbol LSMa_INT1_SOURCE_A	= 0x31	; r
	symbol LSMa_INT1_THS_A		= 0x32	; rw
	symbol LSMa_INT1_DURATION_A	= 0x33	; rw
	symbol LSMa_INT2_CFG_A		= 0x34	; rw
	symbol LSMa_INT2_SOURCE_A	= 0x35	; r
	symbol LSMa_INT2_THS_A		= 0x36	; rw
	symbol LSMa_INT2_DURATION_A	= 0x37	; rw
	symbol LSMa_CLICK_CFG_A		= 0x38	; rw
	symbol LSMa_CLICK_SRC_A		= 0x39	; rw
	symbol LSMa_CLICK_THS_A		= 0x3A	; rw
	symbol LSMa_TIME_LIMIT_A	= 0x3B	; rw
	symbol LSMa_TIME_LATENCY_A	= 0x3C	; rw
	symbol LSMa_TIME_WINDOW_A	= 0x3D	; rw
	symbol LSMm_CRA_REG_M		= 0x00	; rw
	symbol LSMm_CRB_REG_M		= 0x01	; rw
	symbol LSMm_MR_REG_M		= 0x02	; rw
	symbol LSMm_OUT_X_H_M		= 0x03	; r
	symbol LSMm_OUT_X_L_M		= 0x04	; r
	symbol LSMm_OUT_Z_H_M		= 0x05	; r
	symbol LSMm_OUT_Z_L_M		= 0x06	; r
	symbol LSMm_OUT_Y_H_M		= 0x07	; r
	symbol LSMm_OUT_Y_L_M		= 0x08	; r
	symbol LSMm_SR_REG_Mg		= 0x09	; r
	symbol LSMm_IRA_REG_M		= 0x0A	; r
	symbol LSMm_IRB_REG_M		= 0x0B	; r
	symbol LSMm_IRC_REG_M		= 0x0C	; r
	symbol LSMm_TEMP_OUT_H_M	= 0x31
	symbol LSMm_TEMP_OUT_L_M	= 0x32
;
;	Accelerometer scale
;
	symbol accel_scale_c		= 2		; Accelerometer full scale, should be 2, 4 or 8
;	
; Variables
	symbol flg_imu		= bit0	; IMY status flag - 1 (detected) or 0 (not detected)
	symbol accel_scale	= b2		; Accelerometer scale
	symbol gyro_status	= b3		; Gyro status
	symbol OUT_X_H_G		= b4		; X-axis angular rate data. The value is expressed as two’s complement
	symbol OUT_X_L_G		= b5
	symbol OUT_Y_H_G		= b6		; Y-axis angular rate data. The value is expressed as two’s complement
	symbol OUT_Y_L_G		= b7
	symbol OUT_Z_H_G		= b8		; Z-axis angular rate data. The value is expressed as two’s complement
	symbol OUT_Z_L_G		= b9
	symbol OUT_X_H_A		= b10		; X-axis acceleration data. The value is expressed in 2’s complement.
	symbol OUT_X_L_A		= b11
	symbol OUT_Y_H_A		= b12		; Y-axis acceleration data. The value is expressed in 2’s complement.
	symbol OUT_Y_L_A		= b13
	symbol OUT_Z_H_A		= b14		; Z-axis acceleration data. The value is expressed in 2’s complement.
	symbol OUT_Z_L_A		= b15
	symbol OUT_X_H_M		= b16		; X-axis magnetic field data. The value is expressed as 2’s complement.
	symbol OUT_X_L_M		= b17
	symbol OUT_Y_H_M		= b18		; Y-axis magnetic field data. The value is expressed as 2’s complement.
	symbol OUT_Y_L_M		= b19
	symbol OUT_Z_H_M		= b20		; Z-axis magnetic field data. The value is expressed as 2’s complement.
	symbol OUT_Z_L_M		= b21
	symbol SR_REG_M		= b22		; SR register contents
	symbol GyroX 		= W11		; Gyro X raw value
	symbol GyroY		= W12		; Gyro Y raw value
	symbol GyroZ		= W3		; Gyro Z raw value
	symbol AccelX		= W4		; Accelerometer X raw value
	symbol AccelY		= W5		; Accelerometer Y raw value
	symbol AccelZ		= W6		; Accelerometer Z raw value
	symbol MagX			= W7		; Magnetometer X raw value
	symbol MagY			= W8		; Magnetometer Y raw value
	symbol MagZ			= W9		; Magnetometer Z raw value
;
;===================================================================
;
;	Initialise
;
init:
	
	
	gosub init_imu		; Set up IMU
	if flg_imu = 0 then 
		sertxd (CR,LF, "IMU not found")
		goto imu_not_found		; IMU not detected - get out of here
	endif
	
main:
	; Get gyro x, y and z
	hi2cin [gyroID], L3G_OUT_X_L, (OUT_X_L_G)		; X-axis angular rate data. The value is expressed as two’s complement.
	hi2cin [gyroID], L3G_OUT_X_H, (OUT_X_H_G)
	hi2cin [gyroID], L3G_OUT_Y_L, (OUT_Y_L_G)		; Y-axis angular rate data. The value is expressed as two’s complement.
	hi2cin [gyroID], L3G_OUT_Y_H, (OUT_Y_H_G)
	hi2cin [gyroID], L3G_OUT_Z_L, (OUT_Z_L_G)		; Z-axis angular rate data. The value is expressed as two’s complement.
	hi2cin [gyroID], L3G_OUT_Z_H, (OUT_Z_H_G)
	
	; Get magnetometer x, y and z
	do 
		hi2cin [magID], LSMm_SR_REG_Mg, (SR_REG_M)	; Wait for magentometer readings to be ready
		let SR_REG_M = SR_REG_M & $01
	loop while SR_REG_M != 1 
	hi2cin [magID], LSMm_OUT_X_H_M, (OUT_X_H_M)		; X-axis magnetic field data. The value is expressed as 2’s complement.
	hi2cin [magID], LSMm_OUT_X_L_M, (OUT_X_L_M)
	hi2cin [magID], LSMm_OUT_Y_H_M, (OUT_Y_H_M)		; Y-axis magnetic field data. The value is expressed as 2’s complement.
	hi2cin [magID], LSMm_OUT_Y_L_M, (OUT_Y_L_M)
	hi2cin [magID], LSMm_OUT_Z_H_M, (OUT_Z_H_M)		; Z-axis magnetic field data. The value is expressed as 2’s complement.
	hi2cin [magID], LSMm_OUT_Z_L_M, (OUT_Z_L_M)
	
	; Get accelerometer x, y and z
	hi2cin [accelID], LSMa_OUT_X_L_A, (OUT_X_L_A)	; X-axis acceleration data. The value is expressed in 2’s complement.
	hi2cin [accelID], LSMa_OUT_X_H_A, (OUT_X_H_A)
	hi2cin [accelID], LSMa_OUT_Y_L_A, (OUT_Y_L_A)	; Y-axis acceleration data. The value is expressed in 2’s complement.
	hi2cin [accelID], LSMa_OUT_Y_H_A, (OUT_Y_H_A)
	hi2cin [accelID], LSMa_OUT_Z_L_A, (OUT_Z_L_A)	; Z-axis acceleration data. The value is expressed in 2’s complement.
	hi2cin [accelID], LSMa_OUT_Z_H_A, (OUT_Z_H_A)

	let GyroX  = OUT_X_L_G << 8 | OUT_X_H_G	; Combine high and low byte for gyro
	let GyroY  = OUT_Y_L_G << 8 | OUT_Y_H_G
	let GyroZ  = OUT_Z_L_G << 8 | OUT_Z_H_G
	let AccelX = OUT_X_L_A << 8 | OUT_X_H_A	; Combine high and low byte for accelerometer
	let AccelY = OUT_Y_L_A << 8 | OUT_Y_H_A
	let AccelZ = OUT_Z_L_A << 8 | OUT_Z_H_A
	let MagX   = OUT_X_L_M << 8 | OUT_X_H_M	; Combine high and low byte for magnetometer
	let MagY   = OUT_Y_L_M << 8 | OUT_Y_H_M
	let MagZ   = OUT_Z_L_M << 8 | OUT_Z_H_M
	
	
	sertxd (CR,LF,"Raw values>>>")
	sertxd (CR,LF,"Gyro x,y,x: ", #GyroX, ", ", #GyroY, ", ", #GyroZ)
	sertxd (CR,LF,"Accel x,y,x: ", #AccelX, ", ", #AccelY, ", ", #AccelZ)
	sertxd (CR,LF,"Mag x,y,x: ", #MagX, ", ", #MagY, ", ", #MagZ)
	
	pause 1000
	goto main
	
imu_not_found:

	end	
	


;===================================================================
; init_imu		Initailise the IMU
;
init_imu:
	
	; Gyro
	hi2csetup i2cmaster, gyroID, i2cslow, i2cbyte
	hi2cout [gyroID], L3G_CTRL_REG1, (0x0F)	; normal power mode, all axes enabled, 100 Hz
	hi2cout [gyroID], L3G_CTRL_REG4, (0x20)	; 2000 dps full scale
	hi2cin [gyroID], L3G_WHO_AM_I, (gyro_status)	; Check gyro status
	
	if gyro_status <> $d4 then 
		let flg_imu	= 0	; IMU not detected
		goto init_imu_exit
	else
		let flg_imu	= 1	; IMU detected
	endif

	; Accelerometer
	hi2csetup i2cmaster, accelID, i2cslow, i2cbyte
	hi2cout [accelID], LSMa_CTRL_REG1_A, ($27)		; z,y,x axis enabled , 50Hz data rate
	let accel_scale = accel_scale_c
	if accel_scale = 8 or accel_scale = 4  then
		let accel_scale = accel_scale - accel_scale/2 - 1
		let accel_scale = accel_scale << 4
		let accel_scale = $00 | accel_scale
		hi2cout [accelID], LSMa_CTRL_REG4_A, (accel_scale)	; Full scale range , continuous data
	else
		hi2cout [accelID], LSMa_CTRL_REG4_A, ($00)
	endif
		
	; Magnetometer
	hi2csetup i2cmaster, magID, i2cslow, i2cbyte
	hi2cout [magID], LSMm_CRA_REG_M, ($14)			; ODR 30Hz
	hi2cout [magID], LSMm_MR_REG_M, ($00)			; Continuous conversion mode

init_imu_exit:

	return
;
;
;===================================================================
;
 
W

westaust55

Moderator
Using just one group of program lines as an example, I believe that you can reduce the size of the program, reduce the number of variables used,
and speed up the receipt of data and storing received data into final variables with some simple changes to the program code.

Thus for the program lines (extracted from various points in the program);
Code: 
symbol OUT_X_[B]H[/B]_G  = b4	; X-axis angular rate data. The value is expressed as two&#8217;s complement
symbol OUT_X_[B]L[/B]_G	  = b5

symbol GyroX	= W11		; Gyro X raw value

symbol L3G_OUT_X_L	= 0x28	; r
symbol L3G_OUT_X_H	= 0x29	; r

hi2cin [gyroID], L3G_OUT_X_L, (OUT_X_L_G)	; X-axis angular rate data. The value is expressed as two&#8217;s complement.
hi2cin [gyroID], L3G_OUT_X_H, (OUT_X_H_G)

let GyroX  = OUT_X_L_G << 8 | OUT_X_H_G	; Combine high and low byte for gyro
consider changing these to:
Code: 
symbol GyroX 	    = W[COLOR="#FF0000"]2[/COLOR]	    ; w[COLOR="#FF0000"]2[/COLOR]= b5:b4 Gyro X raw value
  symbol OUT_X_L_G      = b4   ; X-axis angular rate data. The value is expressed as two&#8217;s complement
  symbol OUT_X_H_G	  = b5

symbol L3G_OUT_X_L	= 0x28	; r
; symbol L3G_OUT_X_H	= 0x29	; r - this second symbol definition  is now effectively redundant

    hi2cin [gyroID], L3G_OUT_X_L, (OUT_X_L_G,OUT_X_H_G ) ; X-axis angular rate data as two&#8217;s complement.
; The data is now already in GyroX without further further manipulation
Furthermore, in the original line:
let GyroX = OUT_X_L_G << 8 | OUT_X_H_G ; Combine high and low byte for gyro

you are moving the low byte (OUT_X_L_G) data into the high byte of the word variable (GyroX) with the << 8 function and then putting the high byte data into the low byte of the word variable.
 
Last edited:
J

jaybeetoo

New Member
Thanks for your comments westaust55. Very useful.

I assume that 'symbol GyroX = W4' should be 'symbol GyroX = W2'?

Code: 
symbol GyroX 	    = W4	    ; w4 = b5:b4 Gyro X raw value
  symbol OUT_X_L_G      = b4   ; X-axis angular rate data. The value is expressed as two&#8217;s complement
  symbol OUT_X_H_G	  = b5

symbol L3G_OUT_X_L	= 0x28	; r
; symbol L3G_OUT_X_H	= 0x29	; r - this second symbol definition  is now effectively redundant

    hi2cin [gyroID], L3G_OUT_X_L, (OUT_X_L_G,OUT_X_H_G ) ; X-axis angular rate data as two&#8217;s complement.
; The data is now already in GyroX without further further manipulation


Cheers
 
W

westaust55

Moderator
Correct GyroX should be W2. ( which == b5:b4) :mad:
Note that while b5:b4 is used to indicate the byte variables comprising word variable w2, you cannot use that (b5:b4) in lieu of w2

Will fix above (in red) so future readers don't get confused.
 
Last edited:
You must log in or register to reply here.
Top