Shop OBEX P1 Docs P2 Docs Learn Events
i2c Nothing Works!!! - Page 4 — Parallax Forums

i2c Nothing Works!!!

124»

Comments

  • T ChapT Chap Posts: 4,241
    edited 2014-02-13 13:36
    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.
  • zlantzzlantz Posts: 136
    edited 2014-02-13 13:51
    T Chap wrote: »
    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).
  • T ChapT Chap Posts: 4,241
    edited 2014-02-13 14:29
    You already have been "retrieving data from 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
  • User NameUser Name Posts: 1,451
    edited 2014-02-13 14:42
    zlantz:

    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.
  • zlantzzlantz Posts: 136
    edited 2014-02-13 14:46
    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.
  • T ChapT Chap Posts: 4,241
    edited 2014-02-13 19:18 
    WHO_AM_I                  long    $75

    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.
  • zlantzzlantz Posts: 136
    edited 2014-02-13 20:46
    T Chap wrote: »
    WHO_AM_I                  long    $75

    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
    ~~mZ

    This 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
    ~~gZ

    This 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.
Sign In or Register to comment.