'Sample Code for Driving GY-87 IMU (Inertial Measurement Unit)
'This unit contains MPU6050 - Accelerometers and Gyros, BMP180 Barometer and Thermometer, HMC5883L 3 axis Compass
'Jimmy Hong Ning
'29/3/2016
'

'I2C addresses
MpuAdd = &H68: '104: ' MPU6050 Accelerometer and Gryo Address. b1101000 AD0 is tied to ground in the GY-87
BmpAdd = &H77: '119: ' BMP180 Barometer Address
CmpAdd = &H1E: ' HMC5883L Compass address

Dim VBuff(6) ' Buffer to received I2C data
I2C Open 20,100,PU ' Open I2C at 20KHz, 100ms timeout and internal pull ups

'Initialise MPU6050
I2C write MpuAdd,1,2,25 ,7 ' Set Sample Rate
I2C write MpuAdd,1,2,27 ,8 ' Gyro Config
I2C write MpuAdd,1,2,107,2 ' Power management PLL with Y axis reference
I2C WRITE MpuAdd,1,2,55 ,2 ' Enable I2C Bypass to access the compass.

'Init Barometer - Retrieve Calibration Settings
AC1 = GetBmpReg16(&HAA)
AC2 = GetBmpReg16(&HAC)
AC3 = GetBmpReg16(&HAE)
AC4 = GetBmpRegU16(&HB0)
AC5 = GetBmpRegU16(&HB2)
AC6 = GetBmpRegU16(&HB4)
B1 = GetBmpReg16(&HB6)
B2 = GetBmpReg16(&HB8)
MB = GetBmpReg16(&HBA)
MC = GetBmpReg16(&HBC)
MD = GetBmpReg16(&HBE)

BmpPress = 0
BmpTemp = 0
Altitude = 0

'Init Compass - Compass doesn't require much initialisation.
CompassID$ = " "
Dim Compass(3)
I2C Write CmpAdd, 1,1,2 :'Read Mode

'Uncomment this section to verify basic comms with the I2C devices.
'Note that compass cannot be accessed until MPU6050 is put into bypass mode.
'I2C write MpuAdd,0,1,117
'I2C read MpuAdd,1,1,whoami
'Print whoami
'
'I2C write BmpAdd,0,1,&hD0
'I2C read BmpAdd,1,1,whoami
'Print whoami
'
'I2C Write CmpAdd, 1,1,10
'I2C Read CmpAdd,1,3,CompassID$
'Print "Compass ID: " ;CompassID$

CLS

While Inkey$=""

' These variables are used to improve screen refresh by erase old lines.
OldAccX = AccX
OldAccY = AccY
OldAccZ = AccZ

OldGyroX = GyroX
OldGyroY = GyroY
OldGyroZ = GyroZ

OldCmpX = CmpX
OldCmpY = CmpY
OldCmpZ = CmpZ


'Get MPU6050 Data
AccX = GetMpuReg16(59)
AccY = GetMpuReg16(61)
AccZ = GetMpuReg16(63)

GyroX = GetMpuReg16(67)
GyroY = GetMpuReg16(69)
GyroZ = GetMpuReg16(71)

'Draw data for 2 axes
Line 100,100,OldAccX/200+100 ,-OldAccY/200+100 ,1, rgb(Black)
Line 300,100,OldGyroX/200+300,OldGyroY/200+100 ,1, rgb(Black)

Line 100,100,AccX/200+100,-AccY/200+100 ' Will show a line that stays horizontal
Line 300,100,GyroX/200+300,GyroY/200+100 ' Will show direction of rotation

'Get Barometer Data
UpdateBmp ' This routine updates temp and pressure and applies calibration
text 10,20,str$(int(BmpTemp*10)/100)+" " ' Display temp to 0.1C
text 200,20,str$(int(BmpPress))+" " ' Display pressure in Pascals
text 200,40,str$(int(Altitude*10)/10)+" " ' Display altitude to 0.1m

'Get Compass Data
I2C Write CmpAdd,1,2,2,1 ' Trigger a read by setting mode to 01
pause 20 ' Maximum time required for read,
I2C Write CmpAdd,1,1,3 ' Read Data
I2C READ CmpAdd,1,6,VBuff() ' Get all 3 words in one go

