MPU-9150 PASM Driver
zlantz
Posts: 136
I have still been trying to get a driver to be satisfactory. I have wrote a spin driver, which works well, however it is noizy & no matter what I do, I cannot filter the noise. Then there is the mpu-6050 pasm driver, it works for the mpu-9150, but you only get accel, gyro & temp values.
So I have taken the advice to keep working on the pasm version. I can read the MPU Chip ID, the AK8975 ID, and the Mag x, y, & z registers on the AK8975.
The problem is, I need to be able to set $0A on the AK8975 to %00000001 (Single-Measurement Mode) before each read of the Mag. I am pretty confident that I have coded this right, but when I try to set this bit, my Mag reads out 0. But if I Dont put it in single measure mode, it will read the x, y, z position on power up, but will remain constant without changing.
So I have taken the advice to keep working on the pasm version. I can read the MPU Chip ID, the AK8975 ID, and the Mag x, y, & z registers on the AK8975.
The problem is, I need to be able to set $0A on the AK8975 to %00000001 (Single-Measurement Mode) before each read of the Mag. I am pretty confident that I have coded this right, but when I try to set this bit, my Mag reads out 0. But if I Dont put it in single measure mode, it will read the x, y, z position on power up, but will remain constant without changing.
'' MPU-60X0-PASM.spin
'' Reads gyro and accelerometer data from the MPU-60X0 chips
'' Read loop is in Propeller Assembler
''
'' Based on Jason Dorie's code for the ITG-3200 and ADCL345 chips
''
'' Note that this code assumes an 80 MHz clock
''
'' The TestMPU routine can be used to verify correct setup of, and
'' communication with, the MPU-60X0. Load the object into RAM, then
'' use f12 to bring up the terminal emulator to see the output.
''
{
*******************************************
* User Init Select *
* Updates by: Zack Lantz *
*******************************************
Place in Main .Spin:
Con
' // Accelerometer Settings
mAFS0 = 0
mAFS1 = 1
mAFS2 = 2
mAFS3 = 3
' // Gyroscope Settings
mFS0 = 0
mFS1 = 1
mFS2 = 2
mFS3 = 3
' // Digital Low Pass Filter Settings
DLP0 = 0 ' Bandwidth = 260 Hz
DLP1 = 1 ' Bandwidth = 184 Hz
DLP2 = 2 ' Bandwidth = 94 Hz
DLP3 = 3 ' Bandwidth = 44 Hz
DLP4 = 4 ' Bandwidth = 21 Hz
DLP5 = 5 ' Bandwidth = 10 Hz
DLP6 = 6 ' Bandwidth = 5 Hz
DLP7 = 7 ' Reserved
' // Current Settings (Passed to MPU-6050.spin)
AccelFS = AFS2
GyroFS = FS3
mAFS = mAFS2
mFS = mFS3
mDLP = DLP3
Then start MPU Driver with:
MPU.Start(MPUscl, MPUsda, mAFS, mFS, mDLP) ' MPU-6050 Gyro & Accel Sensor Data w/ User Init AFS, FS, & DLP
Everything else works the same.
New Functions:
Start (SCL, SDA, aFS, gFS, fDLP)
StartA(SCL, SDA, aFS, gFS, fDLP, PM) ', SF)
StartX(SCL, SDA)
*** Note: I didn't add in SampleRate feature as it is 0 to 255 & Calculated as follows:
Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
Where Gyroscope Output Rate = 8kHz when the DLPF is disabled (DLPF_CFG = 0 or 7), and 1kHz when the DLPF is enabled (see Register 26)
}
{{
The slave address of the MPU-60X0 is b110100X which is 7 bits long. The LSB bit of the 7 bit address is
determined by the logic level on pin AD0. This allows two MPU-60X0s to be connected to the same I2C bus.
When used in this configuration, the address of the one of the devices should be b1101000 (pin AD0
is logic low) and the address of the other should be b1101001 (pin AD0 is logic high).
}}
CON
_clkmode = xtal1 + pll16x
_xinfreq = 5_000_000
CON ' CONs for TestMPU test routine
SDA_PIN = 10
SCL_PIN = 11
SERIAL_TX_PIN = 30
SERIAL_RX_PIN = 31
VAR
long x0, y0, z0, a0, b0, c0, t
long Cog
long rx, ry, rz, temp, ax, ay, az, arx, ary, mx, my, mz 'PASM code assumes these to be contiguous
long cid, mid 'PASM code assumes these to be contiguous
OBJ
debug : "FullDuplexSerial"
PUB TestMPU | MPUcog
'-----------------------------------------------
' Start serial i/o cog
' Start cog to pull gyro/accel data from chip
' Print data to serial out every few seconds
'------------------------------------------------
debug.start(SERIAL_RX_PIN, SERIAL_TX_PIN, 0, 115200) 'Start cog to allow IO with serial terminal
debug.str(string("Starting..."))
debug.tx(13)
debug.str(string("GX GY GZ AX AY AZ"))
debug.tx(13)
debug.str(string("-------------------------"))
debug.tx(13)
MPUcog := StartX( SCL_PIN, SDA_PIN )
'Output gyro data, then accel data, once per second
repeat
debug.str(string("0x"))
debug.hex(GetChipID, 2)
debug.str(string(", "))
debug.str(string("0x"))
debug.hex(GetMagID, 2)
debug.str(string(" "))
debug.dec(GetRX)
debug.str(string(", "))
debug.dec(GetRY)
debug.str(string(", "))
debug.dec(GetRZ)
debug.str(string(" "))
debug.dec(GetAX)
debug.str(string(", "))
debug.dec(GetAY)
debug.str(string(", "))
debug.dec(GetAZ)
debug.str(string(" "))
debug.dec(GetARX)
debug.str(string(", "))
debug.dec(GetARY)
debug.str(string(" "))
debug.dec(GetMX)
debug.str(string(", "))
debug.dec(GetMY)
debug.str(string(", "))
debug.dec(GetMZ)
debug.tx(13)
' // Basic Start with User Accel AFS & Gyro FS & DLP
PUB Start( SCL, SDA, aFS, gFS, fDLP ) : Status
if aFS == 0
AccelFS := %00000000 ' ± 2 g
elseif aFS == 1
AccelFS := %00001000 ' ± 4 g
elseif aFS == 2
AccelFS := %00010000 ' ± 8 g
elseif aFS == 3
AccelFS := %00011000 ' ± 16 g
if gFS == 0
GyroFS := %00000000 ' ± 250 ° /s
elseif gFS == 1
GyroFS := %00001000 ' ± 500 ° /s
elseif gFS == 2
GyroFS := %00010000 ' ± 1000 ° /s
elseif gFS == 3
GyroFS := %00011000 ' ± 2000 ° /s
'| DLPF_CFG | Accelerometer | Gyroscope |
if fDLP == 0 ' Bw (Hz) Delay (ms) Bw (Hz) Delay (ms) FS (Khz)
DLP := %00000000 ' 0 260 0 256 0.98 8
elseif fDLP == 1
DLP := %00000001 ' 1 184 2.0 188 1.9 1
elseif fDLP == 2
DLP := %00000010 ' 2 94 3.0 98 2.8 1
elseif fDLP == 3
DLP := %00000011 ' 3 44 4.9 42 4.8 1
elseif fDLP == 4
DLP := %00000100 ' 4 21 8.5 20 8.3 1
elseif fDLP == 5
DLP := %00000101 ' 5 10 13.8 10 13.4 1
elseif fDLP == 6
DLP := %00000110 ' 6 5 19.0 5 18.6 1
elseif fDLP == 7
DLP := %00000111 ' 7 RESERVED RESERVED 8
PowerMgmt := %00000001 ' X gyro as clock source
SampleRate := %00000001 ' 500 Hz
ComputeTimes
gyroSCL := 1 << SCL 'save I2C pins
gyroSDA := 1 << SDA
Status := Cog := cognew(@Start_Sensors, @rx) + 1
CalibrateAccel
' // Start w/ Full User Init Settings
PUB StartA( SCL, SDA, aFS, gFS, fDLP, PM ) : Status
if aFS == 0
AccelFS := %00000000
elseif aFS == 1
AccelFS := %00001000
elseif aFS == 2
AccelFS := %00010000
elseif aFS == 3
AccelFS := %00011000
if gFS == 0
GyroFS := %00000000
elseif gFS == 1
GyroFS := %00001000
elseif gFS == 2
GyroFS := %00010000
elseif gFS == 3
GyroFS := %00011000
'| DLPF_CFG | Accelerometer | Gyroscope |
if fDLP == 0 ' Bw (Hz) Delay (ms) Bw (Hz) Delay (ms) FS (Khz)
DLP := %00000000 ' 0 260 0 256 0.98 8
elseif fDLP == 1
DLP := %00000001 ' 1 184 2.0 188 1.9 1
elseif fDLP == 2
DLP := %00000010 ' 2 94 3.0 98 2.8 1
elseif fDLP == 3
DLP := %00000011 ' 3 44 4.9 42 4.8 1
elseif fDLP == 4
DLP := %00000100 ' 4 21 8.5 20 8.3 1
elseif fDLP == 5
DLP := %00000101 ' 5 10 13.8 10 13.4 1
elseif fDLP == 6
DLP := %00000110 ' 6 5 19.0 5 18.6 1
elseif fDLP == 7
DLP := %00000111 ' 7 RESERVED RESERVED 8
if PM == 0
PowerMgmt := %00000000 ' Internal 8MHz oscillator
elseif PM == 1
PowerMgmt := %00000001 ' PLL with X axis gyroscope reference
elseif PM == 2
PowerMgmt := %00000010 ' PLL with Y axis gyroscope reference
elseif PM == 3
PowerMgmt := %00000011 ' PLL with Z axis gyroscope reference
elseif PM == 4
PowerMgmt := %00000100 ' PLL with external 32.768kHz reference
elseif PM == 5
PowerMgmt := %00000101 ' PLL with external 19.2MHz referenc
elseif PM == 6
PowerMgmt := %00000110 ' Reserved
elseif PM == 7
PowerMgmt := %00000111 ' Stops the clock and keeps the timing generator in reset
' *** Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
' // 0 threw 255
SampleRate := %00000001 ' 500 Hz Sample Rate, %00000000 = 1000 Hz Sample rate
ComputeTimes
gyroSCL := 1 << SCL 'save I2C pins
gyroSDA := 1 << SDA
Status := Cog := cognew(@Start_Sensors, @rx) + 1
CalibrateAccel
' // Start Basic, No User Init
PUB StartX( SCL, SDA ) : Status
AccelFS := %00010000 ' AFS2
GyroFS := %00011000 ' FS3
DLP := %00000011 ' 40 Hz
PowerMgmt := %00000001 ' X gyro as clock source
SampleRate := %00000001 ' 500 Hz
ComputeTimes
gyroSCL := 1 << SCL 'save I2C pins
gyroSDA := 1 << SDA
Status := Cog := cognew(@Start_Sensors, @rx) + 1
CalibrateAccel
CalibrateGyro
PUB Stop
if Cog
cogstop(Cog~ - 1)
'**********************
' Accessors
'**********************
PUB GetTemp ' Temperature
return temp
'Pub GetTempF ' Temp Deg F
'Pub GetTempC ' Temp Deg C
PUB GetRX ' Accel X - Zero Offset
return rx - x0
PUB GetRY ' Accel Y - Zero Offset
return ry - y0
PUB GetRZ ' Accel Z - Zero Offset
return rz - z0
PUB GetAX ' Gyro X - Zero Offset
return ax - a0
PUB GetAY ' Gyro Y - Zero Offset
return ay - b0
PUB GetAZ ' Gyrp Z - Zero Offset
return az - c0
PUB GetARX ' Pitch Angle
return arx
PUB GetARY ' Roll Angle
return ary
Pub GetAccelOffsetX ' Accelerometer Zero Offset X
return x0
Pub GetAccelOffsetY ' Accelerometer Zero Offset Y
return y0
Pub GetAccelOffsetZ ' Accelerometer Zero Offset Z
return z0
Pub GetGyroOffsetX ' Gyroscope Zero Offset X
return a0
Pub GetGyroOffsetY ' Gyroscope Zero Offset Y
return b0
Pub GetGyroOffsetZ ' Gyroscope Zero Offset Z
return c0
Pub CalAccel ' Calibrate Accelerometer
CalibrateAccel
Pub CalGyro ' Calibrate Gyroscope
CalibrateGyro
Pub GetChipID
return cID
Pub GetMagID
return mID
Pub GetMX
return mx
Pub GetMY
return my
Pub GetMZ
return mz
PRI computeTimes '' Set up timing constants in assembly
' (Done this way to avoid overflow)
i2cDataSet := ((clkfreq / 10000) * 350) / 100000 ' Data setup time - 350ns (400KHz)
i2cClkLow := ((clkfreq / 10000) * 1300) / 100000 ' Clock low time - 1300ns (400KHz)
i2cClkHigh := ((clkfreq / 10000) * 600) / 100000 ' Clock high time - 600ns (400KHz)
i2cPause := clkfreq / 100000 ' Pause between checks for operations
PRI CalibrateAccel | tc, xc, yc, zc, dr
x0 := 0 ' Initialize offsets
y0 := 0
z0 := 0
'wait 1/2 second for the body to stop moving
waitcnt( constant(80_000_000 / 2) + cnt )
'Find the zero points of the 3 axis by reading for ~1 sec and averaging the results
xc := 0
yc := 0
zc := 0
repeat 256
xc += rx
yc += ry
zc += rz
waitcnt( constant(80_000_000/192) + cnt )
'Perform rounding
if( xc > 0 )
xc += 128
elseif( xc < 0 )
xc -= 128
if( yc > 0 )
yc += 128
elseif( yc < 0 )
yc -= 128
if( zc > 0 )
zc += 128
elseif( zc < 0 )
zc -= 128
x0 := xc / 256
y0 := yc / 256
z0 := zc / 256
PRI CalibrateGyro | tc, xc, yc, zc, dr
a0 := 0 ' Initialize offsets
b0 := 0
c0 := 0
'wait 1/2 second for the body to stop moving
waitcnt( constant(80_000_000 / 2) + cnt )
'Find the zero points of the 3 axis by reading for ~1 sec and averaging the results
xc := 0
yc := 0
zc := 0
repeat 256
xc += ax
yc += ay
zc += az
waitcnt( constant(80_000_000/192) + cnt )
'Perform rounding
if( xc > 0 )
xc += 128
elseif( xc < 0 )
xc -= 128
if( yc > 0 )
yc += 128
elseif( yc < 0 )
yc -= 128
if( zc > 0 )
zc += 128
elseif( zc < 0 )
zc -= 128
a0 := xc / 256
b0 := yc / 256
c0 := zc / 256
DAT
org 0
Start_Sensors
' --------- Debugger Kernel add this at Entry (Addr 0) ---------
' long $34FC1202,$6CE81201,$83C120B,$8BC0E0A,$E87C0E03,$8BC0E0A
' long $EC7C0E05,$A0BC1207,$5C7C0003,$5C7C0003,$7FFC,$7FF8
' --------------------------------------------------------------
mov p1, par ' Get data pointer
mov prX, p1 ' Store the pointer to the rx var in HUB RAM
add p1, #4
mov prY, p1 ' Store the pointer to the ry var in HUB RAM
add p1, #4
mov prZ, p1 ' Store the pointer to the rz var in HUB RAM
add p1, #4
mov pT, p1 ' Store the pointer to the temp var in HUB RAM
add p1, #4
mov paX, p1 ' Store the pointer to the ax var in HUB RAM
add p1, #4
mov paY, p1 ' Store the pointer to the ay var in HUB RAM
add p1, #4
mov paZ, p1 ' Store the pointer to the az var in HUB RAM
add p1, #4
mov paRX, p1 ' Store the pointer to the arx var in HUB RAM
add p1, #4
mov paRY, p1 ' Store the pointer to the ary var in HUB RAM
add p1, #4
mov pmX, p1 ' Store the pointer to the ax var in HUB RAM
add p1, #4
mov pmY, p1 ' Store the pointer to the ay var in HUB RAM
add p1, #4
mov pmZ, p1 ' Store the pointer to the az var in HUB RAM
add p1, #4
mov pcID, p1 ' Store the pointer to the ary var in HUB RAM
add p1, #4
mov pmID, p1 ' Store the pointer to the ary var in HUB RAM
mov i2cTemp,i2cPause
add i2cTemp,CNT ' Wait 10us before starting
waitcnt i2cTemp,#0
call #SetConfig
mov loopCount, CNT
add loopCount, loopDelay
'------------------------------------------------------------------------------
' Main loop
' loopDelay defined in data section
' Nominally set to CLK_FREQ/200 give 200hz update rate, but the update takes less than
' 500us, so the delay could potentially be set to give an update rate as high as 2000hz
'
:loop
call #MPUReadValues
call #MPUComputeDrift
call #ComputeAngles
'call #MPUGetChipID ' = 0x68 = Correct
'call #MPUGetMagID ' = 0x48 = Correct
[B]call #ConfigMag ' // *** FINAL PROBLEM HERE!!! Cannot get AK8975 to go into Single-Read Mode. [/B]
call #MPUGetMagX
call #MPUGetMagY
call #MPUGetMagZ
wrlong iT, pT
subs irX, drift
wrlong irX, prX
subs irY, drift
wrlong irY, prY
subs irZ, drift
wrlong irZ, prZ
wrlong iaX, paX
wrlong iaY, paY
wrlong iaZ, paZ
'Sign extend the 15th bit
test iaRX, i2cWordReadMask wc
muxc iaRX, i2cWordMask
'Sign extend the 15th bit
test iaRY, i2cWordReadMask wc
muxc iaRY, i2cWordMask
wrlong iaRX, paRX
wrlong iaRY, paRY
wrlong iMX, pMX
wrlong iMY, pMY
wrlong iMZ, pMZ
mov icID, #$68 ' (Not Calling MPUGetChipID or MPUGetMagID)
mov imID, #$48
wrlong icID, pcID
wrlong imID, pmID
waitcnt loopCount, loopDelay
jmp #:loop
'------------------------------------------------------------------------------
' MPUReadValues
'
' Starting at the ACCEL_X data register, read in the 3 accel values,
' the temperature, and the 3 gyro values, as these are held in
' sequential register locations.
'
MPUReadValues
mov i2cSDA, gyroSDA 'Use gyro SDA,SCL
mov i2cSCL, gyroSCL
mov i2cAddr, #59 ' Address of ACCEL_XOUT_H
mov i2cDevID, #%11010000 ' Device ID of the MPU
call #StartRead ' Tell the I2C device we're starting
mov i2cMask, i2cWordReadMask
test i2cTestCarry, #0 wc ' Clear the carry flag to make reads auto-increment
call #i2cRead
call #i2cRead
'Sign extend the 15th bit
test i2cData, i2cWordReadMask wc
muxc i2cData, i2cWordMask
mov iaX, i2cData
mov i2cMask, i2cWordReadMask
test i2cTestCarry, #0 wc ' Clear the carry flag to make reads auto-increment
call #i2cRead
call #i2cRead
'Sign extend the 15th bit
test i2cData, i2cWordReadMask wc
muxc i2cData, i2cWordMask
mov iaY, i2cData
mov i2cMask, i2cWordReadMask
test i2cTestCarry, #0 wc ' Clear the carry flag to make reads auto-increment
call #i2cRead
call #i2cRead
'Sign extend the 15th bit
test i2cData, i2cWordReadMask wc
muxc i2cData, i2cWordMask
mov iaZ, i2cData
mov i2cMask, i2cWordReadMask
test i2cTestCarry, #0 wc ' Clear the carry flag to make reads auto-increment
call #i2cRead
call #i2cRead
test i2cData, i2cWordReadMask wc
muxc i2cData, i2cWordMask
mov iT, i2cData
mov i2cMask, i2cWordReadMask
test i2cTestCarry, #0 wc ' Clear the carry flag to make reads auto-increment
call #i2cRead
call #i2cRead
'Sign extend the 15th bit
test i2cData, i2cWordReadMask wc
muxc i2cData, i2cWordMask
mov irX, i2cData
mov i2cMask, i2cWordReadMask
test i2cTestCarry, #0 wc ' Clear the carry flag to make reads auto-increment
call #i2cRead
call #i2cRead
'Sign extend the 15th bit
test i2cData, i2cWordReadMask wc
muxc i2cData, i2cWordMask
mov irY, i2cData
mov i2cMask, i2cWordReadMask
test i2cTestCarry, #0 wc ' Clear the carry flag to make reads auto-increment
call #i2cRead
test i2cTestCarry, #1 wc ' Set the carry flag to tell it we're done
call #i2cRead
'Sign extend the 15th bit
test i2cData, i2cWordReadMask wc
muxc i2cData, i2cWordMask
mov irZ, i2cData
call #i2cStop
MPUReadValues_Ret ret
'------------------------------------------------------------------------------
{
MPUGetChipID
mov i2cSDA, gyroSDA 'Use gyro SDA,SCL
mov i2cSCL, gyroSCL
mov i2cAddr, #117 ' Address of ACCEL_XOUT_H
mov i2cDevID, #%11010000 ' Device ID of the MPU
call #StartRead ' Tell the I2C device we're starting
mov i2cMask, i2cWordReadMask
test i2cTestCarry, #1 wc ' Clear the carry flag to make reads auto-increment
call #i2cRead
' // Single Byte Read; Shift Right 8 Bits
shr i2cData, #8
mov icID, i2cData ' // Fixed = 0x68
call #i2cStop
MPUGetChipID_Ret ret
'------------------------------------------------------------------------------
MPUGetMagID
mov i2cSDA, gyroSDA 'Use gyro SDA,SCL
mov i2cSCL, gyroSCL
mov i2cAddr, #00
mov i2cDevID, #%00011000
call #StartRead ' Tell the I2C device we're starting
mov i2cMask, i2cWordReadMask
test i2cTestCarry, #1 wc ' Clear the carry flag to make reads auto-increment
call #i2cRead
' // Single Byte Read; Shift Right 8 Bits
shr i2cData, #8
mov imID, i2cData ' // Fixed = 0x48
call #i2cStop
MPUGetMagID_Ret ret
}
'------------------------------------------------------------------------------
MPUGetMagX
[B]'jmpret MPUGetMagX, #MPUMag ' // *** FINAL PROBLEM HERE!!! Cannot get AK8975 to go into Single-Read Mode.[/B]
mov i2cSDA, gyroSDA 'Use gyro SDA,SCL
mov i2cSCL, gyroSCL
mov i2cAddr, #03
mov i2cDevID, #%00011000
call #StartRead ' Tell the I2C device we're starting
mov i2cMask, i2cWordReadMask
test i2cTestCarry, #0 wc
call #i2cRead
test i2cTestCarry, #1 wc ' Clear the carry flag to make reads auto-increment
call #i2cRead
'Sign extend the 15th bit
test i2cData, i2cWordReadMask wc
muxc i2cData, i2cWordMask
mov imX, i2cData
call #i2cStop
MPUGetMagX_Ret ret
'------------------------------------------------------------------------------
MPUGetMagY
[B] 'jmpret MPUGetMagY, #MPUMag ' // *** FINAL PROBLEM HERE!!! Cannot get AK8975 to go into Single-Read Mode.[/B]
mov i2cSDA, gyroSDA 'Use gyro SDA,SCL
mov i2cSCL, gyroSCL
mov i2cAddr, #05
mov i2cDevID, #%00011000
call #StartRead ' Tell the I2C device we're starting
mov i2cMask, i2cWordReadMask
test i2cTestCarry, #0 wc
call #i2cRead
test i2cTestCarry, #1 wc ' Clear the carry flag to make reads auto-increment
call #i2cRead
'Sign extend the 15th bit
test i2cData, i2cWordReadMask wc
muxc i2cData, i2cWordMask
mov imY, i2cData
call #i2cStop
MPUGetMagY_Ret ret
'------------------------------------------------------------------------------
MPUGetMagZ
[B] 'jmpret MPUGetMagZ, #MPUMag ' // *** FINAL PROBLEM HERE!!! Cannot get AK8975 to go into Single-Read Mode. [/B]
mov i2cSDA, gyroSDA 'Use gyro SDA,SCL
mov i2cSCL, gyroSCL
mov i2cAddr, #07
mov i2cDevID, #%00011000
call #StartRead ' Tell the I2C device we're starting
mov i2cMask, i2cWordReadMask
test i2cTestCarry, #0 wc
call #i2cRead
test i2cTestCarry, #1 wc ' Clear the carry flag to make reads auto-increment
call #i2cRead
'Sign extend the 15th bit
test i2cData, i2cWordReadMask wc
muxc i2cData, i2cWordMask
mov imZ, i2cData
call #i2cStop
MPUGetMagZ_Ret ret
'------------------------------------------------------------------------------
[B]
' // *** FINAL PROBLEM HERE!!! Cannot get AK8975 to go into Single-Read Mode.
' // From my Spin Driver (Works):
{
i2c.Start(SCL)
i2c.Write(SCL, MagAddrW) ' AK8975 Write Address
i2c.Write(SCL, $0A) ' CTRL Register
i2c.Write(SCL, %00000001) ' Set config $0A to %00000001 to turn on the device.
i2c.Stop(SCL)
}
ConfigMag
mov i2cSDA, gyroSDA 'Use gyro SDA,SCL
mov i2cSCL, gyroSCL
call #i2cReset
mov i2cDevID, #%00011000 ' MagAddrW
mov i2cMask, #%10000000
call #i2cWrite
mov i2cAddr, #10 ' $0A
mov i2cMask, #%10000000
call #i2cWrite
mov i2cValue, #%00000001 ' %00000001
mov i2cMask, #%10000000
call #i2cWrite
call #i2cStop
ConfigMag_Ret ret[/B]
'------------------------------------------------------------------------------
' Compute drift - for my gyro (Jason's ITG-3200)
'(Temp + 15000) / 100 = drift
'------------------------------------------------------------------------------
MPUComputeDrift
mov drift, iT ' Start with the temperature reading
add drift, tempOffset ' Offset it by 15,000
' divide drift by 100
mov divisor, #100
mov dividend, drift
test dividend, i2cWordReadMask wc
muxc signbit, #1 ' record the sign of the original value
abs dividend, dividend
mov divCounter, #10
shl divisor, divCounter
mov resultShifted, #1
shl resultShifted, divCounter
add divCounter, #1
mov drift, #0
:divLoop
cmp dividend, divisor wc
if_nc add drift, resultShifted
if_nc sub dividend, divisor
shr resultShifted, #1
shr divisor, #1
djnz divCounter, #:divLoop
test signbit, #1 wc
negc drift, drift
MPUComputeDrift_Ret ret
'------------------------------------------------------------------------------
ComputeAngles
mov cx, iaZ
mov cy, iaX
call #cordic
mov iaRX, ca
mov cx, iaZ
mov cy, iaY
call #cordic
mov iaRY, ca
ComputeAngles_ret ret
'------------------------------------------------------------------------------
' SetConfig
'
' See MPU-6000/6050 Register Map document for register addresses and
' valid settings
'
SetConfig
mov i2cSDA, gyroSDA 'Use gyro SDA,SCL
mov i2cSCL, gyroSCL
call #i2cReset 'Reset i2c
:MPUSetConfig mov i2cDevID, #%11010000 'Device ID for the MPU-6000/6050
' // Power Management
mov i2cAddr, #107 'Set PWR_MGMT_1 register bit 0 to choose
mov i2cValue, PowerMgmt ' X gyro as clock source '
call #i2cWriteRegisterByte
' // I2C Master Disabled
mov i2cAddr, #106
mov i2cValue, #%00000000
call #i2cWriteRegisterByte
' // Digital Low Pass Filter _ Config
mov i2cAddr, #26
mov i2cValue, DLP 'Set DLPF_CONFIG to 3 for 40Hz bandwidth
call #i2cWriteRegisterByte
' // Sample Rate Divider
mov i2cAddr, #25 'SMPLRT_DIV = 1 => 1khz/(1+0) = 1000hz sample rate
mov i2cValue, SampleRate
call #i2cWriteRegisterByte
' // Gyro _ Config
mov i2cAddr, #27 'GYRO_CONFIG register, set FS_SEL bits to 3 gives a
mov i2cValue, GyroFS ' full scale range of +-2000 deg/sec
call #i2cWriteRegisterByte
' // Accel _ Config
mov i2cAddr, #28 'Set ACCEL_CONFIG register AFS_SEL bits to 2
mov i2cValue, AccelFS ' sets +-8g full scale range
call #i2cWriteRegisterByte 'ACCEL_HPF is zero which turns off high-pass filtering
' // I2C Bypass Enabled
mov i2cAddr, #55
mov i2cValue, #%00000010
call #i2cWriteRegisterByte
[B]
' // Enable Magnetometer for Reading
MPUMag mov i2cSDA, gyroSDA 'Use gyro SDA,SCL
mov i2cSCL, gyroSCL
call #i2cReset 'Reset i2c
mov i2cDevID, #%00011000 'Device ID for the MPU-Mag
mov i2cAddr, #10 '#$0A
mov i2cValue, #%00000001 ' Set to Single-Measurement Mode
call #i2cWriteRegisterByte [/B]
{
' // Extra Config Settings:
mov i2cAddr, #Address
mov i2cValue, #%BIN_Value
call #i2cWriteRegisterByte
}
SetConfig_Ret ret
'------------------------------------------------------------------------------
StartRead
call #i2cStart
mov i2cData, i2cDevID
mov i2cMask, #%10000000
call #i2cWrite
mov i2cData, i2cAddr
mov i2cMask,#%10000000
call #i2cWrite
call #i2cStart
mov i2cData, i2cDevID
or i2cData, #1
mov i2cMask, #%10000000
call #i2cWrite
StartRead_Ret ret
'------------------------------------------------------------------------------
i2cWriteRegisterByte
call #i2cStart
mov i2cData, i2cDevID
mov i2cMask,#%10000000
call #i2cWrite
mov i2cTime,i2cClkLow
add i2cTime,cnt ' Allow for minimum SCL low
waitcnt i2cTime, #0
mov i2cData, i2cAddr
mov i2cMask,#%10000000
call #i2cWrite
mov i2cTime,i2cClkLow
add i2cTime,cnt ' Allow for minimum SCL low
waitcnt i2cTime, #0
mov i2cData, i2cValue
mov i2cMask,#%10000000
call #i2cWrite
call #i2cStop
i2cWriteRegisterByte_Ret
ret
'------------------------------------------------------------------------------
'' Low level I2C routines. These are designed to work either with a standard I2C bus
'' (with pullups on both SCL and SDA) or the Propellor Demo Board (with a pullup only
'' on SDA). Timing can be set by the caller to 100KHz or 400KHz.
'------------------------------------------------------------------------------
'' Do I2C Reset Sequence. Clock up to 9 cycles. Look for SDA high while SCL
'' is high. Device should respond to next Start Sequence. Leave SCL high.
i2cReset andn dira,i2cSDA ' Pullup drive SDA high
mov i2cBitCnt,#9 ' Number of clock cycles
mov i2cTime,i2cClkLow
add i2cTime,cnt ' Allow for minimum SCL low
:i2cResetClk andn outa,i2cSCL ' Active drive SCL low
or dira,i2cSCL
waitcnt i2cTime,i2cClkHigh
or outa,i2cSCL ' Active drive SCL high
or dira,i2cSCL
andn dira,i2cSCL ' Pullup drive SCL high
waitcnt i2cTime,i2cClkLow ' Allow minimum SCL high
test i2cSDA,ina wz ' Stop if SDA is high
if_z djnz i2cBitCnt,#:i2cResetClk ' Stop after 9 cycles
i2cReset_ret ret ' Should be ready for Start
'------------------------------------------------------------------------------
'' Do I2C Start Sequence. This assumes that SDA is a floating input and
'' SCL is also floating, but may have to be actively driven high and low.
'' The start sequence is where SDA goes from HIGH to LOW while SCL is HIGH.
i2cStart
or outa,i2cSCL ' Active drive SCL high
or dira,i2cSCL
or outa,i2cSDA ' Active drive SDA high
or dira,i2cSDA
mov i2cTime,i2cClkHigh
add i2cTime,cnt ' Allow for bus free time
waitcnt i2cTime,i2cClkLow
andn outa,i2cSDA ' Active drive SDA low
waitcnt i2cTime,#0
andn outa,i2cSCL ' Active drive SCL low
i2cStart_ret ret
'------------------------------------------------------------------------------
'' Do I2C Stop Sequence. This assumes that SCL is low and SDA is indeterminant.
'' The stop sequence is where SDA goes from LOW to HIGH while SCL is HIGH.
'' i2cStart must have been called prior to calling this routine for initialization.
'' The state of the (c) flag is maintained so a write error can be reported.
i2cStop
or outa,i2cSCL ' Active drive SCL high
mov i2cTime,i2cClkHigh
add i2cTime,cnt ' Wait for minimum clock low
waitcnt i2cTime,i2cClkLow
or outa,i2cSDA ' Active drive SDA high
waitcnt i2cTime,i2cClkLow
andn dira,i2cSCL ' Pullup drive SCL high
waitcnt i2cTime,i2cClkLow ' Wait for minimum setup time
andn dira,i2cSDA ' Pullup drive SDA high
waitcnt i2cTime,#0 ' Allow for bus free time
i2cStop_ret ret
'------------------------------------------------------------------------------
'' Write I2C data. This assumes that i2cStart has been called and that SCL is low,
'' SDA is indeterminant. The (c) flag will be set on exit from ACK/NAK with ACK == false
'' and NAK == true. Bytes are handled in "little-endian" order so these routines can be
'' used with words or longs although the bits are in msb..lsb order.
i2cWrite mov i2cBitCnt,#8
mov i2cTime,i2cClkLow
add i2cTime,cnt ' Wait for minimum SCL low
:i2cWriteBit waitcnt i2cTime,i2cDataSet
test i2cData,i2cMask wz
if_z or dira,i2cSDA ' Copy data bit to SDA
if_nz andn dira,i2cSDA
waitcnt i2cTime,i2cClkHigh ' Wait for minimum setup time
or outa,i2cSCL ' Active drive SCL high
waitcnt i2cTime,i2cClkLow
andn outa,i2cSCL ' Active drive SCL low
ror i2cMask,#1 ' Go do next bit if not done
djnz i2cBitCnt,#:i2cWriteBit
andn dira,i2cSDA ' Switch SDA to input and
waitcnt i2cTime,i2cClkHigh ' wait for minimum SCL low
or outa,i2cSCL ' Active drive SCL high
waitcnt i2cTime,i2cClkLow ' Wait for minimum high time
test i2cSDA,ina wc ' Sample SDA (ACK/NAK) then
andn outa,i2cSCL ' active drive SCL low
andn outa,i2cSDA ' active drive SDA low
or dira,i2cSDA ' Leave SDA low
rol i2cMask,#16 ' Prepare for multibyte write
waitcnt i2cTime,#0 ' Wait for minimum low time
i2cWrite_ret ret
'------------------------------------------------------------------------------
'' Read I2C data. This assumes that i2cStart has been called and that SCL is low,
'' SDA is indeterminant. ACK/NAK will be copied from the (c) flag on entry with
'' ACK == low and NAK == high. Bytes are handled in "little-endian" order so these
'' routines can be used with words or longs although the bits are in msb..lsb order.
i2cRead mov i2cBitCnt,#8
andn dira,i2cSDA ' Make sure SDA is set to input
mov i2cTime,i2cClkLow
add i2cTime,cnt ' Wait for minimum SCL low
:i2cReadBit waitcnt i2cTime,i2cClkHigh
or outa,i2cSCL ' Active drive SCL high
waitcnt i2cTime,i2cClkLow ' Wait for minimum clock high
test i2cSDA,ina wz ' Sample SDA for data bits
andn outa,i2cSCL ' Active drive SCL low
if_nz or i2cData,i2cMask ' Accumulate data bits
if_z andn i2cData,i2cMask
ror i2cMask,#1 ' Shift the bit mask and
djnz i2cBitCnt,#:i2cReadBit ' continue until done
waitcnt i2cTime,i2cDataSet ' Wait for end of SCL low
if_c or outa,i2cSDA ' Copy the ACK/NAK bit to SDA
if_nc andn outa,i2cSDA
or dira,i2cSDA ' Make sure SDA is set to output
waitcnt i2cTime,i2cClkHigh ' Wait for minimum setup time
or outa,i2cSCL ' Active drive SCL high
waitcnt i2cTime,i2cClkLow ' Wait for minimum clock high
andn outa,i2cSCL ' Active drive SCL low
andn outa,i2cSDA ' Leave SDA low
waitcnt i2cTime,#0 ' Wait for minimum low time
i2cRead_ret ret
{
ComputeAngles
mov cx, iaZ
mov cy, iaX
call #cordic
mov iaRX, ca
mov cx, iaZ
mov cy, iaY
call #cordic
mov iaRY, ca
ComputeAngles_ret ret
}
'------------------------------------------------------------------------------
'' Perform CORDIC cartesian-to-polar conversion
''Input = cx(x) and cy(x)
''Output = cx(ro) and ca(theta)
cordic abs cx,cx wc
if_c neg cy,cy
mov ca,#0
rcr ca,#1
movs :lookup,#cordicTable
mov t1,#0
mov t2,#20
:loop mov dx,cy wc
sar dx,t1
mov dy,cx
sar dy,t1
sumc cx,dx
sumnc cy,dy
:lookup sumc ca,cordicTable
add :lookup,#1
add t1,#1
djnz t2,#:loop
shr ca, #16
cordic_ret ret
cordicTable long $20000000
long $12E4051E
long $09FB385B
long $051111D4
long $028B0D43
long $0145D7E1
long $00A2F61E
long $00517C55
long $0028BE53
long $00145F2F
long $000A2F98
long $000517CC
long $00028BE6
long $000145F3
long $0000A2FA
long $0000517D
long $000028BE
long $0000145F
long $00000A30
long $00000518
dx long 0
dy long 0
cx long 0
cy long 0
ca long 0
t1 long 0
t2 long 0
'' Variables for the gyro routines
p1 long 0
pT long 0 ' Pointer to Temperature in hub ram
prX long 0 ' Pointer to X rotation in hub ram
prY long 0 ' Pointer to Y rotation in hub ram
prZ long 0 ' Pointer to Z rotation in hub ram
paX long 0 ' Pointer to X accel in hub ram
paY long 0 ' Pointer to Y accel in hub ram
paZ long 0 ' Pointer to Z accel in hub ram
paRX long 0 ' Pointer to X accel angle in hub ram
paRY long 0 ' Pointer to Y accel angle in hub ram
pmX long 0
pmY long 0
pmZ long 0
pcID long 0
pmID long 0
iT long 0 ' Interim temperature value
irX long 0 ' Interim rX value
irY long 0 ' Interim rY value - These values are temp storage before drift compensation
irZ long 0 ' Interim rZ value
iaX long 0 ' Interim aX value
iaY long 0 ' Interim aY value
iaZ long 0 ' Interim aZ value
iaRX long 0 ' Interim aX value
iaRY long 0 ' Interim aY value
imX long 0
imY long 0
imZ long 0
icID long 0
imID long 0
i2cWordReadMask long %10000000_00000000
i2cWordMask long $ffff0000
loopDelay long 80_000_000 / 200
loopCount long 0
'' Variables for dealing with drift / division
tempOffset long 15000
drift long 0
divisor long 0
dividend long 0
resultShifted long 0
signbit long 0
divCounter long 0
'' Variables for i2c routines
i2cTemp long 0
i2cCount long 0
i2cValue long 0
i2cDevID long 0
i2cAddr long 0
i2cDataSet long 0 ' Minumum data setup time (ticks)
i2cClkLow long 0 ' Minimum clock low time (ticks)
i2cClkHigh long 0 ' Minimum clock high time (ticks)
i2cPause long 0 ' Pause before re-fetching next operation
i2cTestCarry long 1 ' Used for setting the carry flag
'' Local variables for low level I2C routines
' // MPU-6050 User Set Init Settings
GyroFS long %00000000
AccelFS long %00000000
PowerMgmt long %00000000
SampleRate long %00000000
DLP long %00000000
gyroSCL long 0 ' Bit mask for SCL
gyroSDA long 0 ' Bit mask for SDA
i2cSCL long 0 ' Bit mask for SCL
i2cSDA long 0 ' Bit mask for SDA
i2cTime long 0 ' Used for timekeeping
i2cData long 0 ' Data to be transmitted / received
i2cMask long 0 ' Bit mask for bit to be tx / rx
i2cBitCnt long 0 ' Number of bits to tx / rx
FIT 496
Comments
It should be possible with only the pasm i2c driver:
'' MPU-60X0-PASM.spin '' Reads gyro and accelerometer data from the MPU-60X0 chips '' Read loop is in Propeller Assembler '' '' Based on Jason Dorie's code for the ITG-3200 and ADCL345 chips '' '' Note that this code assumes an 80 MHz clock '' '' The TestMPU routine can be used to verify correct setup of, and '' communication with, the MPU-60X0. Load the object into RAM, then '' use f12 to bring up the terminal emulator to see the output. '' { ******************************************* * User Init Select & MPU-9150 * * Updates by: Zack Lantz * ******************************************* Place in Main .Spin: Con ' // Accelerometer Settings mAFS0 = 0 mAFS1 = 1 mAFS2 = 2 mAFS3 = 3 ' // Gyroscope Settings mFS0 = 0 mFS1 = 1 mFS2 = 2 mFS3 = 3 ' // Digital Low Pass Filter Settings DLP0 = 0 ' Bandwidth = 260 Hz DLP1 = 1 ' Bandwidth = 184 Hz DLP2 = 2 ' Bandwidth = 94 Hz DLP3 = 3 ' Bandwidth = 44 Hz DLP4 = 4 ' Bandwidth = 21 Hz DLP5 = 5 ' Bandwidth = 10 Hz DLP6 = 6 ' Bandwidth = 5 Hz DLP7 = 7 ' Reserved ' // Current Settings (Passed to MPU-6050.spin) AccelFS = AFS2 GyroFS = FS3 mAFS = mAFS2 mFS = mFS3 mDLP = DLP3 Then start MPU Driver with: MPU.Start(MPUscl, MPUsda, mAFS, mFS, mDLP) ' MPU-6050 Gyro & Accel Sensor Data w/ User Init AFS, FS, & DLP Everything else works the same. New Functions: Start (SCL, SDA, aFS, gFS, fDLP) StartA(SCL, SDA, aFS, gFS, fDLP, PM) ', SF) StartX(SCL, SDA) *** Note: I didn't add in SampleRate feature as it is 0 to 255 & Calculated as follows: Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) Where Gyroscope Output Rate = 8kHz when the DLPF is disabled (DLPF_CFG = 0 or 7), and 1kHz when the DLPF is enabled (see Register 26) } {{ The slave address of the MPU-60X0 is b110100X which is 7 bits long. The LSB bit of the 7 bit address is determined by the logic level on pin AD0. This allows two MPU-60X0s to be connected to the same I2C bus. When used in this configuration, the address of the one of the devices should be b1101000 (pin AD0 is logic low) and the address of the other should be b1101001 (pin AD0 is logic high). }} CON _clkmode = xtal1 + pll16x _xinfreq = 5_000_000 CON ' CONs for TestMPU test routine SDA_PIN = 10 SCL_PIN = 11 SERIAL_TX_PIN = 30 SERIAL_RX_PIN = 31 VAR long x0, y0, z0, a0, b0, c0, d0, e0, f0, t long Cog long rx, ry, rz, temp, ax, ay, az, arx, ary, mx, my, mz 'PASM code assumes these to be contiguous long cid, mid 'PASM code assumes these to be contiguous OBJ debug : "FullDuplexSerial" i2c : "Minimal_i2c_Driver" PUB TestMPU | MPUcog '----------------------------------------------- ' Start serial i/o cog ' Start cog to pull gyro/accel data from chip ' Print data to serial out every few seconds '------------------------------------------------ debug.start(SERIAL_RX_PIN, SERIAL_TX_PIN, 0, 115200) 'Start cog to allow IO with serial terminal debug.str(string("Starting...")) debug.tx(13) debug.str(string("GX GY GZ AX AY AZ")) debug.tx(13) debug.str(string("-------------------------")) debug.tx(13) [B]i2c.Initialize(SCL_PIN)[/B] MPUcog := StartX( SCL_PIN, SDA_PIN ) repeat debug.str(string("0x")) debug.hex(GetChipID, 2) debug.str(string(", ")) debug.str(string("0x")) debug.hex(GetMagID, 2) debug.str(string(" ")) debug.dec(GetRX) debug.str(string(", ")) debug.dec(GetRY) debug.str(string(", ")) debug.dec(GetRZ) debug.str(string(" ")) debug.dec(GetAX) debug.str(string(", ")) debug.dec(GetAY) debug.str(string(", ")) debug.dec(GetAZ) debug.str(string(" ")) debug.dec(GetARX) debug.str(string(", ")) debug.dec(GetARY) debug.str(string(" ")) [B]InitMag ' // Couldnt get this working below in the PASM section[/B] debug.dec(GetMX) debug.str(string(", ")) debug.dec(GetMY) debug.str(string(", ")) debug.dec(GetMZ) debug.tx(13) [B]' // Set Mag to Single-Measurement Mode Pri InitMag i2c.Start(gyroSCL) i2c.Write(gyroSCL, %00011000) ' AK8975 Write Address i2c.Write(gyroSCL, $0A) ' CTRL Register i2c.Write(gyroSCL, %00000001) ' Set config $0A to %00000001 to turn on the device. i2c.Stop(gyroSCL) [/B] ' // Basic Start with User Accel AFS & Gyro FS & DLP PUB Start( SCL, SDA, aFS, gFS, fDLP ) : Status if aFS == 0 AccelFS := %00000000 ' ± 2 g elseif aFS == 1 AccelFS := %00001000 ' ± 4 g elseif aFS == 2 AccelFS := %00010000 ' ± 8 g elseif aFS == 3 AccelFS := %00011000 ' ± 16 g if gFS == 0 GyroFS := %00000000 ' ± 250 ° /s elseif gFS == 1 GyroFS := %00001000 ' ± 500 ° /s elseif gFS == 2 GyroFS := %00010000 ' ± 1000 ° /s elseif gFS == 3 GyroFS := %00011000 ' ± 2000 ° /s '| DLPF_CFG | Accelerometer | Gyroscope | if fDLP == 0 ' Bw (Hz) Delay (ms) Bw (Hz) Delay (ms) FS (Khz) DLP := %00000000 ' 0 260 0 256 0.98 8 elseif fDLP == 1 DLP := %00000001 ' 1 184 2.0 188 1.9 1 elseif fDLP == 2 DLP := %00000010 ' 2 94 3.0 98 2.8 1 elseif fDLP == 3 DLP := %00000011 ' 3 44 4.9 42 4.8 1 elseif fDLP == 4 DLP := %00000100 ' 4 21 8.5 20 8.3 1 elseif fDLP == 5 DLP := %00000101 ' 5 10 13.8 10 13.4 1 elseif fDLP == 6 DLP := %00000110 ' 6 5 19.0 5 18.6 1 elseif fDLP == 7 DLP := %00000111 ' 7 RESERVED RESERVED 8 PowerMgmt := %00000001 ' X gyro as clock source SampleRate := %00000001 ' 500 Hz ComputeTimes gyroSCL := 1 << SCL 'save I2C pins gyroSDA := 1 << SDA Status := Cog := cognew(@Start_Sensors, @rx) + 1 CalibrateAccel 'CalibrateGyro 'CalibrateMag ' // Start w/ Full User Init Settings PUB StartA( SCL, SDA, aFS, gFS, fDLP, PM ) : Status if aFS == 0 AccelFS := %00000000 elseif aFS == 1 AccelFS := %00001000 elseif aFS == 2 AccelFS := %00010000 elseif aFS == 3 AccelFS := %00011000 if gFS == 0 GyroFS := %00000000 elseif gFS == 1 GyroFS := %00001000 elseif gFS == 2 GyroFS := %00010000 elseif gFS == 3 GyroFS := %00011000 '| DLPF_CFG | Accelerometer | Gyroscope | if fDLP == 0 ' Bw (Hz) Delay (ms) Bw (Hz) Delay (ms) FS (Khz) DLP := %00000000 ' 0 260 0 256 0.98 8 elseif fDLP == 1 DLP := %00000001 ' 1 184 2.0 188 1.9 1 elseif fDLP == 2 DLP := %00000010 ' 2 94 3.0 98 2.8 1 elseif fDLP == 3 DLP := %00000011 ' 3 44 4.9 42 4.8 1 elseif fDLP == 4 DLP := %00000100 ' 4 21 8.5 20 8.3 1 elseif fDLP == 5 DLP := %00000101 ' 5 10 13.8 10 13.4 1 elseif fDLP == 6 DLP := %00000110 ' 6 5 19.0 5 18.6 1 elseif fDLP == 7 DLP := %00000111 ' 7 RESERVED RESERVED 8 if PM == 0 PowerMgmt := %00000000 ' Internal 8MHz oscillator elseif PM == 1 PowerMgmt := %00000001 ' PLL with X axis gyroscope reference elseif PM == 2 PowerMgmt := %00000010 ' PLL with Y axis gyroscope reference elseif PM == 3 PowerMgmt := %00000011 ' PLL with Z axis gyroscope reference elseif PM == 4 PowerMgmt := %00000100 ' PLL with external 32.768kHz reference elseif PM == 5 PowerMgmt := %00000101 ' PLL with external 19.2MHz referenc elseif PM == 6 PowerMgmt := %00000110 ' Reserved elseif PM == 7 PowerMgmt := %00000111 ' Stops the clock and keeps the timing generator in reset ' *** Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) ' // 0 threw 255 SampleRate := %00000001 ' 500 Hz Sample Rate, %00000000 = 1000 Hz Sample rate ComputeTimes gyroSCL := 1 << SCL 'save I2C pins gyroSDA := 1 << SDA Status := Cog := cognew(@Start_Sensors, @rx) + 1 CalibrateAccel 'CalibrateGyro 'CalibrateMag ' // Start Basic, No User Init PUB StartX( SCL, SDA ) : Status AccelFS := %00010000 ' AFS2 GyroFS := %00011000 ' FS3 DLP := %00000011 ' 40 Hz PowerMgmt := %00000001 ' X gyro as clock source SampleRate := %00000001 ' 500 Hz ComputeTimes gyroSCL := 1 << SCL 'save I2C pins gyroSDA := 1 << SDA Status := Cog := cognew(@Start_Sensors, @rx) + 1 CalibrateAccel 'CalibrateGyro 'CalibrateMag PUB Stop if Cog cogstop(Cog~ - 1) '********************** ' Accessors '********************** PUB GetTemp ' Temperature return temp 'Pub GetTempF ' Temp Deg F 'Pub GetTempC ' Temp Deg C PUB GetRX ' Accel X - Zero Offset return rx - x0 PUB GetRY ' Accel Y - Zero Offset return ry - y0 PUB GetRZ ' Accel Z - Zero Offset return rz - z0 PUB GetAX ' Gyro X - Zero Offset return ax - a0 PUB GetAY ' Gyro Y - Zero Offset return ay - b0 PUB GetAZ ' Gyrp Z - Zero Offset return az - c0 PUB GetARX ' Pitch Angle return arx PUB GetARY ' Roll Angle return ary Pub GetAccelOffsetX ' Accelerometer Zero Offset X return x0 Pub GetAccelOffsetY ' Accelerometer Zero Offset Y return y0 Pub GetAccelOffsetZ ' Accelerometer Zero Offset Z return z0 Pub GetGyroOffsetX ' Gyroscope Zero Offset X return a0 Pub GetGyroOffsetY ' Gyroscope Zero Offset Y return b0 Pub GetGyroOffsetZ ' Gyroscope Zero Offset Z return c0 Pub CalAccel ' Calibrate Accelerometer CalibrateAccel Pub CalGyro ' Calibrate Gyroscope CalibrateGyro Pub GetChipID return cID Pub GetMagID return mID Pub GetMX return mx Pub GetMY return my Pub GetMZ return mz PRI computeTimes '' Set up timing constants in assembly ' (Done this way to avoid overflow) i2cDataSet := ((clkfreq / 10000) * 350) / 100000 ' Data setup time - 350ns (400KHz) i2cClkLow := ((clkfreq / 10000) * 1300) / 100000 ' Clock low time - 1300ns (400KHz) i2cClkHigh := ((clkfreq / 10000) * 600) / 100000 ' Clock high time - 600ns (400KHz) i2cPause := clkfreq / 100000 ' Pause between checks for operations PRI CalibrateAccel | tc, xc, yc, zc, dr x0 := 0 ' Initialize offsets y0 := 0 z0 := 0 'wait 1/2 second for the body to stop moving waitcnt( constant(80_000_000 / 2) + cnt ) 'Find the zero points of the 3 axis by reading for ~1 sec and averaging the results xc := 0 yc := 0 zc := 0 repeat 256 xc += rx yc += ry zc += rz waitcnt( constant(80_000_000/192) + cnt ) 'Perform rounding if( xc > 0 ) xc += 128 elseif( xc < 0 ) xc -= 128 if( yc > 0 ) yc += 128 elseif( yc < 0 ) yc -= 128 if( zc > 0 ) zc += 128 elseif( zc < 0 ) zc -= 128 x0 := xc / 256 y0 := yc / 256 z0 := zc / 256 PRI CalibrateGyro | tc, xc, yc, zc, dr a0 := 0 ' Initialize offsets b0 := 0 c0 := 0 'wait 1/2 second for the body to stop moving waitcnt( constant(80_000_000 / 2) + cnt ) 'Find the zero points of the 3 axis by reading for ~1 sec and averaging the results xc := 0 yc := 0 zc := 0 repeat 256 xc += ax yc += ay zc += az waitcnt( constant(80_000_000/192) + cnt ) 'Perform rounding if( xc > 0 ) xc += 128 elseif( xc < 0 ) xc -= 128 if( yc > 0 ) yc += 128 elseif( yc < 0 ) yc -= 128 if( zc > 0 ) zc += 128 elseif( zc < 0 ) zc -= 128 a0 := xc / 256 b0 := yc / 256 c0 := zc / 256 Pri CalibrateMag | tc, xc, yc, zc, dr d0 := 0 ' Initialize offsets e0 := 0 f0 := 0 'wait 1/2 second for the body to stop moving waitcnt( constant(80_000_000 / 2) + cnt ) 'Find the zero points of the 3 axis by reading for ~1 sec and averaging the results xc := 0 yc := 0 zc := 0 repeat 256 xc += mX yc += mY zc += mZ waitcnt( constant(80_000_000/192) + cnt ) 'Perform rounding if( xc > 0 ) xc += 128 elseif( xc < 0 ) xc -= 128 if( yc > 0 ) yc += 128 elseif( yc < 0 ) yc -= 128 if( zc > 0 ) zc += 128 elseif( zc < 0 ) zc -= 128 d0 := xc / 256 e0 := yc / 256 f0 := zc / 256 DAT org 0 Start_Sensors ' --------- Debugger Kernel add this at Entry (Addr 0) --------- ' long $34FC1202,$6CE81201,$83C120B,$8BC0E0A,$E87C0E03,$8BC0E0A ' long $EC7C0E05,$A0BC1207,$5C7C0003,$5C7C0003,$7FFC,$7FF8 ' -------------------------------------------------------------- mov p1, par ' Get data pointer mov prX, p1 ' Store the pointer to the rx var in HUB RAM add p1, #4 mov prY, p1 ' Store the pointer to the ry var in HUB RAM add p1, #4 mov prZ, p1 ' Store the pointer to the rz var in HUB RAM add p1, #4 mov pT, p1 ' Store the pointer to the temp var in HUB RAM add p1, #4 mov paX, p1 ' Store the pointer to the ax var in HUB RAM add p1, #4 mov paY, p1 ' Store the pointer to the ay var in HUB RAM add p1, #4 mov paZ, p1 ' Store the pointer to the az var in HUB RAM add p1, #4 mov paRX, p1 ' Store the pointer to the arx var in HUB RAM add p1, #4 mov paRY, p1 ' Store the pointer to the ary var in HUB RAM add p1, #4 mov pmX, p1 ' Store the pointer to the ax var in HUB RAM add p1, #4 mov pmY, p1 ' Store the pointer to the ay var in HUB RAM add p1, #4 mov pmZ, p1 ' Store the pointer to the az var in HUB RAM add p1, #4 mov pcID, p1 ' Store the pointer to the ary var in HUB RAM add p1, #4 mov pmID, p1 ' Store the pointer to the ary var in HUB RAM mov i2cTemp,i2cPause add i2cTemp,CNT ' Wait 10us before starting waitcnt i2cTemp,#0 call #SetConfig mov loopCount, CNT add loopCount, loopDelay '------------------------------------------------------------------------------ ' Main loop ' loopDelay defined in data section ' Nominally set to CLK_FREQ/200 give 200hz update rate, but the update takes less than ' 500us, so the delay could potentially be set to give an update rate as high as 2000hz ' :loop call #MPUReadValues call #MPUComputeDrift call #ComputeAngles call #MPUGetChipID ' = 0x68 = Correct call #MPUGetMagID ' = 0x48 = Correct 'call #ConfigMag ' // *** FINAL PROBLEM HERE!!! Cannot get AK8975 to go into Single-Read Mode. call #MPUGetMagX call #MPUGetMagY call #MPUGetMagZ wrlong iT, pT subs irX, drift wrlong irX, prX subs irY, drift wrlong irY, prY subs irZ, drift wrlong irZ, prZ wrlong iaX, paX wrlong iaY, paY wrlong iaZ, paZ 'Sign extend the 15th bit test iaRX, i2cWordReadMask wc muxc iaRX, i2cWordMask 'Sign extend the 15th bit test iaRY, i2cWordReadMask wc muxc iaRY, i2cWordMask wrlong iaRX, paRX wrlong iaRY, paRY wrlong iMX, pMX wrlong iMY, pMY wrlong iMZ, pMZ 'mov icID, #$68 ' (Not Calling MPUGetChipID or MPUGetMagID) 'mov imID, #$48 wrlong icID, pcID wrlong imID, pmID waitcnt loopCount, loopDelay jmp #:loop '------------------------------------------------------------------------------ ' MPUReadValues ' ' Starting at the ACCEL_X data register, read in the 3 accel values, ' the temperature, and the 3 gyro values, as these are held in ' sequential register locations. ' MPUReadValues mov i2cSDA, gyroSDA 'Use gyro SDA,SCL mov i2cSCL, gyroSCL mov i2cAddr, #59 ' Address of ACCEL_XOUT_H mov i2cDevID, #%11010000 ' Device ID of the MPU call #StartRead ' Tell the I2C device we're starting mov i2cMask, i2cWordReadMask test i2cTestCarry, #0 wc ' Clear the carry flag to make reads auto-increment call #i2cRead call #i2cRead 'Sign extend the 15th bit test i2cData, i2cWordReadMask wc muxc i2cData, i2cWordMask mov iaX, i2cData mov i2cMask, i2cWordReadMask test i2cTestCarry, #0 wc ' Clear the carry flag to make reads auto-increment call #i2cRead call #i2cRead 'Sign extend the 15th bit test i2cData, i2cWordReadMask wc muxc i2cData, i2cWordMask mov iaY, i2cData mov i2cMask, i2cWordReadMask test i2cTestCarry, #0 wc ' Clear the carry flag to make reads auto-increment call #i2cRead call #i2cRead 'Sign extend the 15th bit test i2cData, i2cWordReadMask wc muxc i2cData, i2cWordMask mov iaZ, i2cData mov i2cMask, i2cWordReadMask test i2cTestCarry, #0 wc ' Clear the carry flag to make reads auto-increment call #i2cRead call #i2cRead test i2cData, i2cWordReadMask wc muxc i2cData, i2cWordMask mov iT, i2cData mov i2cMask, i2cWordReadMask test i2cTestCarry, #0 wc ' Clear the carry flag to make reads auto-increment call #i2cRead call #i2cRead 'Sign extend the 15th bit test i2cData, i2cWordReadMask wc muxc i2cData, i2cWordMask mov irX, i2cData mov i2cMask, i2cWordReadMask test i2cTestCarry, #0 wc ' Clear the carry flag to make reads auto-increment call #i2cRead call #i2cRead 'Sign extend the 15th bit test i2cData, i2cWordReadMask wc muxc i2cData, i2cWordMask mov irY, i2cData mov i2cMask, i2cWordReadMask test i2cTestCarry, #0 wc ' Clear the carry flag to make reads auto-increment call #i2cRead test i2cTestCarry, #1 wc ' Set the carry flag to tell it we're done call #i2cRead 'Sign extend the 15th bit test i2cData, i2cWordReadMask wc muxc i2cData, i2cWordMask mov irZ, i2cData call #i2cStop MPUReadValues_Ret ret '------------------------------------------------------------------------------ MPUGetChipID mov i2cSDA, gyroSDA 'Use gyro SDA,SCL mov i2cSCL, gyroSCL mov i2cAddr, #117 ' Address of ACCEL_XOUT_H mov i2cDevID, #%11010000 ' Device ID of the MPU call #StartRead ' Tell the I2C device we're starting mov i2cMask, i2cWordReadMask test i2cTestCarry, #1 wc ' Clear the carry flag to make reads auto-increment call #i2cRead ' // Single Byte Read; Shift Right 8 Bits shr i2cData, #8 mov icID, i2cData ' // Fixed = 0x68 call #i2cStop MPUGetChipID_Ret ret '------------------------------------------------------------------------------ MPUGetMagID mov i2cSDA, gyroSDA 'Use gyro SDA,SCL mov i2cSCL, gyroSCL mov i2cAddr, #00 mov i2cDevID, #%00011000 call #StartRead ' Tell the I2C device we're starting mov i2cMask, i2cWordReadMask test i2cTestCarry, #1 wc ' Clear the carry flag to make reads auto-increment call #i2cRead ' // Single Byte Read; Shift Right 8 Bits shr i2cData, #8 mov imID, i2cData ' // Fixed = 0x48 call #i2cStop MPUGetMagID_Ret ret '------------------------------------------------------------------------------ MPUGetMagX 'jmpret MPUGetMagX, #MPUMag ' // *** FINAL PROBLEM HERE!!! Cannot get AK8975 to go into Single-Read Mode. mov i2cSDA, gyroSDA 'Use gyro SDA,SCL mov i2cSCL, gyroSCL mov i2cAddr, #03 mov i2cDevID, #%00011000 call #StartRead ' Tell the I2C device we're starting mov i2cMask, i2cWordReadMask test i2cTestCarry, #0 wc call #i2cRead test i2cTestCarry, #1 wc ' Clear the carry flag to make reads auto-increment call #i2cRead 'Sign extend the 15th bit test i2cData, i2cWordReadMask wc muxc i2cData, i2cWordMask mov imX, i2cData call #i2cStop MPUGetMagX_Ret ret '------------------------------------------------------------------------------ MPUGetMagY 'jmpret MPUGetMagY, #MPUMag ' // *** FINAL PROBLEM HERE!!! Cannot get AK8975 to go into Single-Read Mode. mov i2cSDA, gyroSDA 'Use gyro SDA,SCL mov i2cSCL, gyroSCL mov i2cAddr, #05 mov i2cDevID, #%00011000 call #StartRead ' Tell the I2C device we're starting mov i2cMask, i2cWordReadMask test i2cTestCarry, #0 wc call #i2cRead test i2cTestCarry, #1 wc ' Clear the carry flag to make reads auto-increment call #i2cRead 'Sign extend the 15th bit test i2cData, i2cWordReadMask wc muxc i2cData, i2cWordMask mov imY, i2cData call #i2cStop MPUGetMagY_Ret ret '------------------------------------------------------------------------------ MPUGetMagZ 'jmpret MPUGetMagZ, #MPUMag ' // *** FINAL PROBLEM HERE!!! Cannot get AK8975 to go into Single-Read Mode. mov i2cSDA, gyroSDA 'Use gyro SDA,SCL mov i2cSCL, gyroSCL mov i2cAddr, #07 mov i2cDevID, #%00011000 call #StartRead ' Tell the I2C device we're starting mov i2cMask, i2cWordReadMask test i2cTestCarry, #0 wc call #i2cRead test i2cTestCarry, #1 wc ' Clear the carry flag to make reads auto-increment call #i2cRead 'Sign extend the 15th bit test i2cData, i2cWordReadMask wc muxc i2cData, i2cWordMask mov imZ, i2cData call #i2cStop MPUGetMagZ_Ret ret '------------------------------------------------------------------------------ ' // *** FINAL PROBLEM HERE!!! Cannot get AK8975 to go into Single-Read Mode. { i2c.Start(SCL) i2c.Write(SCL, MagAddrW) ' AK8975 Write Address i2c.Write(SCL, $0A) ' CTRL Register i2c.Write(SCL, %00000001) ' Set config $0A to %00000001 to turn on the device. i2c.Stop(SCL) } ConfigMag mov i2cSDA, gyroSDA 'Use gyro SDA,SCL mov i2cSCL, gyroSCL call #i2cReset mov i2cDevID, #%00011000 ' MagAddrW mov i2cMask, #%10000000 call #i2cWrite mov i2cAddr, #10 ' $0A mov i2cMask, #%10000000 call #i2cWrite mov i2cValue, #%00000001 ' %00000001 mov i2cMask, #%10000000 call #i2cWrite call #i2cStop ConfigMag_Ret ret '------------------------------------------------------------------------------ ' Compute drift - for my gyro (Jason's ITG-3200) '(Temp + 15000) / 100 = drift '------------------------------------------------------------------------------ MPUComputeDrift mov drift, iT ' Start with the temperature reading add drift, tempOffset ' Offset it by 15,000 ' divide drift by 100 mov divisor, #100 mov dividend, drift test dividend, i2cWordReadMask wc muxc signbit, #1 ' record the sign of the original value abs dividend, dividend mov divCounter, #10 shl divisor, divCounter mov resultShifted, #1 shl resultShifted, divCounter add divCounter, #1 mov drift, #0 :divLoop cmp dividend, divisor wc if_nc add drift, resultShifted if_nc sub dividend, divisor shr resultShifted, #1 shr divisor, #1 djnz divCounter, #:divLoop test signbit, #1 wc negc drift, drift MPUComputeDrift_Ret ret '------------------------------------------------------------------------------ ComputeAngles mov cx, iaZ mov cy, iaX call #cordic mov iaRX, ca mov cx, iaZ mov cy, iaY call #cordic mov iaRY, ca ComputeAngles_ret ret '------------------------------------------------------------------------------ ' SetConfig ' ' See MPU-6000/6050 Register Map document for register addresses and ' valid settings ' SetConfig mov i2cSDA, gyroSDA 'Use gyro SDA,SCL mov i2cSCL, gyroSCL call #i2cReset 'Reset i2c :MPUSetConfig mov i2cDevID, #%11010000 'Device ID for the MPU-6000/6050 ' // Power Management mov i2cAddr, #107 'Set PWR_MGMT_1 register bit 0 to choose mov i2cValue, PowerMgmt ' X gyro as clock source ' call #i2cWriteRegisterByte ' // I2C Master Disabled mov i2cAddr, #106 mov i2cValue, #%00000000 call #i2cWriteRegisterByte ' // Digital Low Pass Filter _ Config mov i2cAddr, #26 mov i2cValue, DLP 'Set DLPF_CONFIG to 3 for 40Hz bandwidth call #i2cWriteRegisterByte ' // Sample Rate Divider mov i2cAddr, #25 'SMPLRT_DIV = 1 => 1khz/(1+0) = 1000hz sample rate mov i2cValue, SampleRate call #i2cWriteRegisterByte ' // Gyro _ Config mov i2cAddr, #27 'GYRO_CONFIG register, set FS_SEL bits to 3 gives a mov i2cValue, GyroFS ' full scale range of +-2000 deg/sec call #i2cWriteRegisterByte ' // Accel _ Config mov i2cAddr, #28 'Set ACCEL_CONFIG register AFS_SEL bits to 2 mov i2cValue, AccelFS ' sets +-8g full scale range call #i2cWriteRegisterByte 'ACCEL_HPF is zero which turns off high-pass filtering ' // I2C Bypass Enabled mov i2cAddr, #55 mov i2cValue, #%00000010 call #i2cWriteRegisterByte ' // Enable Magnetometer for Reading MPUMag mov i2cSDA, gyroSDA 'Use gyro SDA,SCL mov i2cSCL, gyroSCL call #i2cReset 'Reset i2c mov i2cDevID, #%00011000 'Device ID for the MPU-Mag mov i2cAddr, #10 '#$0A mov i2cValue, #%00000001 ' Set to Single-Measurement Mode call #i2cWriteRegisterByte { ' // Extra Config Settings: mov i2cAddr, #Address mov i2cValue, #%BIN_Value call #i2cWriteRegisterByte } SetConfig_Ret ret '------------------------------------------------------------------------------ StartRead call #i2cStart mov i2cData, i2cDevID mov i2cMask, #%10000000 call #i2cWrite mov i2cData, i2cAddr mov i2cMask,#%10000000 call #i2cWrite call #i2cStart mov i2cData, i2cDevID or i2cData, #1 mov i2cMask, #%10000000 call #i2cWrite StartRead_Ret ret '------------------------------------------------------------------------------ i2cWriteRegisterByte call #i2cStart mov i2cData, i2cDevID mov i2cMask,#%10000000 call #i2cWrite mov i2cTime,i2cClkLow add i2cTime,cnt ' Allow for minimum SCL low waitcnt i2cTime, #0 mov i2cData, i2cAddr mov i2cMask,#%10000000 call #i2cWrite mov i2cTime,i2cClkLow add i2cTime,cnt ' Allow for minimum SCL low waitcnt i2cTime, #0 mov i2cData, i2cValue mov i2cMask,#%10000000 call #i2cWrite call #i2cStop i2cWriteRegisterByte_Ret ret '------------------------------------------------------------------------------ '' Low level I2C routines. These are designed to work either with a standard I2C bus '' (with pullups on both SCL and SDA) or the Propellor Demo Board (with a pullup only '' on SDA). Timing can be set by the caller to 100KHz or 400KHz. '------------------------------------------------------------------------------ '' Do I2C Reset Sequence. Clock up to 9 cycles. Look for SDA high while SCL '' is high. Device should respond to next Start Sequence. Leave SCL high. i2cReset andn dira,i2cSDA ' Pullup drive SDA high mov i2cBitCnt,#9 ' Number of clock cycles mov i2cTime,i2cClkLow add i2cTime,cnt ' Allow for minimum SCL low :i2cResetClk andn outa,i2cSCL ' Active drive SCL low or dira,i2cSCL waitcnt i2cTime,i2cClkHigh or outa,i2cSCL ' Active drive SCL high or dira,i2cSCL andn dira,i2cSCL ' Pullup drive SCL high waitcnt i2cTime,i2cClkLow ' Allow minimum SCL high test i2cSDA,ina wz ' Stop if SDA is high if_z djnz i2cBitCnt,#:i2cResetClk ' Stop after 9 cycles i2cReset_ret ret ' Should be ready for Start '------------------------------------------------------------------------------ '' Do I2C Start Sequence. This assumes that SDA is a floating input and '' SCL is also floating, but may have to be actively driven high and low. '' The start sequence is where SDA goes from HIGH to LOW while SCL is HIGH. i2cStart or outa,i2cSCL ' Active drive SCL high or dira,i2cSCL or outa,i2cSDA ' Active drive SDA high or dira,i2cSDA mov i2cTime,i2cClkHigh add i2cTime,cnt ' Allow for bus free time waitcnt i2cTime,i2cClkLow andn outa,i2cSDA ' Active drive SDA low waitcnt i2cTime,#0 andn outa,i2cSCL ' Active drive SCL low i2cStart_ret ret '------------------------------------------------------------------------------ '' Do I2C Stop Sequence. This assumes that SCL is low and SDA is indeterminant. '' The stop sequence is where SDA goes from LOW to HIGH while SCL is HIGH. '' i2cStart must have been called prior to calling this routine for initialization. '' The state of the (c) flag is maintained so a write error can be reported. i2cStop or outa,i2cSCL ' Active drive SCL high mov i2cTime,i2cClkHigh add i2cTime,cnt ' Wait for minimum clock low waitcnt i2cTime,i2cClkLow or outa,i2cSDA ' Active drive SDA high waitcnt i2cTime,i2cClkLow andn dira,i2cSCL ' Pullup drive SCL high waitcnt i2cTime,i2cClkLow ' Wait for minimum setup time andn dira,i2cSDA ' Pullup drive SDA high waitcnt i2cTime,#0 ' Allow for bus free time i2cStop_ret ret '------------------------------------------------------------------------------ '' Write I2C data. This assumes that i2cStart has been called and that SCL is low, '' SDA is indeterminant. The (c) flag will be set on exit from ACK/NAK with ACK == false '' and NAK == true. Bytes are handled in "little-endian" order so these routines can be '' used with words or longs although the bits are in msb..lsb order. i2cWrite mov i2cBitCnt,#8 mov i2cTime,i2cClkLow add i2cTime,cnt ' Wait for minimum SCL low :i2cWriteBit waitcnt i2cTime,i2cDataSet test i2cData,i2cMask wz if_z or dira,i2cSDA ' Copy data bit to SDA if_nz andn dira,i2cSDA waitcnt i2cTime,i2cClkHigh ' Wait for minimum setup time or outa,i2cSCL ' Active drive SCL high waitcnt i2cTime,i2cClkLow andn outa,i2cSCL ' Active drive SCL low ror i2cMask,#1 ' Go do next bit if not done djnz i2cBitCnt,#:i2cWriteBit andn dira,i2cSDA ' Switch SDA to input and waitcnt i2cTime,i2cClkHigh ' wait for minimum SCL low or outa,i2cSCL ' Active drive SCL high waitcnt i2cTime,i2cClkLow ' Wait for minimum high time test i2cSDA,ina wc ' Sample SDA (ACK/NAK) then andn outa,i2cSCL ' active drive SCL low andn outa,i2cSDA ' active drive SDA low or dira,i2cSDA ' Leave SDA low rol i2cMask,#16 ' Prepare for multibyte write waitcnt i2cTime,#0 ' Wait for minimum low time i2cWrite_ret ret '------------------------------------------------------------------------------ '' Read I2C data. This assumes that i2cStart has been called and that SCL is low, '' SDA is indeterminant. ACK/NAK will be copied from the (c) flag on entry with '' ACK == low and NAK == high. Bytes are handled in "little-endian" order so these '' routines can be used with words or longs although the bits are in msb..lsb order. i2cRead mov i2cBitCnt,#8 andn dira,i2cSDA ' Make sure SDA is set to input mov i2cTime,i2cClkLow add i2cTime,cnt ' Wait for minimum SCL low :i2cReadBit waitcnt i2cTime,i2cClkHigh or outa,i2cSCL ' Active drive SCL high waitcnt i2cTime,i2cClkLow ' Wait for minimum clock high test i2cSDA,ina wz ' Sample SDA for data bits andn outa,i2cSCL ' Active drive SCL low if_nz or i2cData,i2cMask ' Accumulate data bits if_z andn i2cData,i2cMask ror i2cMask,#1 ' Shift the bit mask and djnz i2cBitCnt,#:i2cReadBit ' continue until done waitcnt i2cTime,i2cDataSet ' Wait for end of SCL low if_c or outa,i2cSDA ' Copy the ACK/NAK bit to SDA if_nc andn outa,i2cSDA or dira,i2cSDA ' Make sure SDA is set to output waitcnt i2cTime,i2cClkHigh ' Wait for minimum setup time or outa,i2cSCL ' Active drive SCL high waitcnt i2cTime,i2cClkLow ' Wait for minimum clock high andn outa,i2cSCL ' Active drive SCL low andn outa,i2cSDA ' Leave SDA low waitcnt i2cTime,#0 ' Wait for minimum low time i2cRead_ret ret { ComputeAngles mov cx, iaZ mov cy, iaX call #cordic mov iaRX, ca mov cx, iaZ mov cy, iaY call #cordic mov iaRY, ca ComputeAngles_ret ret } '------------------------------------------------------------------------------ '' Perform CORDIC cartesian-to-polar conversion ''Input = cx(x) and cy(x) ''Output = cx(ro) and ca(theta) cordic abs cx,cx wc if_c neg cy,cy mov ca,#0 rcr ca,#1 movs :lookup,#cordicTable mov t1,#0 mov t2,#20 :loop mov dx,cy wc sar dx,t1 mov dy,cx sar dy,t1 sumc cx,dx sumnc cy,dy :lookup sumc ca,cordicTable add :lookup,#1 add t1,#1 djnz t2,#:loop shr ca, #16 cordic_ret ret cordicTable long $20000000 long $12E4051E long $09FB385B long $051111D4 long $028B0D43 long $0145D7E1 long $00A2F61E long $00517C55 long $0028BE53 long $00145F2F long $000A2F98 long $000517CC long $00028BE6 long $000145F3 long $0000A2FA long $0000517D long $000028BE long $0000145F long $00000A30 long $00000518 dx long 0 dy long 0 cx long 0 cy long 0 ca long 0 t1 long 0 t2 long 0 MagInit long 0 '' Variables for the gyro routines p1 long 0 pT long 0 ' Pointer to Temperature in hub ram prX long 0 ' Pointer to X rotation in hub ram prY long 0 ' Pointer to Y rotation in hub ram prZ long 0 ' Pointer to Z rotation in hub ram paX long 0 ' Pointer to X accel in hub ram paY long 0 ' Pointer to Y accel in hub ram paZ long 0 ' Pointer to Z accel in hub ram paRX long 0 ' Pointer to X accel angle in hub ram paRY long 0 ' Pointer to Y accel angle in hub ram pmX long 0 pmY long 0 pmZ long 0 pcID long 0 pmID long 0 iT long 0 ' Interim temperature value irX long 0 ' Interim rX value irY long 0 ' Interim rY value - These values are temp storage before drift compensation irZ long 0 ' Interim rZ value iaX long 0 ' Interim aX value iaY long 0 ' Interim aY value iaZ long 0 ' Interim aZ value iaRX long 0 ' Interim aX value iaRY long 0 ' Interim aY value imX long 0 imY long 0 imZ long 0 icID long 0 imID long 0 i2cWordReadMask long %10000000_00000000 i2cWordMask long $ffff0000 loopDelay long 80_000_000 / 200 loopCount long 0 '' Variables for dealing with drift / division tempOffset long 15000 drift long 0 divisor long 0 dividend long 0 resultShifted long 0 signbit long 0 divCounter long 0 '' Variables for i2c routines i2cTemp long 0 i2cCount long 0 i2cValue long 0 i2cDevID long 0 i2cAddr long 0 i2cDataSet long 0 ' Minumum data setup time (ticks) i2cClkLow long 0 ' Minimum clock low time (ticks) i2cClkHigh long 0 ' Minimum clock high time (ticks) i2cPause long 0 ' Pause before re-fetching next operation i2cTestCarry long 1 ' Used for setting the carry flag '' Local variables for low level I2C routines ' // MPU-6050 User Set Init Settings GyroFS long %00000000 AccelFS long %00000000 PowerMgmt long %00000000 SampleRate long %00000000 DLP long %00000000 gyroSCL long 0 ' Bit mask for SCL gyroSDA long 0 ' Bit mask for SDA i2cSCL long 0 ' Bit mask for SCL i2cSDA long 0 ' Bit mask for SDA i2cTime long 0 ' Used for timekeeping i2cData long 0 ' Data to be transmitted / received i2cMask long 0 ' Bit mask for bit to be tx / rx i2cBitCnt long 0 ' Number of bits to tx / rx FIT 496:loop call #MPUReadValues call #MPUComputeDrift call #ComputeAngles call #MPUGetChipID ' = 0x68 = Correct call #MPUGetMagID ' = 0x48 = Correct ' // if ChipID <> $68 ' // jmp #:loop mov MagTmp, icID ' MagTmp = icID sub MagTmp, #$68 ' MagTmp - $68 (IF CID = $68 will = 0) tjnz MagTmp, #:loop ' if MagTmp <> 0 jump to :loop 'call #ConfigMag ' // *** FINAL PROBLEM HERE!!! Cannot get AK8975 to go into Single-Read Mode. call #MPUGetMagX call #MPUGetMagY call #MPUGetMagZ wrlong iT, pT subs irX, drift wrlong irX, prX subs irY, drift wrlong irY, prY subs irZ, drift wrlong irZ, prZ wrlong iaX, paX wrlong iaY, paY wrlong iaZ, paZ 'Sign extend the 15th bit test iaRX, i2cWordReadMask wc muxc iaRX, i2cWordMask 'Sign extend the 15th bit test iaRY, i2cWordReadMask wc muxc iaRY, i2cWordMask wrlong iaRX, paRX wrlong iaRY, paRY wrlong iMX, pMX wrlong iMY, pMY wrlong iMZ, pMZ 'mov icID, #$68 ' (Not Calling MPUGetChipID or MPUGetMagID) 'mov imID, #$48 wrlong icID, pcID wrlong imID, pmID waitcnt loopCount, loopDelay jmp #:loopAttached is a demo program that I've been working on.
MPU-9150 demo 2 - Archive.zip
Wow! your mag code works! Everything is giving values but I dont understand your Accel results, they appear to be more like Angle values, acceleration has nothing to do with their value. And there is no Zero point.
Your code is difficult to understand.
How are you doing the Temperature calculations?!?! It gets Read along with gyro & accel, but then "L3 := L3 / 340 + 35" does something (and the only existence of "L3" in the project is that line, but is part of the temp calc? how?)
I can still use the angle calculations, but I do also need the g-force, and I cannot obtain this from your code.
Thanks,
Zack
Values are being created out of thin air, that respond fairly accurately (temperature). Math is being done and visibly not used but behind the vail of magic it makes the temperature work.
I also noticed that your MPU Address is Wrong, but wont work with the correct address.
Incorrect: MPU = %0110_1000 *** Works
Correct: MPU = %1101_0000 *** Don't Work
See here my comments:
CON _clkmode = xtal1 + pll16x _xinfreq = 5_000_000 [B]SDA_PIN = 10 SCL_PIN = 11[/B] Bitrate = 400_000 VAR long Stack[60] long Acc_X, Acc_Y, Acc_Z, Temp, Gyro_X, Gyro_Y, Gyro_Z long Mag_X, Mag_Y, Mag_Z word word_buffer[7] byte ASAX, ASAY, ASAZ ' byte buffer[14] OBJ I2C : "I2C Spin driver v1.2" ' I2C : "I2C PASM driver v1.4" FDS : "FullDuplexSerial" f : "FloatMath" ' 0 Cog fS : "FloatString" ' 0 Cog PUB Demo | AccelX, AccelY, AccelZ FDS.start(31,30,0,115_200) waitcnt(cnt + clkfreq) start repeat [I] ' // Convert Accel Value to G-Force ' G-Force = Accel[x] / 2,048 (AFS-3) AccelX := f.fDiv(f.fFloat(Acc_X), 2048.0) AccelY := f.fDiv(f.fFloat(Acc_Y), 2048.0) AccelZ := f.fDiv(f.fFloat(Acc_Z), 2048.0)[/I] waitcnt((clkfreq / 50) + cnt) ' // Raw Accel FDS.tx($00) fds.str(string("Acc_X",$09)) fds.dec(Acc_X) fds.str(string($0D,"Acc_Y",$09)) fds.dec(Acc_Y) fds.str(string($0D,"Acc_Z",$09)) fds.dec(Acc_Z) [I] ' // G-Force fds.str(string($0D, $0D,"G-Force_X",$09)) fds.str(fs.floattostring(AccelX)) fds.str(string($0D,"G-Force_Y",$09)) fds.str(fs.floattostring(AccelY)) fds.str(string($0D,"G-Force_Z",$09)) fds.str(fs.floattostring(AccelZ)) [/I] ' // Raw Gyro fds.str(string($0D,$0D,"Gyro_X",$09)) fds.dec(Gyro_X) fds.str(string($0D,"Gyro_Y",$09)) fds.dec(Gyro_Y) fds.str(string($0D,"Gyro_Z",$09)) fds.dec(Gyro_Z) ' // Mag uT FDS.str(string($0D,$0D,"Mag_X",$09)) FDS.dec(Mag_X) FDS.str(string($0D,"Mag_Y",$09)) FDS.dec(Mag_Y) FDS.str(string($0D,"Mag_Z",$09)) FDS.dec(Mag_Z) fds.str(string($0D,$0D,"Temp",$09)) fds.dec(Temp) fds.str(string("°C",$09)) ' die temperature, about 8°C hotter than ambient? [B] ' // Finish Temp Calculation (Where was the Raw values converted to Deg C in the first place???)[/B] fds.dec(Temp * 9 / 5 + 32) fds.str(string("°F")) PUB start cognew(Main,@Stack) PUB Main | L0, L1, L2, [B]L3[/B], L4, L5, L6, i, cx, cy, cz I2C.start(SCL_pin,SDA_pin) ' I2C.start(SCL_pin,SDA_pin,Bitrate) waitcnt(clkfreq + cnt) [B] ' // MPU_SetConfig Start[/B] \I2C.write(I2C#MPU,$6B,$01) ' take out of sleep and use gyro-x as clock source \I2C.write(I2C#MPU,$37,$02) ' enable I2C bypass in order to communicate with the magnetometer at address $0C \I2C.write($0C,$0A,%1111) ' access the magnetometer Fuse ROM \I2C.read_page($0C,$10,@ASAX,3) ' Read the magnetometer adjustment values ($A2, $A4, $AF) ASAX := (ASAX - 128) / 2 ' equation from datasheet: Hadj = H x (((ASA - 128) x 0.5 / 128) + 1) ASAY := (ASAY - 128) / 2 ' microcontroller friendlier: Hadj = (H x (ASA - 128) / 2 + 1) / 128 + H ASAZ := (ASAZ - 128) / 2 ' These values never change so might as well do a partial calculation here \I2C.write(I2C#MPU,$19,$01) ' Sample rate divider (divide gyro_rate by 1 + x) \I2C.write(I2C#MPU,$1A,%00_000_110) ' Digital low-pass filtering (0 = 8KHz gyro_rate, !0 = 1KHz gyro_rate) \I2C.write(I2C#MPU,$1B,%000_11_000) ' Accelerometer sensitivity ±2000°/s \I2C.write(I2C#MPU,$1C,%000_11_000) ' Accelerometer sensitivity ±16g [B] ' // MPU_SetConfig End ' // Calibrate_gyro Start[/B] cx := 0 cy := 0 cz := 0 repeat 25 repeat until \I2C.read(I2C#MPU,$3A) & $01 \I2C.read_words(I2C#MPU,$43,@Word_buffer,3) cx += ~~Word_buffer[0] cy += ~~Word_buffer[1] cz += ~~Word_buffer[2] cx /= 25 cy /= 25 cz /= 25 [B] ' // Calibrate_gyro End[/B] repeat [B]' // Wait for Data, then Read 7 Bytes[/B] repeat until \I2C.read(I2C#MPU,$3A) & $01 ' wait for new data (based on sampling rate and digital low-pass filtering registers) \I2C.read_words(I2C#MPU,$3B,@Word_buffer,7) ' Read the samples into a word buffer [B] ' // Copy Buffer to L0[/B] repeat i from 0 to 6 ' Copy the word buffer into sign-extended local longs L0[i] := ~~Word_buffer[i] [B] ' // Temperature Calculation ???? *** Cannot Remove & ONLY used HERE [/B] [U] L3 := L3 / 340 + 35 ' Perform any required calculations on the local longs (also the place to do averaging)[/U] [B]' // Does Nothing ???? *** Can be Removed[/B] L4 -= cx L5 -= cy L6 -= cz [B] ' // Re-Copy Buffer (L0) to Acc_X (7 Bytes)[/B] repeat i from 0 to 6 Acc_X[i] := L0[i] ' Copy samples to the global vars ' // ???? L0~ L1~ L2~ ' // Read Magnetometer [B]Works 100%[/B] repeat 16 ' Gather 16 samples on each axis I2C.write($0C,$0A,%0001) ' Single_measurement repeat until I2C.read($0C,$02) & 1 ' Wait until DRDY bit is set I2C.read_page($0C,$03,@word_buffer,6) ' Read XOUT_L through ZOUT_H registers into buffer L0 += ~~word_buffer[0] L1 += ~~word_buffer[1] L2 += ~~word_buffer[2] L0 ~>= 4 ' Divide each sample by 16, keeping the same sign L1 ~>= 4 L2 ~>= 4 Mag_X := ((~~L0 * ASAX + 1) / 128 + ~~L0) ' Apply the adjustment values Mag_Y := ((~~L1 * ASAY + 1) / 128 + ~~L1) ' and store in the global vars Mag_Z := ((~~L2 * ASAZ + 1) / 128 + ~~L2) DAT {{ ┌──────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────┐ │ TERMS OF USE: MIT License │ ├──────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────┤ │Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation │ │files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, │ │modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software│ │is furnished to do so, subject to the following conditions: │ │ │ │The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.│ │ │ │THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE │ │WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR │ │COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, │ │ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. │ └──────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────┘ }}The MPU-9150 still remains a Mysterious Creature.
The code is still in a pretty rough shape, trying to figure out how I want to handle everything. The top section of the loop reads 14 bytes (as seven words) and stores them in a word buffer. The words are copied into L0 through L6 (local variables that I was experimenting with) in the same order that they were written into the buffer. L0, L1, L2 contain the sign-extended accelerometer values, L3 contains the temperature reading, L4, L5, L6 contain the gyro readings.
repeat i from 0 to 6 L0[i] := ~~Word_buffer[i]The formula for converting the temperature sensor reading into Celsius is described in section 4.18 of the MPU-9150 register map. After any necessary calculations are performed on the local variables, L0 through L6 are copied into the top VAR section, usingrepeat i from 0 to 6 Acc_X[i] := L0[i]It takes a bit of time to gather all of these readings and average, convert, and correct them, so I thought to run it in a separate cog. But having done that I needed to have someplace to perform those averages, conversions, and corrections before sharing them for other cogs to enjoy. Just a thought, and probably not a very good one as it complicates other matters, such as having the I2C routine controlled from inside this sampling routine rather than the main routine. And it makes it rather difficult to read.Here's an earlier version of the demo before I tried to get fancy ... you'll have to un-comment sections to get things to work, but it should be easier to see how the measurements are done.
MPU-9150 demo - Archive.zip
Wow you post fast. My I2C drivers work with 7-bit addressing, the drivers shift the address and append the read/write bit. I've noticed that most, but not all, datasheets give the device ID as 7 bits so thought to try to keep it simple. Oh well, for my drivers, $68 is the correct device ID for the accelerometer and gyro, and $0C is the correct address for the magnetometer.
That applies a correction factor to the gyro. cx, cy, and cz were zeroed, accumulated 25 samples worth of readings, and divided by 25. They're then subtracted from L4, L5, L6 (temporary registers holding Gyro_X, Gyro_Y, and Gyro_Z) to get a near-zero reading when not rotating. close, copies seven longs.
Lol yea, I am pretty on it with getting this thing working, I have spent many hours (now days) on just this sensor. I need it mostly of Heading Hold mode on my heli. I can get by without it & use the 6050 & Rate mode, but why should I if I have a 9150 sitting right in front of me? Or I could do 6050 + hmc5883 but I think that is a little excessive with a 9150 here.
G-Force = Accel[x] / 8,192 (AFS-1) ' x = 0, 1, 2 for X, Y, Z
Here is a snap of the code I just posted, all I added was the G-Force division in italics.
Level:
I am kinda starting to see how your code works, your methos to copy to memory locations similar to PASM. I didnt know that works in spin the same as pasm. Rather interesting of a method to code, will prob adopt some of it.
I have been trying to compare the results with other drivers. What I see and what I expect are two totally different things. Except when I use MPU-6050 Pasm Driver, then it reads close to 2000, 0, 0 when flat.
I still dont understand what your Accel outputs are, as they dont respond to acceleration.
PUB Main | L0, L1, L2, L3, L4, L5, L6, i, cx, cy, cz I2C.start(SCL_pin,SDA_pin) ' I2C.start(SCL_pin,SDA_pin,Bitrate) waitcnt(clkfreq + cnt) ' // MPU_SetConfig Start \I2C.write(I2C#MPU,$6B,$01) ' take out of sleep and use gyro-x as clock source \I2C.write(I2C#MPU,$37,$02) ' enable I2C bypass in order to communicate with the magnetometer at address $0C \I2C.write($0C,$0A,%1111) ' access the magnetometer Fuse ROM \I2C.read_page($0C,$10,@ASAX,3) ' Read the magnetometer adjustment values ($A2, $A4, $AF) ASAX := (ASAX - 128) / 2 ' equation from datasheet: Hadj = H x (((ASA - 128) x 0.5 / 128) + 1) ASAY := (ASAY - 128) / 2 ' microcontroller friendlier: Hadj = (H x (ASA - 128) / 2 + 1) / 128 + H ASAZ := (ASAZ - 128) / 2 ' These values never change so might as well do a partial calculation here \I2C.write(I2C#MPU,$19,$01) ' Sample rate divider (divide gyro_rate by 1 + x) \I2C.write(I2C#MPU,$1A,%00_000_110) ' Digital low-pass filtering (0 = 8KHz gyro_rate, !0 = 1KHz gyro_rate) \I2C.write(I2C#MPU,$1B,%000_11_000) ' Accelerometer sensitivity ±2000°/s \I2C.write(I2C#MPU,$1C,%000_11_000) ' Accelerometer sensitivity ±16g ' // MPU_SetConfig End ' // Calibrate_gyro Start cx := 0 cy := 0 cz := 0 repeat 25 repeat until \I2C.read(I2C#MPU,$3A) & $01 \I2C.read_words(I2C#MPU,$43,@Word_buffer,3) cx += ~~Word_buffer[0] cy += ~~Word_buffer[1] cz += ~~Word_buffer[2] cx /= 25 cy /= 25 cz /= 25 ' // Calibrate_gyro End repeat ' // Wait for Data, then Read 7 Word's repeat until \I2C.read(I2C#MPU,$3A) & $01 ' wait for new data (based on sampling rate and digital low-pass filtering registers) \I2C.read_words(I2C#MPU,$3B,@Word_buffer,7) ' Read the samples into a word buffer ' // Copy Buffer to L0 Word to Long *** Starting at L0 to L6 repeat i from 0 to 6 ' Copy the word buffer into sign-extended local longs L0[i] := ~~Word_buffer[i] ' // Calculate Temperature L3 := L3 / 340 + 35 ' Perform any required calculations on the local longs (also the place to do averaging) ' // Subtract Offset from Gyro L4 -= cx L5 -= cy L6 -= cz ' // Re-Copy Buffer (L0) to Acc_X (7 Longs) repeat i from 0 to 6 ' *** Starting at Acc_X to Gyro_Z (Temp gets copied Here) Acc_X[i] := L0[i] ' Copy samples to the global vars ' // Clear L0, L1, L2 L0~ L1~ L2~ ' // Read Magnetometer Works 100% repeat 16 ' Gather 16 samples on each axis I2C.write($0C,$0A,%0001) ' Single_measurement repeat until I2C.read($0C,$02) & 1 ' Wait until DRDY bit is set I2C.read_page($0C,$03,@word_buffer,6) ' Read XOUT_L through ZOUT_H registers into buffer L0 += ~~word_buffer[0] L1 += ~~word_buffer[1] L2 += ~~word_buffer[2] L0 ~>= 4 ' Divide each sample by 16, keeping the same sign L1 ~>= 4 L2 ~>= 4 Mag_X := ((~~L0 * ASAX + 1) / 128 + ~~L0) ' Apply the adjustment values Mag_Y := ((~~L1 * ASAY + 1) / 128 + ~~L1) ' and store in the global vars Mag_Z := ((~~L2 * ASAZ + 1) / 128 + ~~L2)I'm away from my Prop at the moment so I can't experiment at the moment. Maybe try increasing the stack size some more, I had to bump it up a couple times while creating this monster.
Okay, the temperature is read into the word_buffer along with the accelerometer and gyro readings, specifically it's read into word_buffer[3]. That is copied into L3, also known as L0[3]. The value in L3 is divided by 340 and added to 35, then stored back into L3. Finally, L3 is copied to the VARs at the top into Temp, also known as Acc_X[3].
The reason for a lot that is trying to format everything properly. The sensors were read in bytes, corrected for endian-ness and stored as words, needed to be operated on as longs due to the possibility of certain operations overflowing 16 bits, and finally stored as longs in order to keep the sign extended to bit 31.
I am starting to get your code. It is a little different than what I am used to.
As for the accelerometer, I know it is working as I have tested it with other code. The MPU-6050 PASM driver works fine for everything but the Mag. And is why I started down the road to writing my own drivers. Of every code I have tested, only part (or none) of them typically works. It has been very frustrating with this little sensor getting full access to it's most basic features.
I wrote a minimal i2c spin version that works, but has a random glitch in all the values that cannot be found nor filtered. Then I tried re-modding the pasm driver, I can read from the Mag ok, but cannot write to it. Plus I am out of FIT 496 space.
How could I test this? I was concerned this might be a possibility.
One spin driver works for everything but the accelerometer (values totally wrong), another works for everything but is glitchy, the one that works perfect is for the 6050 and doesn't include the mag and is pasm. Ive tried modding it, I can read from the mag, just cant write to it.
I have a modded 6050 pasm driver that is "working", but I had to use a Spin i2c driver to write to the mag while the pasm i2c driver reads from it. So I have 2 i2c drivers running at once (only way I could get it to work), but this causes frequent glitching on the pasm readouts. I cleaned up the results by checking the Chip ID, if = $68 then I allow the data, otherwise it gets dropped.
I am seriously trying everything under the sun to get this module to work 100%.
I tried re-writing the 6050 PASM driver again, this time with success!
All that was needed was a delay after writing to the mag for the new data to arrive! Now I have a 100% working PASM Driver!
'' MPU-60X0-PASM.spin '' Reads gyro and accelerometer data from the MPU-60X0 chips '' Read loop is in Propeller Assembler '' '' Based on Jason Dorie's code for the ITG-3200 and ADCL345 chips '' '' Note that this code assumes an 80 MHz clock '' '' The TestMPU routine can be used to verify correct setup of, and '' communication with, the MPU-60X0. Load the object into RAM, then '' use f12 to bring up the terminal emulator to see the output. '' { ******************************************* * User Init Select * * Updates by: Zack Lantz * ******************************************* Place in Main .Spin: Con ' // Accelerometer Settings mAFS0 = 0 mAFS1 = 1 mAFS2 = 2 mAFS3 = 3 ' // Gyroscope Settings mFS0 = 0 mFS1 = 1 mFS2 = 2 mFS3 = 3 ' // Digital Low Pass Filter Settings DLP0 = 0 ' Bandwidth = 260 Hz DLP1 = 1 ' Bandwidth = 184 Hz DLP2 = 2 ' Bandwidth = 94 Hz DLP3 = 3 ' Bandwidth = 44 Hz DLP4 = 4 ' Bandwidth = 21 Hz DLP5 = 5 ' Bandwidth = 10 Hz DLP6 = 6 ' Bandwidth = 5 Hz DLP7 = 7 ' Reserved ' // Current Settings (Passed to MPU-6050.spin) AccelFS = AFS2 GyroFS = FS3 mAFS = mAFS2 mFS = mFS3 mDLP = DLP3 Then start MPU Driver with: MPU.Start(MPUscl, MPUsda, mAFS, mFS, mDLP) ' MPU-6050 Gyro & Accel Sensor Data w/ User Init AFS, FS, & DLP Everything else works the same. New Functions: Start (SCL, SDA, aFS, gFS, fDLP) StartA(SCL, SDA, aFS, gFS, fDLP, PM) ', SF) StartX(SCL, SDA) *** Note: I didn't add in SampleRate feature as it is 0 to 255 & Calculated as follows: Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) Where Gyroscope Output Rate = 8kHz when the DLPF is disabled (DLPF_CFG = 0 or 7), and 1kHz when the DLPF is enabled (see Register 26) } {{ The slave address of the MPU-60X0 is b110100X which is 7 bits long. The LSB bit of the 7 bit address is determined by the logic level on pin AD0. This allows two MPU-60X0s to be connected to the same I2C bus. When used in this configuration, the address of the one of the devices should be b1101000 (pin AD0 is logic low) and the address of the other should be b1101001 (pin AD0 is logic high). }} CON _clkmode = xtal1 + pll16x _xinfreq = 5_000_000 CON ' CONs for TestMPU test routine SDA_PIN = 10 SCL_PIN = 11 SERIAL_TX_PIN = 30 SERIAL_RX_PIN = 31 VAR long x0, y0, z0, a0, b0, c0, t long Cog long rx, ry, rz, temp, ax, ay, az, arx, ary, mx, my, mz 'PASM code assumes these to be contiguous OBJ debug : "FullDuplexSerial" PUB TestMPU | MPUcog '----------------------------------------------- ' Start serial i/o cog ' Start cog to pull gyro/accel data from chip ' Print data to serial out every few seconds '------------------------------------------------ debug.start(SERIAL_RX_PIN, SERIAL_TX_PIN, 0, 115200) 'Start cog to allow IO with serial terminal 'repeat 4 ' waitcnt(clkfreq + cnt) debug.str(string("Starting...")) debug.tx(13) debug.str(string("GX GY GZ AX AY AZ")) debug.tx(13) debug.str(string("-------------------------")) debug.tx(13) MPUcog := StartX( SCL_PIN, SDA_PIN ) 'Output gyro data, then accel data, once per second repeat debug.dec(GetRX) debug.str(string(", ")) debug.dec(GetRY) debug.str(string(", ")) debug.dec(GetRZ) debug.str(string(" ")) debug.dec(GetAX) debug.str(string(", ")) debug.dec(GetAY) debug.str(string(", ")) debug.dec(GetAZ) debug.str(string(" ")) debug.dec(GetARX) debug.str(string(", ")) debug.dec(GetARY) debug.str(string(" ")) debug.str(string(" ")) debug.dec(GetMX) debug.str(string(", ")) debug.dec(GetMY) debug.str(string(", ")) debug.dec(GetMZ) debug.tx(13) 'waitcnt((clkfreq / 10) + cnt) ' // Basic Start with User Accel AFS & Gyro FS & DLP PUB Start( SCL, SDA, aFS, gFS, fDLP ) : Status if aFS == 0 AccelFS := %00000000 ' ± 2 g elseif aFS == 1 AccelFS := %00001000 ' ± 4 g elseif aFS == 2 AccelFS := %00010000 ' ± 8 g elseif aFS == 3 AccelFS := %00011000 ' ± 16 g if gFS == 0 GyroFS := %00000000 ' ± 250 ° /s elseif gFS == 1 GyroFS := %00001000 ' ± 500 ° /s elseif gFS == 2 GyroFS := %00010000 ' ± 1000 ° /s elseif gFS == 3 GyroFS := %00011000 ' ± 2000 ° /s '| DLPF_CFG | Accelerometer | Gyroscope | if fDLP == 0 ' Bw (Hz) Delay (ms) Bw (Hz) Delay (ms) FS (Khz) DLP := %00000000 ' 0 260 0 256 0.98 8 elseif fDLP == 1 DLP := %00000001 ' 1 184 2.0 188 1.9 1 elseif fDLP == 2 DLP := %00000010 ' 2 94 3.0 98 2.8 1 elseif fDLP == 3 DLP := %00000011 ' 3 44 4.9 42 4.8 1 elseif fDLP == 4 DLP := %00000100 ' 4 21 8.5 20 8.3 1 elseif fDLP == 5 DLP := %00000101 ' 5 10 13.8 10 13.4 1 elseif fDLP == 6 DLP := %00000110 ' 6 5 19.0 5 18.6 1 elseif fDLP == 7 DLP := %00000111 ' 7 RESERVED RESERVED 8 PowerMgmt := %00000001 ' X gyro as clock source SampleRate := %00000001 ' 500 Hz ComputeTimes gyroSCL := 1 << SCL 'save I2C pins gyroSDA := 1 << SDA Status := Cog := cognew(@Start_Sensors, @rx) + 1 CalibrateAccel ' // Start w/ Full User Init Settings PUB StartA( SCL, SDA, aFS, gFS, fDLP, PM ) : Status if aFS == 0 AccelFS := %00000000 elseif aFS == 1 AccelFS := %00001000 elseif aFS == 2 AccelFS := %00010000 elseif aFS == 3 AccelFS := %00011000 if gFS == 0 GyroFS := %00000000 elseif gFS == 1 GyroFS := %00001000 elseif gFS == 2 GyroFS := %00010000 elseif gFS == 3 GyroFS := %00011000 '| DLPF_CFG | Accelerometer | Gyroscope | if fDLP == 0 ' Bw (Hz) Delay (ms) Bw (Hz) Delay (ms) FS (Khz) DLP := %00000000 ' 0 260 0 256 0.98 8 elseif fDLP == 1 DLP := %00000001 ' 1 184 2.0 188 1.9 1 elseif fDLP == 2 DLP := %00000010 ' 2 94 3.0 98 2.8 1 elseif fDLP == 3 DLP := %00000011 ' 3 44 4.9 42 4.8 1 elseif fDLP == 4 DLP := %00000100 ' 4 21 8.5 20 8.3 1 elseif fDLP == 5 DLP := %00000101 ' 5 10 13.8 10 13.4 1 elseif fDLP == 6 DLP := %00000110 ' 6 5 19.0 5 18.6 1 elseif fDLP == 7 DLP := %00000111 ' 7 RESERVED RESERVED 8 if PM == 0 PowerMgmt := %00000000 ' Internal 8MHz oscillator elseif PM == 1 PowerMgmt := %00000001 ' PLL with X axis gyroscope reference elseif PM == 2 PowerMgmt := %00000010 ' PLL with Y axis gyroscope reference elseif PM == 3 PowerMgmt := %00000011 ' PLL with Z axis gyroscope reference elseif PM == 4 PowerMgmt := %00000100 ' PLL with external 32.768kHz reference elseif PM == 5 PowerMgmt := %00000101 ' PLL with external 19.2MHz referenc elseif PM == 6 PowerMgmt := %00000110 ' Reserved elseif PM == 7 PowerMgmt := %00000111 ' Stops the clock and keeps the timing generator in reset ' *** Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) ' // 0 threw 255 SampleRate := %00000001 ' 500 Hz Sample Rate, %00000000 = 1000 Hz Sample rate ComputeTimes gyroSCL := 1 << SCL 'save I2C pins gyroSDA := 1 << SDA Status := Cog := cognew(@Start_Sensors, @rx) + 1 CalibrateAccel ' // Start Basic, No User Init PUB StartX( SCL, SDA ) : Status AccelFS := %00010000 ' AFS2 GyroFS := %00011000 ' FS3 DLP := %00000011 ' 40 Hz PowerMgmt := %00000001 ' X gyro as clock source SampleRate := %00000001 ' 500 Hz ComputeTimes gyroSCL := 1 << SCL 'save I2C pins gyroSDA := 1 << SDA Status := Cog := cognew(@Start_Sensors, @rx) + 1 CalibrateAccel CalibrateGyro PUB Stop if Cog cogstop(Cog~ - 1) '********************** ' Accessors '********************** PUB GetTemp ' Temperature return temp 'Pub GetTempF ' Temp Deg F 'Pub GetTempC ' Temp Deg C PUB GetRX ' Accel X - Zero Offset return rx - x0 PUB GetRY ' Accel Y - Zero Offset return ry - y0 PUB GetRZ ' Accel Z - Zero Offset return rz - z0 PUB GetAX ' Gyro X - Zero Offset return ax - a0 PUB GetAY ' Gyro Y - Zero Offset return ay - b0 PUB GetAZ ' Gyro Z - Zero Offset return az - c0 PUB GetMX ' Mag X return mx PUB GetMY ' Mag Y return my PUB GetMZ ' Mag Z return mz PUB GetARX ' Pitch Angle return arx PUB GetARY ' Roll Angle return ary Pub GetAccelOffsetX ' Accelerometer Zero Offset X return x0 Pub GetAccelOffsetY ' Accelerometer Zero Offset Y return y0 Pub GetAccelOffsetZ ' Accelerometer Zero Offset Z return z0 Pub GetGyroOffsetX ' Gyroscope Zero Offset X return a0 Pub GetGyroOffsetY ' Gyroscope Zero Offset Y return b0 Pub GetGyroOffsetZ ' Gyroscope Zero Offset Z return c0 Pub CalAccel ' Calibrate Accelerometer CalibrateAccel Pub CalGyro ' Calibrate Gyroscope CalibrateGyro PRI computeTimes '' Set up timing constants in assembly ' (Done this way to avoid overflow) i2cDataSet := ((clkfreq / 10000) * 350) / 100000 ' Data setup time - 350ns (400KHz) i2cClkLow := ((clkfreq / 10000) * 1300) / 100000 ' Clock low time - 1300ns (400KHz) i2cClkHigh := ((clkfreq / 10000) * 600) / 100000 ' Clock high time - 600ns (400KHz) i2cPause := clkfreq / 100000 ' Pause between checks for operations PRI CalibrateAccel | tc, xc, yc, zc, dr x0 := 0 ' Initialize offsets y0 := 0 z0 := 0 'wait 1/2 second for the body to stop moving waitcnt( constant(80_000_000 / 2) + cnt ) 'Find the zero points of the 3 axis by reading for ~1 sec and averaging the results xc := 0 yc := 0 zc := 0 repeat 256 xc += rx yc += ry zc += rz waitcnt( constant(80_000_000/192) + cnt ) 'Perform rounding if( xc > 0 ) xc += 128 elseif( xc < 0 ) xc -= 128 if( yc > 0 ) yc += 128 elseif( yc < 0 ) yc -= 128 if( zc > 0 ) zc += 128 elseif( zc < 0 ) zc -= 128 x0 := xc / 256 y0 := yc / 256 z0 := zc / 256 PRI CalibrateGyro | tc, xc, yc, zc, dr a0 := 0 ' Initialize offsets b0 := 0 c0 := 0 'wait 1/2 second for the body to stop moving waitcnt( constant(80_000_000 / 2) + cnt ) 'Find the zero points of the 3 axis by reading for ~1 sec and averaging the results xc := 0 yc := 0 zc := 0 repeat 256 xc += ax yc += ay zc += az waitcnt( constant(80_000_000/192) + cnt ) 'Perform rounding if( xc > 0 ) xc += 128 elseif( xc < 0 ) xc -= 128 if( yc > 0 ) yc += 128 elseif( yc < 0 ) yc -= 128 if( zc > 0 ) zc += 128 elseif( zc < 0 ) zc -= 128 a0 := xc / 256 b0 := yc / 256 c0 := zc / 256 DAT org 0 Start_Sensors ' --------- Debugger Kernel add this at Entry (Addr 0) --------- ' long $34FC1202,$6CE81201,$83C120B,$8BC0E0A,$E87C0E03,$8BC0E0A ' long $EC7C0E05,$A0BC1207,$5C7C0003,$5C7C0003,$7FFC,$7FF8 ' -------------------------------------------------------------- mov p1, par ' Get data pointer mov prX, p1 ' Store the pointer to the rx var in HUB RAM add p1, #4 mov prY, p1 ' Store the pointer to the ry var in HUB RAM add p1, #4 mov prZ, p1 ' Store the pointer to the rz var in HUB RAM add p1, #4 mov pT, p1 ' Store the pointer to the temp var in HUB RAM add p1, #4 mov paX, p1 ' Store the pointer to the ax var in HUB RAM add p1, #4 mov paY, p1 ' Store the pointer to the ay var in HUB RAM add p1, #4 mov paZ, p1 ' Store the pointer to the az var in HUB RAM add p1, #4 mov paRX, p1 ' Store the pointer to the arx var in HUB RAM add p1, #4 mov paRY, p1 ' Store the pointer to the ary var in HUB RAM add p1, #4 mov pmX, p1 ' Store the pointer to the ax var in HUB RAM add p1, #4 mov pmY, p1 ' Store the pointer to the ay var in HUB RAM add p1, #4 mov pmZ, p1 ' Store the pointer to the az var in HUB RAM mov i2cTemp,i2cPause add i2cTemp,CNT ' Wait 10us before starting waitcnt i2cTemp,#0 call #SetConfig mov loopCount, CNT add loopCount, loopDelay '------------------------------------------------------------------------------ ' Main loop ' loopDelay defined in data section ' Nominally set to CLK_FREQ/200 give 200hz update rate, but the update takes less than ' 500us, so the delay could potentially be set to give an update rate as high as 2000hz ' :loop call #MPUReadValues call #MPUComputeDrift call #ComputeAngles call #MPUGetMagVals wrlong iT, pT subs irX, drift wrlong irX, prX subs irY, drift wrlong irY, prY subs irZ, drift wrlong irZ, prZ wrlong iaX, paX wrlong iaY, paY wrlong iaZ, paZ 'Sign extend the 15th bit test iaRX, i2cWordReadMask wc muxc iaRX, i2cWordMask 'Sign extend the 15th bit test iaRY, i2cWordReadMask wc muxc iaRY, i2cWordMask wrlong iaRX, paRX wrlong iaRY, paRY wrlong imX, pmX wrlong imY, pmY wrlong imZ, pmZ waitcnt loopCount, loopDelay jmp #:loop '------------------------------------------------------------------------------ ' MPUReadValues ' ' Starting at the ACCEL_X data register, read in the 3 accel values, ' the temperature, and the 3 gyro values, as these are held in ' sequential register locations. ' MPUReadValues mov i2cSDA, gyroSDA 'Use gyro SDA,SCL mov i2cSCL, gyroSCL mov i2cAddr, #59 ' Address of ACCEL_XOUT_H mov i2cDevID, #%11010000 ' Device ID of the MPU call #StartRead ' Tell the I2C device we're starting mov i2cMask, i2cWordReadMask test i2cTestCarry, #0 wc ' Clear the carry flag to make reads auto-increment call #i2cRead call #i2cRead 'Sign extend the 15th bit test i2cData, i2cWordReadMask wc muxc i2cData, i2cWordMask mov iaX, i2cData mov i2cMask, i2cWordReadMask test i2cTestCarry, #0 wc ' Clear the carry flag to make reads auto-increment call #i2cRead call #i2cRead 'Sign extend the 15th bit test i2cData, i2cWordReadMask wc muxc i2cData, i2cWordMask mov iaY, i2cData mov i2cMask, i2cWordReadMask test i2cTestCarry, #0 wc ' Clear the carry flag to make reads auto-increment call #i2cRead call #i2cRead 'Sign extend the 15th bit test i2cData, i2cWordReadMask wc muxc i2cData, i2cWordMask mov iaZ, i2cData mov i2cMask, i2cWordReadMask test i2cTestCarry, #0 wc ' Clear the carry flag to make reads auto-increment call #i2cRead 'test i2cTestCarry, #1 wc ' Set the carry flag to tell it we're done call #i2cRead test i2cData, i2cWordReadMask wc muxc i2cData, i2cWordMask mov iT, i2cData mov i2cMask, i2cWordReadMask test i2cTestCarry, #0 wc ' Clear the carry flag to make reads auto-increment call #i2cRead call #i2cRead 'Sign extend the 15th bit test i2cData, i2cWordReadMask wc muxc i2cData, i2cWordMask mov irX, i2cData mov i2cMask, i2cWordReadMask test i2cTestCarry, #0 wc ' Clear the carry flag to make reads auto-increment call #i2cRead call #i2cRead 'Sign extend the 15th bit test i2cData, i2cWordReadMask wc muxc i2cData, i2cWordMask mov irY, i2cData mov i2cMask, i2cWordReadMask test i2cTestCarry, #0 wc ' Clear the carry flag to make reads auto-increment call #i2cRead test i2cTestCarry, #1 wc ' Set the carry flag to tell it we're done call #i2cRead 'Sign extend the 15th bit test i2cData, i2cWordReadMask wc muxc i2cData, i2cWordMask mov irZ, i2cData call #i2cStop MPUReadValues_Ret ret ConfigMag mov i2cSDA, gyroSDA 'Use gyro SDA,SCL mov i2cSCL, gyroSCL call #i2cReset 'Reset i2c mov i2cDevID, #%00011000 'Device ID for the AK8975 mov i2cAddr, #$0A ' CTRL Register mov i2cValue, #%00000001 ' Single-Measurement Mode call #i2cWriteRegisterByte ConfigMag_Ret ret MPUGetMagVals call #ConfigMag ' *** Makes ALL Mag Values = 0 [B] waitcnt loopCount, loopDelay ' // Wait for New Data[/B] mov i2cSDA, gyroSDA 'Use gyro SDA,SCL mov i2cSCL, gyroSCL mov i2cAddr, #03 mov i2cDevID, #%00011000 call #StartRead ' Tell the I2C device we're starting mov i2cMask, i2cWordReadMask test i2cTestCarry, #0 wc call #i2cRead test i2cTestCarry, #1 wc ' Clear the carry flag to make reads auto-increment call #i2cRead 'Sign extend the 15th bit test i2cData, i2cWordReadMask wc muxc i2cData, i2cWordMask mov imX, i2cData mov i2cAddr, #05 mov i2cDevID, #%00011000 call #StartRead ' Tell the I2C device we're starting mov i2cMask, i2cWordReadMask test i2cTestCarry, #0 wc call #i2cRead test i2cTestCarry, #1 wc ' Clear the carry flag to make reads auto-increment call #i2cRead 'Sign extend the 15th bit test i2cData, i2cWordReadMask wc muxc i2cData, i2cWordMask mov imY, i2cData mov i2cAddr, #07 mov i2cDevID, #%00011000 call #StartRead ' Tell the I2C device we're starting mov i2cMask, i2cWordReadMask test i2cTestCarry, #0 wc call #i2cRead test i2cTestCarry, #1 wc ' Clear the carry flag to make reads auto-increment call #i2cRead 'Sign extend the 15th bit test i2cData, i2cWordReadMask wc muxc i2cData, i2cWordMask mov imZ, i2cData call #i2cStop MPUGetMagVals_Ret ret '------------------------------------------------------------------------------ ' Compute drift - for my gyro (Jason's ITG-3200) '(Temp + 15000) / 100 = drift '------------------------------------------------------------------------------ MPUComputeDrift mov drift, iT ' Start with the temperature reading add drift, tempOffset ' Offset it by 15,000 ' divide drift by 100 mov divisor, #100 mov dividend, drift test dividend, i2cWordReadMask wc muxc signbit, #1 ' record the sign of the original value abs dividend, dividend mov divCounter, #10 shl divisor, divCounter mov resultShifted, #1 shl resultShifted, divCounter add divCounter, #1 mov drift, #0 :divLoop cmp dividend, divisor wc if_nc add drift, resultShifted if_nc sub dividend, divisor shr resultShifted, #1 shr divisor, #1 djnz divCounter, #:divLoop test signbit, #1 wc negc drift, drift MPUComputeDrift_Ret ret '------------------------------------------------------------------------------ ComputeAngles mov cx, iaZ mov cy, iaX call #cordic mov iaRX, ca mov cx, iaZ mov cy, iaY call #cordic mov iaRY, ca ComputeAngles_ret ret '------------------------------------------------------------------------------ ' SetConfig ' ' See MPU-6000/6050 Register Map document for register addresses and ' valid settings ' SetConfig mov i2cSDA, gyroSDA 'Use gyro SDA,SCL mov i2cSCL, gyroSCL call #i2cReset 'Reset i2c :MPUSetConfig mov i2cDevID, #%11010000 'Device ID for the MPU-6000/6050 ' // Power Management mov i2cAddr, #107 'Set PWR_MGMT_1 register bit 0 to choose mov i2cValue, PowerMgmt ' X gyro as clock source ' call #i2cWriteRegisterByte ' // I2C Master Disabled mov i2cAddr, #106 mov i2cValue, #%00000000 call #i2cWriteRegisterByte ' // Digital Low Pass Filter _ Config mov i2cAddr, #26 mov i2cValue, DLP 'Set DLPF_CONFIG to 3 for 40Hz bandwidth call #i2cWriteRegisterByte ' // Sample Rate Divider mov i2cAddr, #25 'SMPLRT_DIV = 1 => 1khz/(1+0) = 1000hz sample rate mov i2cValue, SampleRate call #i2cWriteRegisterByte ' // Gyro _ Config mov i2cAddr, #27 'GYRO_CONFIG register, set FS_SEL bits to 3 gives a mov i2cValue, GyroFS ' full scale range of +-2000 deg/sec call #i2cWriteRegisterByte ' // Accel _ Config mov i2cAddr, #28 'Set ACCEL_CONFIG register AFS_SEL bits to 2 mov i2cValue, AccelFS ' sets +-8g full scale range call #i2cWriteRegisterByte 'ACCEL_HPF is zero which turns off high-pass filtering ' // I2C Bypass Enabled mov i2cAddr, #55 mov i2cValue, #%00000010 call #i2cWriteRegisterByte { ' // Extra Config Settings: mov i2cAddr, #Address mov i2cValue, #%BIN_Value call #i2cWriteRegisterByte } SetConfig_Ret ret '------------------------------------------------------------------------------ StartRead call #i2cStart mov i2cData, i2cDevID mov i2cMask, #%10000000 call #i2cWrite mov i2cData, i2cAddr mov i2cMask,#%10000000 call #i2cWrite call #i2cStart mov i2cData, i2cDevID or i2cData, #1 mov i2cMask, #%10000000 call #i2cWrite StartRead_Ret ret '------------------------------------------------------------------------------ i2cWriteRegisterByte call #i2cStart mov i2cData, i2cDevID mov i2cMask,#%10000000 call #i2cWrite mov i2cTime,i2cClkLow add i2cTime,cnt ' Allow for minimum SCL low waitcnt i2cTime, #0 mov i2cData, i2cAddr mov i2cMask,#%10000000 call #i2cWrite mov i2cTime,i2cClkLow add i2cTime,cnt ' Allow for minimum SCL low waitcnt i2cTime, #0 mov i2cData, i2cValue mov i2cMask,#%10000000 call #i2cWrite call #i2cStop i2cWriteRegisterByte_Ret ret '------------------------------------------------------------------------------ '' Low level I2C routines. These are designed to work either with a standard I2C bus '' (with pullups on both SCL and SDA) or the Propellor Demo Board (with a pullup only '' on SDA). Timing can be set by the caller to 100KHz or 400KHz. '------------------------------------------------------------------------------ '' Do I2C Reset Sequence. Clock up to 9 cycles. Look for SDA high while SCL '' is high. Device should respond to next Start Sequence. Leave SCL high. i2cReset andn dira,i2cSDA ' Pullup drive SDA high mov i2cBitCnt,#9 ' Number of clock cycles mov i2cTime,i2cClkLow add i2cTime,cnt ' Allow for minimum SCL low :i2cResetClk andn outa,i2cSCL ' Active drive SCL low or dira,i2cSCL waitcnt i2cTime,i2cClkHigh or outa,i2cSCL ' Active drive SCL high or dira,i2cSCL andn dira,i2cSCL ' Pullup drive SCL high waitcnt i2cTime,i2cClkLow ' Allow minimum SCL high test i2cSDA,ina wz ' Stop if SDA is high if_z djnz i2cBitCnt,#:i2cResetClk ' Stop after 9 cycles i2cReset_ret ret ' Should be ready for Start '------------------------------------------------------------------------------ '' Do I2C Start Sequence. This assumes that SDA is a floating input and '' SCL is also floating, but may have to be actively driven high and low. '' The start sequence is where SDA goes from HIGH to LOW while SCL is HIGH. i2cStart or outa,i2cSCL ' Active drive SCL high or dira,i2cSCL or outa,i2cSDA ' Active drive SDA high or dira,i2cSDA mov i2cTime,i2cClkHigh add i2cTime,cnt ' Allow for bus free time waitcnt i2cTime,i2cClkLow andn outa,i2cSDA ' Active drive SDA low waitcnt i2cTime,#0 andn outa,i2cSCL ' Active drive SCL low i2cStart_ret ret '------------------------------------------------------------------------------ '' Do I2C Stop Sequence. This assumes that SCL is low and SDA is indeterminant. '' The stop sequence is where SDA goes from LOW to HIGH while SCL is HIGH. '' i2cStart must have been called prior to calling this routine for initialization. '' The state of the (c) flag is maintained so a write error can be reported. i2cStop or outa,i2cSCL ' Active drive SCL high mov i2cTime,i2cClkHigh add i2cTime,cnt ' Wait for minimum clock low waitcnt i2cTime,i2cClkLow or outa,i2cSDA ' Active drive SDA high waitcnt i2cTime,i2cClkLow andn dira,i2cSCL ' Pullup drive SCL high waitcnt i2cTime,i2cClkLow ' Wait for minimum setup time andn dira,i2cSDA ' Pullup drive SDA high waitcnt i2cTime,#0 ' Allow for bus free time i2cStop_ret ret '------------------------------------------------------------------------------ '' Write I2C data. This assumes that i2cStart has been called and that SCL is low, '' SDA is indeterminant. The (c) flag will be set on exit from ACK/NAK with ACK == false '' and NAK == true. Bytes are handled in "little-endian" order so these routines can be '' used with words or longs although the bits are in msb..lsb order. i2cWrite mov i2cBitCnt,#8 mov i2cTime,i2cClkLow add i2cTime,cnt ' Wait for minimum SCL low :i2cWriteBit waitcnt i2cTime,i2cDataSet test i2cData,i2cMask wz if_z or dira,i2cSDA ' Copy data bit to SDA if_nz andn dira,i2cSDA waitcnt i2cTime,i2cClkHigh ' Wait for minimum setup time or outa,i2cSCL ' Active drive SCL high waitcnt i2cTime,i2cClkLow andn outa,i2cSCL ' Active drive SCL low ror i2cMask,#1 ' Go do next bit if not done djnz i2cBitCnt,#:i2cWriteBit andn dira,i2cSDA ' Switch SDA to input and waitcnt i2cTime,i2cClkHigh ' wait for minimum SCL low or outa,i2cSCL ' Active drive SCL high waitcnt i2cTime,i2cClkLow ' Wait for minimum high time test i2cSDA,ina wc ' Sample SDA (ACK/NAK) then andn outa,i2cSCL ' active drive SCL low andn outa,i2cSDA ' active drive SDA low or dira,i2cSDA ' Leave SDA low rol i2cMask,#16 ' Prepare for multibyte write waitcnt i2cTime,#0 ' Wait for minimum low time i2cWrite_ret ret '------------------------------------------------------------------------------ '' Read I2C data. This assumes that i2cStart has been called and that SCL is low, '' SDA is indeterminant. ACK/NAK will be copied from the (c) flag on entry with '' ACK == low and NAK == high. Bytes are handled in "little-endian" order so these '' routines can be used with words or longs although the bits are in msb..lsb order. i2cRead mov i2cBitCnt,#8 andn dira,i2cSDA ' Make sure SDA is set to input mov i2cTime,i2cClkLow add i2cTime,cnt ' Wait for minimum SCL low :i2cReadBit waitcnt i2cTime,i2cClkHigh or outa,i2cSCL ' Active drive SCL high waitcnt i2cTime,i2cClkLow ' Wait for minimum clock high test i2cSDA,ina wz ' Sample SDA for data bits andn outa,i2cSCL ' Active drive SCL low if_nz or i2cData,i2cMask ' Accumulate data bits if_z andn i2cData,i2cMask ror i2cMask,#1 ' Shift the bit mask and djnz i2cBitCnt,#:i2cReadBit ' continue until done waitcnt i2cTime,i2cDataSet ' Wait for end of SCL low if_c or outa,i2cSDA ' Copy the ACK/NAK bit to SDA if_nc andn outa,i2cSDA or dira,i2cSDA ' Make sure SDA is set to output waitcnt i2cTime,i2cClkHigh ' Wait for minimum setup time or outa,i2cSCL ' Active drive SCL high waitcnt i2cTime,i2cClkLow ' Wait for minimum clock high andn outa,i2cSCL ' Active drive SCL low andn outa,i2cSDA ' Leave SDA low waitcnt i2cTime,#0 ' Wait for minimum low time i2cRead_ret ret { ComputeAngles mov cx, iaZ mov cy, iaX call #cordic mov iaRX, ca mov cx, iaZ mov cy, iaY call #cordic mov iaRY, ca ComputeAngles_ret ret } '------------------------------------------------------------------------------ '' Perform CORDIC cartesian-to-polar conversion ''Input = cx(x) and cy(x) ''Output = cx(ro) and ca(theta) cordic abs cx,cx wc if_c neg cy,cy mov ca,#0 rcr ca,#1 movs :lookup,#cordicTable mov t1,#0 mov t2,#20 :loop mov dx,cy wc sar dx,t1 mov dy,cx sar dy,t1 sumc cx,dx sumnc cy,dy :lookup sumc ca,cordicTable add :lookup,#1 add t1,#1 djnz t2,#:loop shr ca, #16 cordic_ret ret cordicTable long $20000000 long $12E4051E long $09FB385B long $051111D4 long $028B0D43 long $0145D7E1 long $00A2F61E long $00517C55 long $0028BE53 long $00145F2F long $000A2F98 long $000517CC long $00028BE6 long $000145F3 long $0000A2FA long $0000517D long $000028BE long $0000145F long $00000A30 long $00000518 dx long 0 dy long 0 cx long 0 cy long 0 ca long 0 t1 long 0 t2 long 0 '' Variables for the gyro routines p1 long 0 pT long 0 ' Pointer to Temperature in hub ram prX long 0 ' Pointer to X rotation in hub ram prY long 0 ' Pointer to Y rotation in hub ram prZ long 0 ' Pointer to Z rotation in hub ram paX long 0 ' Pointer to X accel in hub ram paY long 0 ' Pointer to Y accel in hub ram paZ long 0 ' Pointer to Z accel in hub ram paRX long 0 ' Pointer to X accel angle in hub ram paRY long 0 ' Pointer to Y accel angle in hub ram pcID long 0 pmX long 0 ' Pointer to mX value pmY long 0 ' Pointer to mY value pmZ long 0 ' Pointer to mZ value iT long 0 ' Interim temperature value irX long 0 ' Interim rX value irY long 0 ' Interim rY value - These values are temp storage before drift compensation irZ long 0 ' Interim rZ value iaX long 0 ' Interim aX value iaY long 0 ' Interim aY value iaZ long 0 ' Interim aZ value iaRX long 0 ' Interim aX value iaRY long 0 ' Interim aY value icID long 0 imX long 0 ' Interim aX value imY long 0 ' Interim aY value imZ long 0 ' Interim aZ value i2cWordReadMask long %10000000_00000000 i2cWordMask long $ffff0000 loopDelay long 80_000_000 / 200 loopCount long 0 '' Variables for dealing with drift / division tempOffset long 15000 drift long 0 divisor long 0 dividend long 0 resultShifted long 0 signbit long 0 divCounter long 0 '' Variables for i2c routines i2cTemp long 0 i2cCount long 0 i2cValue long 0 i2cDevID long 0 i2cAddr long 0 i2cDataSet long 0 ' Minumum data setup time (ticks) i2cClkLow long 0 ' Minimum clock low time (ticks) i2cClkHigh long 0 ' Minimum clock high time (ticks) i2cPause long 0 ' Pause before re-fetching next operation i2cTestCarry long 1 ' Used for setting the carry flag '' Local variables for low level I2C routines ' // MPU-6050 User Set Init Settings GyroFS long %00000000 AccelFS long %00000000 PowerMgmt long %00000000 SampleRate long %00000000 DLP long %00000000 gyroSCL long 0 ' Bit mask for SCL gyroSDA long 0 ' Bit mask for SDA i2cSCL long 0 ' Bit mask for SCL i2cSDA long 0 ' Bit mask for SDA i2cTime long 0 ' Used for timekeeping i2cData long 0 ' Data to be transmitted / received i2cMask long 0 ' Bit mask for bit to be tx / rx i2cBitCnt long 0 ' Number of bits to tx / rx FIT 496It does have this, the data is stored in register $02 bit 0. However, I am very new to pasm, so I opted for putting a waitcnt in to see if it would work and it sure enough did. I would like to correct this if possible.
I am finishing it up now with the extra magnetometer math I couldnt fit, nor knew how to do, in the pasm section.
Having a little issue with my number line, I am pretty confident that I will figure it out eventually...
In the Mag read section, it is Low byte High byte, wereas the mpu is High byte Low byte. It is currently being read backwards & I need to figure out how to fix this in the pasm section. This should correct my number line with the mag.
I just kept the read data intact until here (not sign extended), then rotated the bytes around then did Sign Extension last.
I am fairly confident all of the Mag data is now correct, as I can compare it against the code posted by ChrisGadd up top.
Now im looking at a problem in the Original MPU-6050 PASM code dealing with the angle computations.
ComputeAngles mov cx, iaZ mov cy, iaX call #cordic mov iaRX, ca mov cx, iaZ mov cy, iaY call #cordic mov iaRY, ca ComputeAngles_ret ret '------------------------------------------------------------------------------ '' Perform CORDIC cartesian-to-polar conversion ''Input = cx(x) and cy(x) ''Output = cx(ro) and ca(theta) cordic abs cx,cx wc if_c neg cy,cy mov ca,#0 rcr ca,#1 movs :lookup,#cordicTable mov t1,#0 mov t2,#20 :loop mov dx,cy wc sar dx,t1 mov dy,cx sar dy,t1 sumc cx,dx sumnc cy,dy :lookup sumc ca,cordicTable add :lookup,#1 add t1,#1 djnz t2,#:loop shr ca, #16 cordic_ret ret cordicTable long $20000000 long $12E4051E long $09FB385B long $051111D4 long $028B0D43 long $0145D7E1 long $00A2F61E long $00517C55 long $0028BE53 long $00145F2F long $000A2F98 long $000517CC long $00028BE6 long $000145F3 long $0000A2FA long $0000517D long $000028BE long $0000145F long $00000A30 long $00000518 dx long 0 dy long 0 cx long 0 cy long 0 ca long 0 t1 long 0 t2 long 0I am not sure how to scale the resulting numbers to a number line that has a Zero point (Level). I can do all the calculations in spin, but they will slow things down quite substantially.
I figured out the Pitch & Roll 6050 calculations. Its just 0 to 65535 on each axis so I just converted them to degrees so they are visibly legible.
Neither code's magnetometer readings are usable for calculating heading thus far. The code posted up top's mag appears correct, however, when you apply the heading algorithm to it you get stuck with a number between 63.0 and 63.9 for a 360 & very bouncy, no way it could be used. The Pasm code gets it the closest, but again the number line is all off (because of no MSB most likely)
I do like the idea of a spin driver as I can code in the angle calculations using floating point math quite easily into the mag cog.
Level:
I also had a look through of the PASM code from post #13, and it does look like you're reading it properly.
I'm not sure what those readings are telling me, but they look okay on the logic analyzer; no NAKs or stuck $FF or $00 readings anywhere. The stop bit could be better formed but it doesn't seem to be harming anything.
There was a bit of a surprise when I connected the module using your I2C pins 10 and 11 and the module stopped working. Seems it does need external pull-ups even though the datasheet says they're included on board the module. A couple 10K-ohms solved that.
I do have pullups on 10 & 11. I decided to switch back to the spin code from above as it is now working and because tho the pasm code is working, the data is wrong. Plus I was able to do additional math in the cog I couldn't do in pasm.
I have the driver updated for the most part to what I am after, however, i am still having a bit of trouble getting the Heading to calculate. In SerialChart you can see as it approaches 0 deg there is an error on either side of 0.
[code]
{
************************************************************************************************************
* *
* AUTO-RECOVER NOTICE: This file was automatically recovered from an earlier Propeller Tool session. *
* *
* ORIGINAL FOLDER: C:\...\Parallax Inc\Propeller Tool v1.3.2\Library\Autopilot Basestation\ *
* TIME AUTO-SAVED: 49 minutes ago (2/25/2014 4:19:35 AM) *
* *
* OPTIONS: 1) RESTORE THIS FILE by deleting these comments and selecting File -> Save. *
* The existing file in the original folder will be replaced by this one. *
* *
* -- OR -- *
* *
* 2) IGNORE THIS FILE by closing it without saving. *
* This file will be discarded and the original will be left intact. *
* *
************************************************************************************************************
.}
{{┌──────────────────────────────────────────┐
│ MPU-9150 demo using my I2C driver │
│ Author: Chris Gadd │
│ Copyright (c) 2012 Chris Gadd │
│ See end of file for terms of use. │
└──────────────────────────────────────────┘
Demonstrates the MPU-9150A 9-Axis Module from SparkFun
MPU-9150 400KHz max. clock frequency from datasheet
┌───────────┐
│ GND │ SCL
│ VCC │ SDA ────────────
│ SDA │ S 1 1 0 1 0 0 x 0 │ Register address │ Register data │ Next register data │ │
│ SCL │ └──────Device CODE──────┘ │ (Ack) (00h - 3Fh) (Ack) (See table below) (Ack) (Automatically advances) (Ack) Stop
│ ESD │ Read (1) (See table below)
│ ESC │ Write(0)
│ COUT │
│ CIN │ Accelerometer, gyroscope, and temperature sensor use device ID $68 ($69 if AD0 pulled high)
│ AD0 │ Magnetometer uses device ID $0C - must first be enabled by writing $02 to register $37 of device $68 ($69 if AD0 high)
│ FSYNC │
│ INT │
└───────────┘
GYRO AND ACCELEROMETER REGISTERS
┌───────┬────────────────────────────┬─────────┬─────────┬─────────┬─────────┬─────────┬─────────┬─────────┬─────────┐
│Address│ Definition │ BIT 7 │ BIT 6 │ BIT 5 │ BIT 4 │ BIT 3 │ BIT 2 │ BIT 1 │ BIT 0 │
├───────┼────────────────────────────┼─────────┴─────────┴─────────┼─────────┴─────────┴─────────┴─────────┴─────────┤
│ 0Dh │ SELF_TEST_X │ XA_TEST[4-2] │ XG_TEST[4-0] │
├───────┼────────────────────────────┼─────────────────────────────┼─────────────────────────────────────────────────┤
│ 0Eh │ SELF_TEST_Y │ YA_TEST[4-2] │ YG_TEST[4-0] │
├───────┼────────────────────────────┼─────────────────────────────┼─────────────────────────────────────────────────┤
│ 0Fh │ SELF_TEST_Z │ ZA_TEST[4-2] │ ZG_TEST[4-0] │
├───────┼────────────────────────────┼───────────────────┬─────────┴─────────┬───────────────────┬───────────────────┤
│ 10h │ SELF_TEST_A │ RESERVED │ XA_TEST[1-0] │ YA_TEST[1-0] │ ZA_TEST[1-0] │
├───────┼────────────────────────────┼───────────────────┴───────────────────┴───────────────────┴───────────────────┤
│ 19h │ SMPLRT_DIV │ SMPLRT_DIV[7:0] │
├───────┼────────────────────────────┼─────────┬─────────┬─────────────────────────────┬─────────────────────────────┤
│ 1Ah │ CONFIG │ --- │ --- │ EXT_SYNC_SET[2:0] │ SLPF_CFG[2:0] │
├───────┼────────────────────────────┼─────────┼─────────┼─────────┬───────────────────┼─────────┬─────────┬─────────┤
│ 1Bh │ GYRO_CONFIG │ XG_ST │ YG_ST │ ZG_ST │ FS_SEL[A:0] │ --- │ --- │ --- │
├───────┼────────────────────────────┼─────────┼─────────┼─────────┼───────────────────┼─────────┴─────────┴─────────┤
│ 1Ch │ ACCEL_CONFIG │ XA_ST │ YA_ST │ ZA_ST │ AFS_SEL[A:0] │ ACCEL_HPF[2:0] │
├───────┼────────────────────────────┼─────────┴─────────┴─────────┴───────────────────┴─────────────────────────────┤
│ 1Dh │ FF_THR │ FF_THR[7:0] │ * Register map V4.2 omits these registers
├───────┼────────────────────────────┼───────────────────────────────────────────────────────────────────────────────┤
│ 1Eh │ FF_DUR │ FF_DUR[7:0] │ *
├───────┼────────────────────────────┼───────────────────────────────────────────────────────────────────────────────┤
│ 1Fh │ MOT_THR │ MOT_THR[7:0] │ *
├───────┼────────────────────────────┼───────────────────────────────────────────────────────────────────────────────┤
│ 20h │ MOT_DUR │ MOT_DUR[7:0] │ *
├───────┼────────────────────────────┼───────────────────────────────────────────────────────────────────────────────┤
│ 21h │ ZRMOT_THR │ ZRMOT_THR[7:0] │ *
├───────┼────────────────────────────┼───────────────────────────────────────────────────────────────────────────────┤
│ 22h │ ZRMOT_DUR │ ZRMOT_DUR[7:0] │ *
├───────┼────────────────────────────┼─────────┬─────────┬─────────┬─────────┬─────────┬─────────┬─────────┬─────────┤
│ 23h │ FIFO_EN │ TEMP │ XG │ YG │ ZG │ ACCEL │ SLV2 │ SLV1 │ SLV0 │
│ │ │ _FIFO_EN│ _FIFO_EN│ _FIFO_EN│ _FIFO_EN│ _FIFO_EN│ _FIFO_EN│ _FIFO_EN│ _FIFO_EN│
├───────┼────────────────────────────┼─────────┼─────────┼─────────┼─────────┼─────────┴─────────┴─────────┴─────────┤
│ 24h │ I2C_MST_CTRL │ MULT │ WAIT │ SLV3 │ I2C_MST │ I2C_MST_CLK[3:0] │
│ │ │ _MST_EN │ _FOR_ES │ _FIFO_EN│ _P_NSR │ │
├───────┼────────────────────────────┼─────────┼─────────┴─────────┴─────────┴───────────────────────────────────────┤
│ 25h │ I2C_SLV0_ADDR │ I2C_SLV0│ I2C_SLV0_ADDR[6:0] │
│ │ │ _RW │ │
├───────┼────────────────────────────┼─────────┴─────────────────────────────────────────────────────────────────────┤
│ 26h │ I2C_SLV0_REG │ I2C_SLV0_REG[7:0] │
├───────┼────────────────────────────┼─────────┬─────────┬─────────┬─────────┬───────────────────────────────────────┤
│ 27h │ I2C_SLV0_CTRL │ I2C_SLV0│ I2C_SLV0│ I2C_SLV0│ I2C_SLV0│ I2C_SLV0_LEN[3:0] │
│ │ │ _EN │ _BYTE_SW│ _REG_DIS│ _GRP │ │
├───────┼────────────────────────────┼─────────┼─────────┴─────────┴─────────┴───────────────────────────────────────┤
│ 28h │ I2C_SLV1_ADDR │ I2C_SLV1│ I2C_SLV1_ADDR[6:0] │
│ │ │ _RW │ │
├───────┼────────────────────────────┼─────────┴─────────────────────────────────────────────────────────────────────┤
│ 29h │ I2C_SLV1_REG │ I2C_SLV1_REG[7:0] │
├───────┼────────────────────────────┼─────────┬─────────┬─────────┬─────────┬───────────────────────────────────────┤
│ 2Ah │ I2C_SLV1_CTRL │ I2C_SLV1│ I2C_SLV1│ I2C_SLV1│ I2C_SLV1│ I2C_SLV1_LEN[3:0] │
│ │ │ _EN │ _BYTE_SW│ _REG_DIS│ _GRP │ │
├───────┼────────────────────────────┼─────────┼─────────┴─────────┴─────────┴───────────────────────────────────────┤
│ 2Bh │ I2C_SLV2_ADDR │ I2C_SLV2│ I2C_SLV2_ADDR[6:0] │
│ │ │ _RW │ │
├───────┼────────────────────────────┼─────────┴─────────────────────────────────────────────────────────────────────┤
│ 2Ch │ I2C_SLV2_REG │ I2C_SLV2_REG[7:0] │
├───────┼────────────────────────────┼─────────┬─────────┬─────────┬─────────┬───────────────────────────────────────┤
│ 2Dh │ I2C_SLV2_CTRL │ I2C_SLV2│ I2C_SLV2│ I2C_SLV2│ I2C_SLV2│ I2C_SLV2_LEN[3:0] │
│ │ │ _EN │ _BYTE_SW│ _REG_DIS│ _GRP │ │
├───────┼────────────────────────────┼─────────┼─────────┴─────────┴─────────┴───────────────────────────────────────┤
│ 2Eh │ I2C_SLV3_ADDR │ I2C_SLV3│ I2C_SLV3_ADDR[6:0] │
│ │ │ _RW │ │
├───────┼────────────────────────────┼─────────┴─────────────────────────────────────────────────────────────────────┤
│ 2Fh │ I2C_SLV3_REG │ I2C_SLV3_REG[7:0] │
├───────┼────────────────────────────┼─────────┬─────────┬─────────┬─────────┬───────────────────────────────────────┤
│ 30h │ I2C_SLV3_CTRL │ I2C_SLV3│ I2C_SLV3│ I2C_SLV3│ I2C_SLV3│ I2C_SLV3_LEN[3:0] │
│ │ │ _EN │ _BYTE_SW│ _REG_DIS│ _GRP │ │
├───────┼────────────────────────────┼─────────┼─────────┴─────────┴─────────┴───────────────────────────────────────┤
│ 31h │ I2C_SLV4_ADDR │ I2C_SLV4│ I2C_SLV4_ADDR[6:0] │
│ │ │ _RW │ │
├───────┼────────────────────────────┼─────────┴─────────────────────────────────────────────────────────────────────┤
│ 32h │ I2C_SLV4_REG │ I2C_SLV4_REG[7:0] │
├───────┼────────────────────────────┼───────────────────────────────────────────────────────────────────────────────┤
│ 33h │ I2C_SLV4_DO │ I2C_SLV4_DO[7:0] │
├───────┼────────────────────────────┼─────────┬─────────┬─────────┬─────────────────────────────────────────────────┤
│ 34h │ I2C_SLV4_CTRL │ I2C_SLV4│ I2C_SLV4│ I2C_SLV4│ I2C_MST_DLY[3:0] │
│ │ │ _EN │ _INT_EN │ _REG_DIS│ │
├───────┼────────────────────────────┼─────────┴─────────┴─────────┴─────────────────────────────────────────────────┤
│ 35h │ I2C_SLV4_DI │ I2C_SLV4_DI[7:0] │
├───────┼────────────────────────────┼─────────┬─────────┬─────────┬─────────┬─────────┬─────────┬─────────┬─────────┤
│ 36h │ I2C_MST_STATUS │ PASS_ │ I2C_SLV4│ I2C_LOST│ I2C_SLV4│ I2C_SLV3│ I2C_SLV2│ I2C_SLV1│ I2C_SLV0│
│ │ │ THROUGH │ _DONE │ _ARB │ _NACK │ _NACK │ _NACK │ _NACK │ _NACK │
├───────┼────────────────────────────┼─────────┼─────────┼─────────┼─────────┼─────────┼─────────┼─────────┼─────────┤
│ 37h │ INT_PIN_CONFIG │ INT │ INT │ LATCH │ INT_RD │ FSYNC_ │ FSYNC │ I2C │ CLKOUT │
│ │ │ _LEVEL │ _OPEN │ _INT_EN │ _CLEAR │INT_LEVEL│ _ENT_EN │BYPASS_EN│ _EN │
├───────┼────────────────────────────┼─────────┼─────────┼─────────┼─────────┼─────────┼─────────┼─────────┼─────────┤
│ 38h │ INT_ENABLE │ FF_EN │ MOT_EN │ ZMOT_EN │ FIFO_ │ I2C_MST │ --- │ --- │ DATA │
│ │ │ │ │ │ OFLOW_EN│ _INT_EN │ │ │ _RDY_EN │
├───────┼────────────────────────────┼─────────┼─────────┼─────────┼─────────┼─────────┼─────────┼─────────┼─────────┤
│ 3Ah │ INT_STATUS │ FF_INT │ MOT_INT│ ZMOT_INT│ FIFO_ │ I2C_MST │ --- │ --- │ DATA │
│ │ │ │ │ │OFLOW_INT│ _INT │ │ │ _RDY_INT│
├───────┼────────────────────────────┼─────────┴─────────┴─────────┴─────────┴─────────┴─────────┴─────────┴─────────┤
│ 3Bh │ ACCEL_XOUT_H │ ACCEL_XOUT[15:8] │
├───────┼────────────────────────────┼───────────────────────────────────────────────────────────────────────────────┤
│ 3Ch │ ACCEL_XOUT_L │ ACCEL_XOUT[7:0] │
├───────┼────────────────────────────┼───────────────────────────────────────────────────────────────────────────────┤
│ 3Dh │ ACCEL_YOUT_H │ ACCEL_YOUT[15:8] │
├───────┼────────────────────────────┼───────────────────────────────────────────────────────────────────────────────┤
│ 3Eh │ ACCEL_YOUT_L │ ACCEL_YOUT[7:0] │
├───────┼────────────────────────────┼───────────────────────────────────────────────────────────────────────────────┤
│ 3Fh │ ACCEL_ZOUT_H │ ACCEL_ZOUT[15:8] │
├───────┼────────────────────────────┼───────────────────────────────────────────────────────────────────────────────┤
│ 40h │ ACCEL_ZOUT_L │ ACCEL_ZOUT[7:0] │
├───────┼────────────────────────────┼───────────────────────────────────────────────────────────────────────────────┤
│ 41h │ TEMP_OUT_H │ TEMP_OUT[15:8] │
├───────┼────────────────────────────┼───────────────────────────────────────────────────────────────────────────────┤
│ 42h │ TEMP_OUT_L │ TEMP_OUT[7:0] │
├───────┼────────────────────────────┼───────────────────────────────────────────────────────────────────────────────┤
│ 43h │ GYRO_XOUT_H │ GYRO_XOUT[15:8] │
├───────┼────────────────────────────┼───────────────────────────────────────────────────────────────────────────────┤
│ 44h │ GYRO_XOUT_L │ GYRO_XOUT[7:0] │
├───────┼────────────────────────────┼───────────────────────────────────────────────────────────────────────────────┤
│ 45h │ GYRO_YOUT_H │ GYRO_YOUT[15:8] │
├───────┼────────────────────────────┼───────────────────────────────────────────────────────────────────────────────┤
│ 46h │ GYRO_YOUT_L │ GYRO_YOUT[7:0] │
├───────┼────────────────────────────┼───────────────────────────────────────────────────────────────────────────────┤
│ 47h │ GYRO_ZOUT_H │ GYRO_ZOUT[15:8] │
├───────┼────────────────────────────┼───────────────────────────────────────────────────────────────────────────────┤
│ 48h │ GYRO_ZOUT_L │ GYRO_ZOUT[7:0] │
├───────┼────────────────────────────┼───────────────────────────────────────────────────────────────────────────────┤
│ 49h │ EXT_SENS_DATA_00 │ EXT_SENS_DATA_00[7:0] │
├───────┼────────────────────────────┼───────────────────────────────────────────────────────────────────────────────┤
│ 4Ah │ EXT_SENS_DATA_01 │ EXT_SENS_DATA_01[7:0] │
├───────┼────────────────────────────┼───────────────────────────────────────────────────
I've been testing the three and was wondering which one people prefer, or if anyone has had problems with a particular one? Faster?
Thanks!
Dave
I have purchase a HoverFlyGimal Experimenters board with 9Dof, somewhat similar code compexity.
The MPU-9150 has the ability to do its own rather proprietary senor fusion or to merely provide raw data for other choices.
Kalman filter was the original standard, but for 9Dof devices, Madjwick has taken the lead.
Madjwick produces results in Quaternion format; other choices are Directional Cosine Matrix or Euler Angles.
=========
So the problem of which of the three objects is the fastest or easiest to learn is pretty much dependent on where you are starting from and where you desire to go.
I have read that Madjwick will outperform internal sensor fusion on the MPU-9150. So I have pretty much accepted personal learning goals of Madjwick, Quaternion output, and conversion of Quaternion output to Direction Cosine Matrix.
Euler Angles have a technical problem that is called Gimbal Lock.
++++++++++
Other issues with code are whether you merely want to Demo the device or you have a particular applications, such as a Quad-copter.
Demo code will work better as an entry level means of learning. Code that is actually intended for flight will have many more features attached and may be overwhelming for some.
I guess you will have to open each one and survey the code for what you feel is best for you. Jason Dorie has other 9Dof code that is also available, has been up-graded to Madjwick, and has a graphic viewing pack that can be compiled from C## or can be view on a Propeller driven VGA via Dave Hein's code.
I'm working on a project that has been on and off of my self a couple of times. Last summer I created my own 6 DOF Kalman filter using separate accel and gyro that ran at about 80 Hz. I think that's fast enough, but who knows. Recently I've decided to pick up the project again I thought it would be good to try and use an "all in one" chip. My goal is pretty lofty, an autonomous flying "ball" similar to this one. http://youtu.be/55d5ppwQBQ4
Have fun!
Dave
During testing I noticed that the sampling rate from both versions was pretty much identical; the continuously sampling version spends a great deal of its time waiting for the sensor. The only real difference was when using a Spin-based I2C driver, taking an extra couple hundred milliseconds or so between samples.
I haven't attempted anything with the built-in digital motion processor to produce what InvenSense calls MotionFusion, it's locked behind proprietary code, but this loader routine looks translatable. I can sort of follow along with this I2C traffic dump, which makes use of some more undocumented registers. Even that will only fuse the accelerometer and gyroscope data, leaving the magnetometer to be handled externally. Supposedly the latest version of the SDK does combine everything.
Chris
Hey Chris, this looks great. You've even added scaling and pitch and roll!
I was able to play with it a little bit this morning and it looks like there is a disconnect between the Gyro and accel X &Y out put. (X Accel increase shows as Y increase on the Gyro).
Dave
I was wondering if you used a filter for calculating pitch and roll? They seemed stable. Is there a reason yaw is left out? Am I missing something?
I was also wondering if you had any thoughts on how to increase the speed that the program to ~100Hz? Do you think reading all the values form the gyro, accel, and compass at once instead of one at a time would help? I've used address pointer before that that seemed to really increase the speed that the program was able to run.
Thanks again for all of your help and insight. This is really a wonderful object that you've created!
Dave
For the sampling rate, the largest speed bump is in sampling the magnetometers. As it's currently written, the magnetometers are sampled 32 times and the average returned. If only a single sample is used, the entire loop runs at ~122Hz, and the readings still look quite stable.
MPU-9150 PASM driver 122Hz.spin
Chris