In PASM, they set Set ACCEL_CONFIG register AFS_SEL bits to 2. Are you setting your range to the same value?
You may be best off just trying to dissect the PASM version since it is already working. The object seems to allow you to PST.DEC(MPU.GetRX) PST.DEC(MPU.GetTemp) etc. You should spend about 5 hours in the manual making notes and getting familiar with the various settings before trying to create an object for it. That device is very involved.
In PASM, they set Set ACCEL_CONFIG register AFS_SEL bits to 2. Are you setting your range to the same value?
The Config bits are chosen in the Start# functions, that was my first experience of learning pasm (using user chosen values instead of the preset values). The output should scale accordingly as it does in the pasm driver, just experience more sensitivity by adjusting those values.
You may be best off just trying to dissect the PASM version since it is already working. The object seems to allow you to PST.DEC(MPU.GetRX) PST.DEC(MPU.GetTemp) etc. You should spend about 5 hours in the manual making notes and getting familiar with the various settings before trying to create an object for it. That device is very involved.
I was just thinking the same thing, now that I know how to read the mag in spin. After the other day I have a fair understanding of how the PASM driver flows... not entirely sure how it works, but I can see how to copy the existing code with slight changes to do what I want.
My biggest conscern is, how do you retrieve data From the pasm read section? At no point is iaX or paX copied to aX, but aX (spin) holds the value of iaX/paX (pasm).
You just might find that PASM is a better friend than you ever imagined. It integrates amazingly well with SPIN, and the respective strengths of SPIN and PASM complement each other in a rather marvelous fashion.
I am trying to jump into pasm... learning a new language all over again is rough. But I think I am getting it.
I managed to get it to return something for the Chip ID, its returning 0xD1 but it should be 0x68.
once I can figure out how to get pasm to give me the right chip id, I should be able to get the mag data with the same method.
also now my Accel X and Gyro Z value is off in my pasm code, but right in the 6050_pasm code.
'' 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
{
*******************************************
* MPU-9150_Spin *
* by: Zack Lantz *
*******************************************
}
_clkmode = xtal1 + pll16x
_xinfreq = 5_000_000
CLK_FREQ = ((_clkmode - xtal1) >> 6) * _xinfreq ' system freq as a constant
MS_001 = CLK_FREQ / 1_000 ' ticks in 1ms
US_001 = CLK_FREQ / 1_000_000 ' ticks in 1us
' // AK8975 Register Map
{
Name Address READ/WRITE Description Bit width Explanation
WIA 00H READ Device ID 8
INFO 01H READ Information 8
ST1 02H READ Status 1 8 Data status
HXL 03H READ Measurement data 8 X-axis data
HXH 04H READ Measurement data 8 X-axis data
HYL 05H READ Measurement data 8 Y-axis data
HYH 06H READ Measurement data 8 Y-axis data
HZL 07H READ Measurement data 8 Z-axis data
HZH 08H READ Measurement data 8 Z-axis data
ST2 09H READ Status 2 8 Data status
CNTL 0AH READ/WRITE Control 8
RSV 0BH READ/WRITE Reserved 8 DO NOT ACCESS
ASTC 0CH READ/WRITE Self-test 8
TS1 0DH READ/WRITE Test 1 8 DO NOT ACCESS
TS2 0EH READ/WRITE Test 2 8 DO NOT ACCESS
I2CDIS 0FH READ/WRITE I2C disable 8
ASAX 10H READ X-axis sensitivity adjustment value 8 Fuse ROM
ASAY 11H READ Y-axis sensitivity adjustment value 8 Fuse ROM
ASAZ 12H READ Z-axis sensitivity adjustment value 8 Fuse ROM
}
' // MPU-6050 Register Map
Con
Self_Test_X = $0D ' 13
Self_Test_Y = $0E ' 14
Self_Test_Z = $0F ' 15
Self_Test_A = $10 ' 15
SMPLRT_Div = $19 ' 25
Config = $1A ' 26
Gyro_Config = $1B ' 27
Accel_Config = $1C ' 28
Mot_Thr = $1F ' 31
FIFO_En = $23 ' 35
I2C_Mst_Ctrl = $24 ' 36
I2C_Slv0_Addr = $25 ' 37
I2C_Slv0_Reg = $26 ' 38
I2C_Slv0_Ctrl = $27 ' 39
I2C_Slv1_Addr = $28 ' 40
I2C_Slv1_Reg = $29 ' 41
I2C_Slv1_Ctrl = $2A ' 42
I2C_Slv2_Addr = $2B ' 43
I2C_Slv2_Reg = $2C ' 44
I2C_Slv2_Ctrl = $2D ' 45
I2C_Slv3_Addr = $2E ' 46
I2C_Slv3_Reg = $2F ' 47
I2C_Slv3_Ctrl = $30 ' 48
I2C_Slv4_Addr = $31 ' 49
I2C_Slv4_Reg = $32 ' 50
I2C_Slv4_Do = $33 ' 51
I2C_Slv4_Ctrl = $34 ' 52
I2C_Slv4_Di = $35 ' 53
I2C_Mst_Status = $36 ' 54
INT_Pin_Cfg = $37 ' 55
INT_Enable = $38 ' 56
INT_Status = $3A ' 58
Accel_XOut_H = $3B ' 59
Accel_XOut_L = $3C ' 60
Accel_YOut_H = $3D ' 61
Accel_YOut_L = $3E ' 62
Accel_ZOut_H = $3F ' 63
Accel_ZOut_L = $40 ' 64
Temp_Out_H = $41 ' 65
Temp_Out_L = $42 ' 66
Gyro_XOut_H = $43 ' 67
Gyro_XOut_L = $44 ' 68
Gyro_YOut_H = $45 ' 69
Gyro_YOut_L = $46 ' 70
Gyro_ZOut_H = $47 ' 71
Gyro_ZOut_L = $48 ' 72
Ext_Sens_Data_00 = $49 ' 73
Ext_Sens_Data_01 = $4A ' 74
Ext_Sens_Data_02 = $4B ' 75
Ext_Sens_Data_03 = $4C ' 76
Ext_Sens_Data_04 = $4D ' 77
Ext_Sens_Data_05 = $4E ' 78
Ext_Sens_Data_06 = $4F ' 79
Ext_Sens_Data_07 = $50 ' 80
Ext_Sens_Data_08 = $51 ' 81
Ext_Sens_Data_09 = $52 ' 82
Ext_Sens_Data_10 = $53 ' 83
Ext_Sens_Data_11 = $54 ' 84
Ext_Sens_Data_12 = $55 ' 85
Ext_Sens_Data_13 = $56 ' 86
Ext_Sens_Data_14 = $57 ' 87
Ext_Sens_Data_15 = $58 ' 88
Ext_Sens_Data_16 = $59 ' 89
Ext_Sens_Data_17 = $5A ' 90
Ext_Sens_Data_18 = $5B ' 91
Ext_Sens_Data_19 = $5C ' 92
Ext_Sens_Data_20 = $5D ' 93
Ext_Sens_Data_21 = $5E ' 94
Ext_Sens_Data_22 = $5F ' 95
Ext_Sens_Data_23 = $60 ' 96
I2C_Slv0_Do = $63 ' 99
I2C_Slv1_Do = $64 ' 100
I2C_Slv2_Do = $65 ' 101
I2C_Slv3_Do = $66 ' 102
I2C_Mst_Delay_Ctrl = $67 ' 103
Signal_Path_Reset = $68 ' 104
Mot_Detect_Ctrl = $69 ' 105
User_Ctrl = $6A ' 106
PWR_MGMT_1 = $6B ' 107
PWR_MGMT_2 = $6C ' 108
FIFO_CountH = $72 ' 114
FIFO_CountL = $73 ' 115
FIFO_R_W = $74 ' 116
WHO_AM_I = $75 ' 117
CON ' CONs for TestMPU test routine
SDA_PIN = 10
SCL_PIN = 11
SERIAL_TX_PIN = 30
SERIAL_RX_PIN = 31
VAR
long Cog
' // Zero Calibration, Measurement Data, and Status Storage
long x0, y0, z0, a0, b0, c0, d0, e0, f0, t
' // PASM / Spin Storage
'long FIFOcnt, FIFOrw, mID, mInfo, mStatus, PT_En, MM_En, FIFO_Enable, Verify_Registers, drift
' // PASM Returns
long rx, ry, rz, temp, ax, ay, az, arx, ary, mID, mInfo, mStatus, mX, mY, mZ, cID ' PASM code assumes these to be contiguous
'long E1, E2, E3, E4, E5, E6, E7, E8, E9, E10, E11, E12 ' 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.hex(GetChipID, 2)
debug.str(string(" "))
debug.dec(GetAXz)
debug.str(string(", "))
debug.dec(GetAYz)
debug.str(string(", "))
debug.dec(GetAZz)
debug.str(string(" "))
debug.dec(GetRXz)
debug.str(string(", "))
debug.dec(GetRYz)
debug.str(string(", "))
debug.dec(GetRZz)
debug.str(string(" "))
debug.dec(GetMXz)
debug.str(string(", "))
debug.dec(GetMYz)
debug.str(string(", "))
debug.dec(GetMZz)
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
'**********************
' // Raw Values
Pub GetaX ' Accel X Raw
return rX 'aX
Pub GetaY ' Accel Y Raw
return rY 'aY
Pub GetaZ ' Accel Z Raw
return rZ 'aZ
Pub GetgX ' Gyro X Raw
return aX 'gX
Pub GetgY ' Gyro Y Raw
return aY 'gY
Pub GetgZ ' Gyro Z Raw
return aZ 'gZ
Pub GetmX ' Mag X Raw
return mX
Pub GetmY ' Mag Y Raw
return mY
Pub GetmZ ' Mag Z Raw
return mZ
PUB GetTemp ' Temperature
return temp
'Pub GetTempF ' Temp Deg F
'Pub GetTempC ' Temp Deg C
PUB GetRXz ' Accel X - Zero Offset
return rx - x0
PUB GetRYz ' Accel Y - Zero Offset
return ry - y0
PUB GetRZz ' Accel Z - Zero Offset
return rz - z0
PUB GetAXz ' Gyro X - Zero Offset
return ax - a0
PUB GetAYz ' Gyro Y - Zero Offset
return ay - b0
PUB GetAZz ' Gyro Z - Zero Offset
return az - c0
PUB GetMXz ' Mag X - Zero Offset
return mx - d0
PUB GetMYz ' Mag Y - Zero Offset
return my - e0
PUB GetMZz ' Mag Z - Zero Offset
return mz - f0
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 GetMagOffsetX ' Magnetometer Zero Offset X
return a0
Pub GetMagOffsetY ' Magnetometer Zero Offset Y
return b0
Pub GetMagOffsetZ ' Magnetometer Zero Offset Z
return c0
Pub GetChipID
return cID
Pub CalAccel ' Calibrate Accelerometer
CalibrateAccel
Pub CalGyro ' Calibrate Gyroscope
CalibrateGyro
Pub CalMag ' Calibrate Magnetometer
CalibrateMag
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
' // Calibrations Copied from MPU-6050 PASM
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 ' aX
yc += rY ' aY
zc += rZ ' 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
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 ' gX
yc += aX ' gY
zc += aX ' gZ
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
' // Resets All Registers on MPU
Pri FactoryReset | i
{
' // *** Reset Value is 0x00 for all registers other than:
' // Register 107: 0x40 (PWR_MGMT_1)
' // Register 117: 0x68 (WHO_AM_I)
repeat i from $00 to $75
Write_Register(i, $00)
if i == PWR_MGMT_1
Write_Register(PWR_MGMT_1, $40) ' Device Off
if i == WHO_AM_I
Write_Register(WHO_AM_I, $68) ' Device ID = 0x68
}
' // Configure Digital Motion Processor - Uses FIFO for Data Access
Pri SetDMP
' // TODO: Addin DMP Control Setup / Load DMP Firmware to MPU Memory
' // Configure FIFO - Used for DMP
Pri SetFIFO
' // TODO: Addin FIFO Control Setup
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 pamID, p1 ' Store the pointer to the ax var in HUB RAM
add p1, #4
mov pamInfo, p1 ' Store the pointer to the ay var in HUB RAM
add p1, #4
mov pamStatus, p1 ' Store the pointer to the az var in HUB RAM
add p1, #4
mov pamX, p1 ' Store the pointer to the ax var in HUB RAM
add p1, #4
mov pamY, p1 ' Store the pointer to the ay var in HUB RAM
add p1, #4
mov pamZ, p1 ' Store the pointer to the az var in HUB RAM
add p1, #4
mov pacID, p1 ' Store the pointer to the az var in HUB RAM
' // cID, E1 threw E12
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
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
wrlong iaRX, paRX
wrlong iaRY, paRY
wrlong iamID, pamID
wrlong iamInfo, pamInfo
wrlong iamStatus, pamStatus
wrlong iaMX, paMX
wrlong iaMY, paMY
wrlong iaMZ, paMZ
wrlong iacID, pacID
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
' // Read Accel, Temp, Gyro (2 Bytes Each)
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
[B]
' // Should Read 0x68, Currently Reading 0xD1
' // Read Chip ID (1 Byte)
mov i2cAddr, WHO_AM_I ' Address of Magnetometer to Read
mov i2cDevID, #%11010000 ' Device ID of the MPU
call #StartRead ' Tell the I2C device we're starting
mov i2cMask, i2cWordReadMask
call #i2cRead
test i2cTestCarry, #1 wc ' Set the carry flag to tell it we're done
muxc i2cData, i2cWordMask
mov iacID, i2cData
[/B]
' // Read Magnetometer via AUX Bus
mov i2cAddr, $00 ' Address of Magnetometer Start Read
mov i2cDevID, #%00011000 ' Device ID of the Magnetometer
call #StartRead ' Tell the I2C device we're starting
' // Read Config Info
mov i2cMask, i2cWordReadMask
test i2cTestCarry, #1 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 iamID, i2cData
mov i2cAddr, $00 ' Address of Magnetometer Start Read
mov i2cDevID, #%00011000 ' Device ID of the Magnetometer
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
'call #i2cRead
'Sign extend the 15th bit
test i2cData, i2cWordReadMask wc
muxc i2cData, i2cWordMask
mov iamInfo, i2cData
mov i2cAddr, $00 ' Address of Magnetometer Start Read
mov i2cDevID, #%00011000 ' Device ID of the Magnetometer
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
'call #i2cRead
'Sign extend the 15th bit
test i2cData, i2cWordReadMask wc
muxc i2cData, i2cWordMask
mov iamStatus, i2cData
mov i2cAddr, $00 ' Address of Magnetometer Start Read
mov i2cDevID, #%00011000 ' Device ID of the Magnetometer
call #StartRead ' Tell the I2C device we're starting
' // Start Measurment Readings
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 iaMX, 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 iaMY, 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 iaMZ, i2cData
call #i2cStop
MPUReadValues_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
{
mov i2cAddr, #107 'Set PWR_MGMT_1 register bit 0 to choose
mov i2cValue, #%00000001 ' X gyro as clock source '
call #i2cWriteRegisterByte
}
' // Digital Low Pass Filter _ Config
{
mov i2cAddr, #26
mov i2cValue, #%00000100 'Set DLPF_CONFIG to 4 for 20Hz bandwidth
call #i2cWriteRegisterByte
}
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+1) = 500hz sample rate
mov i2cValue, #%00000001
call #i2cWriteRegisterByte
}
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
{
' // Gyro _ Config
mov i2cAddr, #27 'GYRO_CONFIG register, set FS_SEL bits to 3 gives a
mov i2cValue, #%00011000 ' full scale range of +-2000 deg/sec
call #i2cWriteRegisterByte
}
' // Accel _ Config
{
mov i2cAddr, #28 'Set ACCEL_CONFIG register AFS_SEL bits to 1,
'mov i2cValue, #%00001000 ' sets +-4g full scale range
mov i2cValue, #%00010000 ' sets +-8g full scale range
call #i2cWriteRegisterByte 'ACCEL_HPF is zero which turns off high-pass filtering
}
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
' Write_Register(User_Ctrl, %00000000) ' 106 - Disable Master Mode
' Write_Register(INT_Pin_Cfg, %00110010) ' 55 - INT_Pin_Cfg i2c Bypass Enabled
mov i2cAddr, User_Ctrl ' Disable Master Mode
mov i2cValue, #%00000000
call #i2cWriteRegisterByte
mov i2cAddr, INT_Pin_Cfg ' Enable Pass-Through Mode
mov i2cValue, #%00110010
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
pamID long 0
pamInfo long 0
pamStatus long 0
paMX long 0
paMY long 0
paMZ long 0
pacID 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
iamID long 0
iamInfo long 0
iamStatus long 0
iaMX long 0
iaMY long 0
iaMZ long 0
iacID 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
' // PASM / Spin Control Settings
FIFOcnt long %00000000
FIFOrw long %00000000
'mID long %00000000
'mInfo long %00000000
'mStatus long %00000000
MM_En long 0
FIFO_Enable long 0
Verify_Registers long 0
' // MPU-6050 User Set Init Settings
GyroFS long %00000000
AccelFS long %00000000
PowerMgmt long %00000000
SampleRate long %00000000
DLP long %00000000
'' Local variables for low level I2C routines
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
Edit: I cannot for the life of me, get pasm to pass info back to spin properly. Or it is not reading it properly or storing it properly in pasm. I really have no idea... Ever try speaking a language you dont know?
I think I will have better luck gettin my Spin driver working, as it mostly is working now. Just need to get the numbers in the right spot on the numberline. Back to the language thing.
My pasm driver gives idental results as My spin driver, except for No Mag data or chip id. And all I did was Copy the 6050 pasm driver, and add extra results to it. I didnt change any of the original code.
Edit2: Just worked on the Spin driver a bit more, it appears that all outputs are correct & to scale.
Maybe someone can confirm this, but I am not sure you can use a constant declared in your top program as a valid number in pasm like you are doing. Instead, try to use the device ID as shown by putting it in DAT.
Maybe someone can confirm this, but I am not sure you can use a constant declared in your top program as a valid number in pasm like you are doing. Instead, try to use the device ID as shown by putting it in DAT.
It didnt do much. I think im going to put the PASM version off to the side as I am horrible with pasm.
I have my Spin driver working near 100%... I have a little bit shifting question...
The accel, temp, & gyro reads 2 bits per axis, High bit First
The Mag reads 2 bits per axis, Low bit First.
I wish pasm was a skill of mine, because the speed at which this runs is much faster. Potentially I could just bit shift to do the same as what I did above much faster, but bit shifting is about the same as pasm to me, a new language. I could try using spin2cpp so I can compile my project with PropellerC, that should make it faster. I'll get it one day!
Any good asm / pasm books to refrence for learning? I have just ordered the 2 mentioned earlier in this thread.
Comments
You may be best off just trying to dissect the PASM version since it is already working. The object seems to allow you to PST.DEC(MPU.GetRX) PST.DEC(MPU.GetTemp) etc. You should spend about 5 hours in the manual making notes and getting familiar with the various settings before trying to create an object for it. That device is very involved.
The Config bits are chosen in the Start# functions, that was my first experience of learning pasm (using user chosen values instead of the preset values). The output should scale accordingly as it does in the pasm driver, just experience more sensitivity by adjusting those values.
I was just thinking the same thing, now that I know how to read the mag in spin. After the other day I have a fair understanding of how the PASM driver flows... not entirely sure how it works, but I can see how to copy the existing code with slight changes to do what I want.
My biggest conscern is, how do you retrieve data From the pasm read section? At no point is iaX or paX copied to aX, but aX (spin) holds the value of iaX/paX (pasm).
ser.dec(aX)
wrlong iaX, paX 'take the pasm value of ax 9iaX) and write it to hub ram at the pointer to ax (pax)... which is aX in hub.
iaX long 0 ' Interim aX value this will get written to hub ram so your main program can use it
You just might find that PASM is a better friend than you ever imagined.
I managed to get it to return something for the Chip ID, its returning 0xD1 but it should be 0x68.
once I can figure out how to get pasm to give me the right chip id, I should be able to get the mag data with the same method.
also now my Accel X and Gyro Z value is off in my pasm code, but right in the 6050_pasm code.
'' 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 { ******************************************* * MPU-9150_Spin * * by: Zack Lantz * ******************************************* } _clkmode = xtal1 + pll16x _xinfreq = 5_000_000 CLK_FREQ = ((_clkmode - xtal1) >> 6) * _xinfreq ' system freq as a constant MS_001 = CLK_FREQ / 1_000 ' ticks in 1ms US_001 = CLK_FREQ / 1_000_000 ' ticks in 1us ' // AK8975 Register Map { Name Address READ/WRITE Description Bit width Explanation WIA 00H READ Device ID 8 INFO 01H READ Information 8 ST1 02H READ Status 1 8 Data status HXL 03H READ Measurement data 8 X-axis data HXH 04H READ Measurement data 8 X-axis data HYL 05H READ Measurement data 8 Y-axis data HYH 06H READ Measurement data 8 Y-axis data HZL 07H READ Measurement data 8 Z-axis data HZH 08H READ Measurement data 8 Z-axis data ST2 09H READ Status 2 8 Data status CNTL 0AH READ/WRITE Control 8 RSV 0BH READ/WRITE Reserved 8 DO NOT ACCESS ASTC 0CH READ/WRITE Self-test 8 TS1 0DH READ/WRITE Test 1 8 DO NOT ACCESS TS2 0EH READ/WRITE Test 2 8 DO NOT ACCESS I2CDIS 0FH READ/WRITE I2C disable 8 ASAX 10H READ X-axis sensitivity adjustment value 8 Fuse ROM ASAY 11H READ Y-axis sensitivity adjustment value 8 Fuse ROM ASAZ 12H READ Z-axis sensitivity adjustment value 8 Fuse ROM } ' // MPU-6050 Register Map Con Self_Test_X = $0D ' 13 Self_Test_Y = $0E ' 14 Self_Test_Z = $0F ' 15 Self_Test_A = $10 ' 15 SMPLRT_Div = $19 ' 25 Config = $1A ' 26 Gyro_Config = $1B ' 27 Accel_Config = $1C ' 28 Mot_Thr = $1F ' 31 FIFO_En = $23 ' 35 I2C_Mst_Ctrl = $24 ' 36 I2C_Slv0_Addr = $25 ' 37 I2C_Slv0_Reg = $26 ' 38 I2C_Slv0_Ctrl = $27 ' 39 I2C_Slv1_Addr = $28 ' 40 I2C_Slv1_Reg = $29 ' 41 I2C_Slv1_Ctrl = $2A ' 42 I2C_Slv2_Addr = $2B ' 43 I2C_Slv2_Reg = $2C ' 44 I2C_Slv2_Ctrl = $2D ' 45 I2C_Slv3_Addr = $2E ' 46 I2C_Slv3_Reg = $2F ' 47 I2C_Slv3_Ctrl = $30 ' 48 I2C_Slv4_Addr = $31 ' 49 I2C_Slv4_Reg = $32 ' 50 I2C_Slv4_Do = $33 ' 51 I2C_Slv4_Ctrl = $34 ' 52 I2C_Slv4_Di = $35 ' 53 I2C_Mst_Status = $36 ' 54 INT_Pin_Cfg = $37 ' 55 INT_Enable = $38 ' 56 INT_Status = $3A ' 58 Accel_XOut_H = $3B ' 59 Accel_XOut_L = $3C ' 60 Accel_YOut_H = $3D ' 61 Accel_YOut_L = $3E ' 62 Accel_ZOut_H = $3F ' 63 Accel_ZOut_L = $40 ' 64 Temp_Out_H = $41 ' 65 Temp_Out_L = $42 ' 66 Gyro_XOut_H = $43 ' 67 Gyro_XOut_L = $44 ' 68 Gyro_YOut_H = $45 ' 69 Gyro_YOut_L = $46 ' 70 Gyro_ZOut_H = $47 ' 71 Gyro_ZOut_L = $48 ' 72 Ext_Sens_Data_00 = $49 ' 73 Ext_Sens_Data_01 = $4A ' 74 Ext_Sens_Data_02 = $4B ' 75 Ext_Sens_Data_03 = $4C ' 76 Ext_Sens_Data_04 = $4D ' 77 Ext_Sens_Data_05 = $4E ' 78 Ext_Sens_Data_06 = $4F ' 79 Ext_Sens_Data_07 = $50 ' 80 Ext_Sens_Data_08 = $51 ' 81 Ext_Sens_Data_09 = $52 ' 82 Ext_Sens_Data_10 = $53 ' 83 Ext_Sens_Data_11 = $54 ' 84 Ext_Sens_Data_12 = $55 ' 85 Ext_Sens_Data_13 = $56 ' 86 Ext_Sens_Data_14 = $57 ' 87 Ext_Sens_Data_15 = $58 ' 88 Ext_Sens_Data_16 = $59 ' 89 Ext_Sens_Data_17 = $5A ' 90 Ext_Sens_Data_18 = $5B ' 91 Ext_Sens_Data_19 = $5C ' 92 Ext_Sens_Data_20 = $5D ' 93 Ext_Sens_Data_21 = $5E ' 94 Ext_Sens_Data_22 = $5F ' 95 Ext_Sens_Data_23 = $60 ' 96 I2C_Slv0_Do = $63 ' 99 I2C_Slv1_Do = $64 ' 100 I2C_Slv2_Do = $65 ' 101 I2C_Slv3_Do = $66 ' 102 I2C_Mst_Delay_Ctrl = $67 ' 103 Signal_Path_Reset = $68 ' 104 Mot_Detect_Ctrl = $69 ' 105 User_Ctrl = $6A ' 106 PWR_MGMT_1 = $6B ' 107 PWR_MGMT_2 = $6C ' 108 FIFO_CountH = $72 ' 114 FIFO_CountL = $73 ' 115 FIFO_R_W = $74 ' 116 WHO_AM_I = $75 ' 117 CON ' CONs for TestMPU test routine SDA_PIN = 10 SCL_PIN = 11 SERIAL_TX_PIN = 30 SERIAL_RX_PIN = 31 VAR long Cog ' // Zero Calibration, Measurement Data, and Status Storage long x0, y0, z0, a0, b0, c0, d0, e0, f0, t ' // PASM / Spin Storage 'long FIFOcnt, FIFOrw, mID, mInfo, mStatus, PT_En, MM_En, FIFO_Enable, Verify_Registers, drift ' // PASM Returns long rx, ry, rz, temp, ax, ay, az, arx, ary, mID, mInfo, mStatus, mX, mY, mZ, cID ' PASM code assumes these to be contiguous 'long E1, E2, E3, E4, E5, E6, E7, E8, E9, E10, E11, E12 ' 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.hex(GetChipID, 2) debug.str(string(" ")) debug.dec(GetAXz) debug.str(string(", ")) debug.dec(GetAYz) debug.str(string(", ")) debug.dec(GetAZz) debug.str(string(" ")) debug.dec(GetRXz) debug.str(string(", ")) debug.dec(GetRYz) debug.str(string(", ")) debug.dec(GetRZz) debug.str(string(" ")) debug.dec(GetMXz) debug.str(string(", ")) debug.dec(GetMYz) debug.str(string(", ")) debug.dec(GetMZz) 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 '********************** ' // Raw Values Pub GetaX ' Accel X Raw return rX 'aX Pub GetaY ' Accel Y Raw return rY 'aY Pub GetaZ ' Accel Z Raw return rZ 'aZ Pub GetgX ' Gyro X Raw return aX 'gX Pub GetgY ' Gyro Y Raw return aY 'gY Pub GetgZ ' Gyro Z Raw return aZ 'gZ Pub GetmX ' Mag X Raw return mX Pub GetmY ' Mag Y Raw return mY Pub GetmZ ' Mag Z Raw return mZ PUB GetTemp ' Temperature return temp 'Pub GetTempF ' Temp Deg F 'Pub GetTempC ' Temp Deg C PUB GetRXz ' Accel X - Zero Offset return rx - x0 PUB GetRYz ' Accel Y - Zero Offset return ry - y0 PUB GetRZz ' Accel Z - Zero Offset return rz - z0 PUB GetAXz ' Gyro X - Zero Offset return ax - a0 PUB GetAYz ' Gyro Y - Zero Offset return ay - b0 PUB GetAZz ' Gyro Z - Zero Offset return az - c0 PUB GetMXz ' Mag X - Zero Offset return mx - d0 PUB GetMYz ' Mag Y - Zero Offset return my - e0 PUB GetMZz ' Mag Z - Zero Offset return mz - f0 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 GetMagOffsetX ' Magnetometer Zero Offset X return a0 Pub GetMagOffsetY ' Magnetometer Zero Offset Y return b0 Pub GetMagOffsetZ ' Magnetometer Zero Offset Z return c0 Pub GetChipID return cID Pub CalAccel ' Calibrate Accelerometer CalibrateAccel Pub CalGyro ' Calibrate Gyroscope CalibrateGyro Pub CalMag ' Calibrate Magnetometer CalibrateMag 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 ' // Calibrations Copied from MPU-6050 PASM 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 ' aX yc += rY ' aY zc += rZ ' 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 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 ' gX yc += aX ' gY zc += aX ' gZ 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 ' // Resets All Registers on MPU Pri FactoryReset | i { ' // *** Reset Value is 0x00 for all registers other than: ' // Register 107: 0x40 (PWR_MGMT_1) ' // Register 117: 0x68 (WHO_AM_I) repeat i from $00 to $75 Write_Register(i, $00) if i == PWR_MGMT_1 Write_Register(PWR_MGMT_1, $40) ' Device Off if i == WHO_AM_I Write_Register(WHO_AM_I, $68) ' Device ID = 0x68 } ' // Configure Digital Motion Processor - Uses FIFO for Data Access Pri SetDMP ' // TODO: Addin DMP Control Setup / Load DMP Firmware to MPU Memory ' // Configure FIFO - Used for DMP Pri SetFIFO ' // TODO: Addin FIFO Control Setup 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 pamID, p1 ' Store the pointer to the ax var in HUB RAM add p1, #4 mov pamInfo, p1 ' Store the pointer to the ay var in HUB RAM add p1, #4 mov pamStatus, p1 ' Store the pointer to the az var in HUB RAM add p1, #4 mov pamX, p1 ' Store the pointer to the ax var in HUB RAM add p1, #4 mov pamY, p1 ' Store the pointer to the ay var in HUB RAM add p1, #4 mov pamZ, p1 ' Store the pointer to the az var in HUB RAM add p1, #4 mov pacID, p1 ' Store the pointer to the az var in HUB RAM ' // cID, E1 threw E12 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 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 wrlong iaRX, paRX wrlong iaRY, paRY wrlong iamID, pamID wrlong iamInfo, pamInfo wrlong iamStatus, pamStatus wrlong iaMX, paMX wrlong iaMY, paMY wrlong iaMZ, paMZ wrlong iacID, pacID 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 ' // Read Accel, Temp, Gyro (2 Bytes Each) 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 [B] ' // Should Read 0x68, Currently Reading 0xD1 ' // Read Chip ID (1 Byte) mov i2cAddr, WHO_AM_I ' Address of Magnetometer to Read mov i2cDevID, #%11010000 ' Device ID of the MPU call #StartRead ' Tell the I2C device we're starting mov i2cMask, i2cWordReadMask call #i2cRead test i2cTestCarry, #1 wc ' Set the carry flag to tell it we're done muxc i2cData, i2cWordMask mov iacID, i2cData [/B] ' // Read Magnetometer via AUX Bus mov i2cAddr, $00 ' Address of Magnetometer Start Read mov i2cDevID, #%00011000 ' Device ID of the Magnetometer call #StartRead ' Tell the I2C device we're starting ' // Read Config Info mov i2cMask, i2cWordReadMask test i2cTestCarry, #1 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 iamID, i2cData mov i2cAddr, $00 ' Address of Magnetometer Start Read mov i2cDevID, #%00011000 ' Device ID of the Magnetometer 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 'call #i2cRead 'Sign extend the 15th bit test i2cData, i2cWordReadMask wc muxc i2cData, i2cWordMask mov iamInfo, i2cData mov i2cAddr, $00 ' Address of Magnetometer Start Read mov i2cDevID, #%00011000 ' Device ID of the Magnetometer 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 'call #i2cRead 'Sign extend the 15th bit test i2cData, i2cWordReadMask wc muxc i2cData, i2cWordMask mov iamStatus, i2cData mov i2cAddr, $00 ' Address of Magnetometer Start Read mov i2cDevID, #%00011000 ' Device ID of the Magnetometer call #StartRead ' Tell the I2C device we're starting ' // Start Measurment Readings 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 iaMX, 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 iaMY, 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 iaMZ, i2cData call #i2cStop MPUReadValues_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 { mov i2cAddr, #107 'Set PWR_MGMT_1 register bit 0 to choose mov i2cValue, #%00000001 ' X gyro as clock source ' call #i2cWriteRegisterByte } ' // Digital Low Pass Filter _ Config { mov i2cAddr, #26 mov i2cValue, #%00000100 'Set DLPF_CONFIG to 4 for 20Hz bandwidth call #i2cWriteRegisterByte } 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+1) = 500hz sample rate mov i2cValue, #%00000001 call #i2cWriteRegisterByte } 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 { ' // Gyro _ Config mov i2cAddr, #27 'GYRO_CONFIG register, set FS_SEL bits to 3 gives a mov i2cValue, #%00011000 ' full scale range of +-2000 deg/sec call #i2cWriteRegisterByte } ' // Accel _ Config { mov i2cAddr, #28 'Set ACCEL_CONFIG register AFS_SEL bits to 1, 'mov i2cValue, #%00001000 ' sets +-4g full scale range mov i2cValue, #%00010000 ' sets +-8g full scale range call #i2cWriteRegisterByte 'ACCEL_HPF is zero which turns off high-pass filtering } 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 ' Write_Register(User_Ctrl, %00000000) ' 106 - Disable Master Mode ' Write_Register(INT_Pin_Cfg, %00110010) ' 55 - INT_Pin_Cfg i2c Bypass Enabled mov i2cAddr, User_Ctrl ' Disable Master Mode mov i2cValue, #%00000000 call #i2cWriteRegisterByte mov i2cAddr, INT_Pin_Cfg ' Enable Pass-Through Mode mov i2cValue, #%00110010 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 pamID long 0 pamInfo long 0 pamStatus long 0 paMX long 0 paMY long 0 paMZ long 0 pacID 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 iamID long 0 iamInfo long 0 iamStatus long 0 iaMX long 0 iaMY long 0 iaMZ long 0 iacID 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 ' // PASM / Spin Control Settings FIFOcnt long %00000000 FIFOrw long %00000000 'mID long %00000000 'mInfo long %00000000 'mStatus long %00000000 MM_En long 0 FIFO_Enable long 0 Verify_Registers long 0 ' // MPU-6050 User Set Init Settings GyroFS long %00000000 AccelFS long %00000000 PowerMgmt long %00000000 SampleRate long %00000000 DLP long %00000000 '' Local variables for low level I2C routines 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 496Edit: I cannot for the life of me, get pasm to pass info back to spin properly. Or it is not reading it properly or storing it properly in pasm. I really have no idea... Ever try speaking a language you dont know?
I think I will have better luck gettin my Spin driver working, as it mostly is working now. Just need to get the numbers in the right spot on the numberline. Back to the language thing.
My pasm driver gives idental results as My spin driver, except for No Mag data or chip id. And all I did was Copy the 6050 pasm driver, and add extra results to it. I didnt change any of the original code.
Edit2: Just worked on the Spin driver a bit more, it appears that all outputs are correct & to scale.
Maybe someone can confirm this, but I am not sure you can use a constant declared in your top program as a valid number in pasm like you are doing. Instead, try to use the device ID as shown by putting it in DAT.
It didnt do much. I think im going to put the PASM version off to the side as I am horrible with pasm.
I have my Spin driver working near 100%... I have a little bit shifting question...
The accel, temp, & gyro reads 2 bits per axis, High bit First
The Mag reads 2 bits per axis, Low bit First.
is this the right way to handle the bit shift?:
mX := i2cRead(0) ' Mag_X_L mX |= i2cRead(0) << 8 ' Mag_X_H mY := i2cRead(0) ' Mag_Y_L mY |= i2cRead(0) << 8 ' Mag_Y_H mZ := i2cRead(0) ' Mag_Z_L mZ |= i2cRead(1) << 8 ' Mag_Z_H ~~mX ~~mY ~~mZThis is the equivalent accel & gyro bit shift, the resulting values, once adjusted, gives clean proper results:
aX := i2cRead(0) << 8 aX |= i2cRead(0) aY := i2cRead(0) << 8 aY |= i2cRead(0) aZ := i2cRead(0) << 8 aZ |= i2cRead(0) Temp := i2cRead(0) << 8 Temp |= i2cRead(0) gX := i2cRead(0) << 8 gX |= i2cRead(0) gY := i2cRead(0) << 8 gY |= i2cRead(0) gZ := i2cRead(0) << 8 gZ |= i2cRead(1) ~~aX ~~aY ~~aZ ~~gX ~~gY ~~gZThis is the other associated math that converts the Raw values into something usable:
' // Update G-Force, Degrees per Second, and Mag Angle Pub UpdateForceVals | faX, faY, faZ, fgX, fgY, fgZ ' // Accelerometer faX := f.fSub(f.fFloat(aX), 32768.0) faY := f.fSub(f.fFloat(aY), 32768.0) faZ := f.fSub(f.fFloat(aZ), 32768.0) ' // Convert Accel Value to G-Force ' G-Force = Accel[x] / 8,192 (AFS-1) Accel[0] := f.fAdd(f.fDiv(faX, cAccelFS), 8.0) Accel[1] := f.fAdd(f.fDiv(faY, cAccelFS), 8.0) Accel[2] := f.fAdd(f.fDiv(faZ, cAccelFS), 8.0) ' // Gyroscope fgX := f.fSub(f.fFloat(gX), 32768.0) fgY := f.fSub(f.fFloat(gY), 32768.0) fgZ := f.fSub(f.fFloat(gZ), 32768.0) ' // Convert Gyro Value to Degrees per Second ' Gyro = Gyro[x] / 16.4 (FS-3) Gyro[0] := f.fAdd(f.fDiv(fgX, cGyroFS), 2048.0) Gyro[1] := f.fAdd(f.fDiv(fgY, cGyroFS), 2048.0) Gyro[2] := f.fAdd(f.fDiv(fgZ, cGyroFS), 2048.0) ' // Magnetometer Max ~= +/- 580 Mag[0] := f.fFloat(mX) Mag[1] := f.fFloat(mY) Mag[2] := f.fFloat(mZ)I wish pasm was a skill of mine, because the speed at which this runs is much faster. Potentially I could just bit shift to do the same as what I did above much faster, but bit shifting is about the same as pasm to me, a new language. I could try using spin2cpp so I can compile my project with PropellerC, that should make it faster. I'll get it one day!
Any good asm / pasm books to refrence for learning? I have just ordered the 2 mentioned earlier in this thread.