'Merge bytes into signed values and assign to Compass() array
for i = 0 to 2
Num = VBuff(i*2)*256+VBuff(i*2+1)
If Num >32767 then Num = Num - 65536
Compass(i) = Num
Next I

'For dispay purposes normalise the Y and Z vectors. These axes are for a vertically mounted board.
Mag = sqr(Compass(1)^2 + Compass(2)^2)
CmpY = Compass(1)/Mag
CmpZ = Compass(2)/Mag

'Display a line pointing magnetic north
Line 50,50,-OldCmpZ*50+50,OldCmpY*50+50 ,1, rgb(Black)
Line 50,50,-CmpZ*50+50,CmpY*50+50


Wend

' S U B R O U T I N E S

SUB SetMpuReg(Reg, Value)
I2C Write MpuAdd,1,2,Reg,Value
End Sub

Function GetMpuReg(Reg)
I2C Write MpuAdd,0,1,Reg
I2C Read MpuAdd,1,1, Value
GetMpuReg = Value
End Function

Function GetMpuReg16(Reg)
I2C Write MpuAdd,0,1,Reg
I2C Read MpuAdd,1,1, ValueH
I2C Write MpuAdd,0,1,Reg+1
I2C Read MpuAdd,1,1, ValueL
Value = ValueH*256+ValueL
If Value > 32767 then Value = Value - 65536
GetMpuReg16 = Value
End Function

Function GetBmpReg(Reg)
I2C Write BmpAdd,0,1,Reg
I2C Read BmpAdd,1,1, Value
GetBmpReg = Value
End Function

Function GetBmpReg16(Reg)
I2C Write BmpAdd,0,1,Reg
I2C Read BmpAdd,1,2, VBuff()
Value = VBuff(0)*256+VBuff(1)
If Value > 32767 then Value = Value - 65536
GetBmpReg16 = Value
End Function

Function GetBmpRegU16(Reg)
' Unsigned Version
I2C Write BmpAdd,0,1,Reg
I2C Read BmpAdd,1,2, VBuff()
GetBmpRegU16 = VBuff(0)*256+VBuff(1)
End Function

Function GetBmpRegU24(Reg)
' Unsigned Version

I2C Write BmpAdd,0,1,Reg
I2C Read BmpAdd,1,3, VBuff()
GetBmpRegU24 = (VBuff(0)*256+VBuff(1))*256+VBuff(2)
End Function

SUB SetBmpReg(Reg, Value)
I2C Write BmpAdd,0,2,Reg,Value
End Sub

SUB UpdateBmp()
' Retrieve Raw Temperature. Note that this only needs to be done once a second - but for simplicity have read it every time.
SetBmpReg(&HF4,&H2E)
pause 4.5
UT = GetBmpRegU16(&HF6)
' Correct temp using calibration formulae
X1=(UT-AC6)*AC5/(2^15)
X2=MC*(2^11)/(X1+MD)
B5=X1+X2
BmpTemp=(B5+8)/(2^4)

' Retrieve Raw Pressure
OSS=3 ' Between 0-3, determines the oversampling size. 2^oss samples.
SetBmpReg(&HF4,&H34+OSS*(2^6))
pause 4.5+7*OSS
UP = GetBmpRegU24(&HF6)/(2^(8-OSS))

'Correct pressure
B6=B5-4000
X1=(B2*((B6^2)/(2^12)))/(2^11)
X2=(AC2*B6)/(2^11)
X3=X1+X2
B3=((((AC1*4+X3)*(2^OSS))+2))/4
X1=AC3*B6/(2^13)
X2=(B1*(B6*B6/(2^12)))/(2^16)
X3=((X1+X2)+2)/4
B4=(AC4*(X3+32768))/(2^15)
B7=(UP-B3)*(50000/(2^OSS))
if (B7<&H80000000) then
p=(B7*2)/B4
else
p=(B7/B4)*2
endif
X1=(P/256)^2
X1=(X1*3038)/(2^16)
X2=(-7357*p)/(2^16)
p = (P+(X1+X2+3791)/(2^4))
' Smooth the output.
if BmpPress = 0 then
BmpPress = P 'First time through do not use smoothing so settling is faster.
else
BmpPress=P*0.1 + BmpPress*0.9
Endif

' Calculate altitude based on mean sea level pressure of 1013.25 hPa
Altitude = 44330*(1-(BmpPress/101325)^(1/5.255))
End Sub