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
;
;
;===================================================================
;