$regfile = "M32DEF.DAT"

$hwstack = 200
$swstack = 32
$framesize = 64

$crystal = 16000000

Config Sda = Portc.1                                        'AtMega32 TQFP Pin20
Config Scl = Portc.0                                        'AtMega32 TQFP Pin19
I2cinit

Config I2cdelay = 1

$baud = 19200                                               'RX Pin9, TX Pin10

Ddrd.2 = 1
Led Alias Portd.2

Config Timer0 = Timer , Prescale = 1024                     'counts 64µs, overflow at 16,384ms
On Timer0 T0isr
Enable Interrupts
Enable Timer0
Stop Timer0

Dim 16mscounter As Word

Config Timer1 = Timer , Prescale = 8                        'counts 0.5µs up to 32ms

'(
Dim A As Byte

Do
   Print "hallo"
   If Ischarwaiting() = 1 Then
      Inputbin A
      If A = 100 Then Toggle Led
   End If
   Wait 1
Loop
')

'#################################################################
'###Reading Gyro L3G4200D and Accelerometer & Compass LSM303DLH###
'##Module MinIMU-9 (L3G4200D and LSM303DLH Carrier) Pololu 1264###
'#################################################################

'##### 7- Bit Adresses Gyro ########
Const Who_am_i = &H0F
Const Ctrl_reg1 = &H20
Const Ctrl_reg2 = &H21
Const Ctrl_reg3 = &H22
Const Ctrl_reg4 = &H23
Const Ctrl_reg5 = &H24
Const Out_temp = &H26
Const Status_reg = &H27
Const Out_x_l = &H28
Const Out_x_h = &H29
Const Out_y_l = &H2A
Const Out_y_h = &H2B
Const Out_z_l = &H2C
Const Out_z_h = &H2D
'##### 7-Bit Adresses Acc ########
Const Ctrl_reg1_a = &H20
Const Ctrl_reg2_a = &H21
Const Ctrl_reg3_a = &H22
Const Ctrl_reg4_a = &H23
Const Ctrl_reg5_a = &H24
Const Status_reg_a = &H27
Const Out_x_l_a = &H28
Const Out_x_h_a = &H29
Const Out_y_l_a = &H2A
Const Out_y_h_a = &H2B
Const Out_z_l_a = &H2C
Const Out_z_h_a = &H2D
'##### 7-Bit Adresses Mag ########
Const Cra_reg_m = &H00
Const Crb_reg_m = &H01
Const Mr_reg_m = &H02
Const Out_x_h_m = &H03

Const Gyroadress = &HD2                                     'Gyro slave adress for write
Const Accadress = &H30                                      'Acc slave adress for write
Const Magadress = &H3C                                      'Mag slave adress for write

Dim Slaveadress As Byte                                     'Adress of the slave to read or write
Dim Subadress As Byte                                       'the register to read or write
Dim Databyte As Byte                                        'Byte to read or write
Dim Dataarray(6) As Byte                                    '6 Bytes to read
Dim Tempgyro As Byte

Dim Xgyro As Integer
Dim Ygyro As Integer
Dim Zgyro As Integer
Dim Xacc As Integer
Dim Yacc As Integer
Dim Zacc As Integer
Dim Xmag As Integer
Dim Ymag As Integer
Dim Zmag As Integer

Dim Xgyrosum As Long
Dim Ygyrosum As Long
Dim Zgyrosum As Long
Dim Xaccsum As Long
Dim Yaccsum As Long
Dim Zaccsum As Long
Dim Xmagsum As Long
Dim Ymagsum As Long
Dim Zmagsum As Long

Dim Xgyrooff As Long
Dim Ygyrooff As Long
Dim Zgyrooff As Long
Dim Xaccoff As Long
Dim Yaccoff As Long
Dim Zaccoff As Long

Dim Xmagmax As Integer
Dim Xmagmin As Integer
Dim Ymagmax As Integer
Dim Ymagmin As Integer
Dim Calibcounter As Word

Dim Xmagoff As Long
Dim Ymagoff As Long
Dim Xmagoff_e As Eram Long
Dim Ymagoff_e As Eram Long

Dim Xtan As Single
Dim Ytan As Single
Dim Alphasingle As Single
Dim Alphaint As Integer

Dim I As Byte
Dim J As Byte
Dim K As Word

Dim Arrayindex As Byte
Dim Xgyroarray(6) As Integer
Dim Ygyroarray(6) As Integer
Dim Zgyroarray(6) As Integer
Dim Xaccarray(6) As Integer
Dim Yaccarray(6) As Integer
Dim Zaccarray(6) As Integer
Dim Xmagarray(6) As Integer
Dim Ymagarray(6) As Integer
Dim Zmagarray(6) As Integer

'Gyro is set to +/-250dps with 0,00875dps/digit, 1dps ~ 114LSB
'Acc is set to +/-2g with 1mg/digit, 1g = 1000LSB
'Mag is set to +/- 1.9 Gauss with 1 Gauss= 795LSB (X/Y) 710LSB (Z)

'#############################################
'############ RC IN and OUT ##################
'#############################################

'RC-input is:
'Ch1 - Yaw - Pinc.3
'Ch2 - Pitch - Pinc.4
'Ch3 - Roll - Pinc.5
'Ch4 - Nick - Pinc.6
'Ch5 - Reserved - Pinc.7

Ch1_in Alias Pinc.3
Ch2_in Alias Pinc.4
Ch3_in Alias Pinc.5
Ch4_in Alias Pinc.6
Ch5_in Alias Pinc.7

Dim C_test As Long

'RC Signal is every about 20ms (not critical) a high for 1.5ms +/- 0.5ms
'Ch1 - 4 follow one after the other without any break
'Timer1 is used manually to measure the input and generate the output

Dim Yaw As Integer
Dim Pitch As Integer
Dim Roll As Integer
Dim Nick As Integer
Dim Ch5 As Integer

'The output signals are in the same format as input

'Servo rotates clock-wise with long signal, counter-clock-wise with short signals
'CW means more
'Servo 1 is front, then counting clock-wise

Esc Alias Portb.0
Servo_out1 Alias Portb.1
Servo_out2 Alias Portb.2
Servo_out3 Alias Portb.3
Servo_out4 Alias Portb.4

Ddrb.0 = 1
Ddrb.1 = 1
Ddrb.2 = 1
Ddrb.3 = 1
Ddrb.4 = 1

Dim Esc As Integer
Dim Servo1 As Integer
Dim Servo2 As Integer
Dim Servo3 As Integer
Dim Servo4 As Integer

'###############################################

Print "Gyro-test"
Gosub Init_gyro
Gosub Init_acc
Gosub Init_mag

Wait 1

Print "Init Ready"

Wait 1
Led = 1
Wait 2
Led = 0
Wait 2
'Xmagoff = -245
'Ymagoff = 45

'######## Measuring Offset of Gyro and Acc ###########
For I = 1 To 100
   Gosub Get_g_a_data
   Xgyrosum = Xgyrosum + Xgyro
   Ygyrosum = Ygyrosum + Ygyro
   Zgyrosum = Zgyrosum + Zgyro
   Xaccsum = Xaccsum + Xacc
   Yaccsum = Yaccsum + Yacc
   Zaccsum = Zaccsum + Zacc
   Waitms 20                                                'about 50Hz, 2 seconds
Next I
   Xgyrooff = Xgyrosum / 100                                'This is the offset
   Ygyrooff = Ygyrosum / 100
   Zgyrooff = Zgyrosum / 100
   Xaccoff = Xaccsum / 100
   Yaccoff = Yaccsum / 100
   Zaccoff = Zaccsum / 100

Led = 1
Gosub Calibrate_mag

Print "Xmin: " ; Xmagmin ; "  Xmax: " ; Xmagmax ; "  Xoff: " ; Xmagoff
Print "Ymin: " ; Ymagmin ; "  Ymax: " ; Ymagmax ; "  Yoff: " ; Ymagoff
Print "Calibcounter: " ; Calibcounter
Led = 0

'####################################################
'################## Main Loop #######################
'####################################################

'(
Do
   Timer1 = Timer1 + Esc
   Timer1 = Timer1 + Servo1

   Gosub Get_g_a_data
   Gosub Get_mag_data

   Do
   Loop Until Timer1 >= 40000

   'Gosub Channels_out

Loop
')

Do
   Xgyrosum = 0
   Ygyrosum = 0
   Zgyrosum = 0
   Xaccsum = 0
   Yaccsum = 0
   Zaccsum = 0
   Xmagsum = 0
   Ymagsum = 0
   Zmagsum = 0
   For I = 1 To 20
      Timer1 = 0

      Gosub Get_g_a_data
      Gosub Get_mag_data
      Xgyrosum = Xgyrosum + Xgyro
      Ygyrosum = Ygyrosum + Ygyro
      Zgyrosum = Zgyrosum + Zgyro
      Xaccsum = Xaccsum + Xacc
      Yaccsum = Yaccsum + Yacc
      Zaccsum = Zaccsum + Zacc
      Xmagsum = Xmagsum + Xmag
      Ymagsum = Ymagsum + Ymag
      Zmagsum = Zmagsum + Zmag                              'this block about 1,2ms

      Do
      Loop Until Timer1 >= 40000                            '20ms
   Next

   'Wait 2

   Toggle Led

   Xgyrosum = Xgyrosum / 229                                'now in 0.1dps
   Ygyrosum = Ygyrosum / 229
   Zgyrosum = Zgyrosum / 229
   Xaccsum = Xaccsum / 320                                  '/16 because 12-bit left-aligned
   Yaccsum = Yaccsum / 320
   Zaccsum = Zaccsum / 320
   Xmagsum = Xmagsum / 20
   Ymagsum = Ymagsum / 20
   Zmagsum = Zmagsum / 20

   'Print "G-X=" ; Xgyrosum ; "   " ; "G-Y=" ; Ygyrosum ; "   " ; "G-Z=" ; Zgyrosum

   'Print "A-X=" ; Xaccsum ; "   " ; "A-Y=" ; Yaccsum ; "   " ; "A-Z=" ; Zaccsum

   Print "M-X=" ; Xmagsum ; "   " ; "M-Y=" ; Ymagsum ; "   " ; "M-Z=" ; Zmagsum

   Xtan = Xmagsum
   Ytan = Ymagsum
   Alphasingle = Atn2(ytan , Xtan)                          'about 480µs
   Alphaint = Rad2deg(alphasingle)

   Print "Degrees: " ; Alphaint

Loop

End

'##############################################################
'######################## Gosubs ##############################
'##############################################################

Get_g_a_data:

   Slaveadress = Gyroadress
   Subadress = Out_x_l
   Gosub Read_6

   Xgyro = Dataarray(2)
   Shift Xgyro , Left , 8
   Xgyro = Xgyro + Dataarray(1)

   Ygyro = Dataarray(4)
   Shift Ygyro , Left , 8
   Ygyro = Ygyro + Dataarray(3)

   Zgyro = Dataarray(6)
   Shift Zgyro , Left , 8
   Zgyro = Zgyro + Dataarray(5)

   Slaveadress = Accadress
   Subadress = Out_x_l_a
   Gosub Read_6

   Xacc = Dataarray(2)
   Shift Xacc , Left , 8
   Xacc = Xacc + Dataarray(1)

   Yacc = Dataarray(4)
   Shift Yacc , Left , 8
   Yacc = Yacc + Dataarray(3)

   Zacc = Dataarray(6)
   Shift Zacc , Left , 8
   Zacc = Zacc + Dataarray(5)

   Xgyro = Xgyro - Xgyrooff
   Ygyro = Ygyro - Ygyrooff
   Zgyro = Zgyro - Zgyrooff
   Xacc = Xacc - Xaccoff
   Yacc = Yacc - Yaccoff
   Zacc = Zacc - Zaccoff

Return

Get_mag_data:

   Slaveadress = Magadress
   Subadress = Out_x_h_m
   Gosub Read_6_mag

   Xmag = Dataarray(1)
   Shift Xmag , Left , 8
   Xmag = Xmag + Dataarray(2)

   Ymag = Dataarray(3)
   Shift Ymag , Left , 8
   Ymag = Ymag + Dataarray(4)

   Zmag = Dataarray(5)
   Shift Zmag , Left , 8
   Zmag = Zmag + Dataarray(6)

   Xmag = Xmag - Xmagoff
   Ymag = Ymag - Ymagoff

Return

Init_gyro:
   Slaveadress = Gyroadress
   Subadress = Ctrl_reg1
   Databyte = &B10001111                                    'Enable all, ODR 400kHz, LPF1 cut off 78Hz
   Gosub Write_byte

   Slaveadress = Gyroadress
   Subadress = Ctrl_reg4
   Databyte = &B10000000                                    'Block Data Update, LSB low, Scale 250 dps
   Gosub Write_byte
Return

Init_acc:
   Slaveadress = Accadress
   Subadress = Ctrl_reg1_a
   Databyte = &B00101111                                    'normal power, 100Hz, 74Hz LPF
   Gosub Write_byte

   Slaveadress = Accadress
   Subadress = Ctrl_reg4_a
   Databyte = &B10000000                                    'Block Data Update, LSB low, Scale +-2g
   Gosub Write_byte
Return

Init_mag:
   Slaveadress = Magadress
   Subadress = Cra_reg_m
   Databyte = &B00011000                                    '75Hz Output
   Gosub Write_byte

   Slaveadress = Magadress
   Subadress = Crb_reg_m
   Databyte = &B01000000                                    'range +-1.9Gauss
   Gosub Write_byte

   Slaveadress = Magadress
   Subadress = Mr_reg_m
   Databyte = &B00000000                                    'continious mode
   Gosub Write_byte
Return

Write_byte:                                                 'needs Slaveadress, Subadress and Databyte
   I2cstart
   I2cwbyte Slaveadress
   I2cwbyte Subadress
   I2cwbyte Databyte
   I2cstop
Return

Read_byte:                                                  'needs Slaveadress, Subadress and gives Databyte
   I2cstart
   I2cwbyte Slaveadress
   I2cwbyte Subadress
   Incr Slaveadress                                         'for reading LSB=1
   I2crepstart
   I2cwbyte Slaveadress
   I2crbyte Databyte , Nack
   I2cstop
Return

Read_6:
   Subadress.7 = 1                                          'Set MSB for multiple reading
   I2cstart
   I2cwbyte Slaveadress                                     'SAD+W
   I2cwbyte Subadress
   Incr Slaveadress                                         'for reading LSB=1
   I2crepstart
   I2cwbyte Slaveadress                                     'SAD+R
   For J = 1 To 5
      I2crbyte Dataarray(j) , Ack
   Next
   I2crbyte Dataarray(6) , Nack
   I2cstop
Return

Read_6_mag:
   I2cstart
   I2cwbyte Slaveadress                                     'SAD+W
   I2cwbyte Subadress
   Incr Slaveadress                                         'for reading LSB=1
   I2crepstart
   I2cwbyte Slaveadress                                     'SAD+R
   For J = 1 To 5
      I2crbyte Dataarray(j) , Ack
   Next
   I2crbyte Dataarray(6) , Nack
   I2cstop
Return

Get_rc_in:
   Bitwait Ch1_in , Set                                     'wait until Ch1 starts
   Timer1 = 0
   Bitwait Ch1_in , Reset                                   'end of Ch1
   Yaw = Timer1
   Bitwait Ch2_in , Set                                     'Ch2 starts
   Timer1 = 0
   Bitwait Ch2_in , Reset                                   'end of Ch2
   Pitch = Timer1
   Bitwait Ch3_in , Set                                     'Ch3 starts
   Timer1 = 0
   Bitwait Ch3_in , Reset
   Roll = Timer1
   Bitwait Ch4_in , Set
   Timer1 = 0
   Bitwait Ch4_in , Reset
   Nick = Timer1
   Bitwait Ch5_in , Set
   Timer1 = 0
   Bitwait Ch5_in , Reset
   Ch5 = Timer1
Return

Calibrate_test:
   For I = 1 To 100
      Gosub Get_rc_in
      C_test = C_test + Yaw
   Next
   If C_test < 250000 Or C_test > 350000 Then               'Calibration

   End If
Return

Calibrate_mag:
   Start Timer0
   Xmagmin = 0
   Ymagmin = 0
   Xmagmax = 0
   Ymagmax = 0

   Calibcounter = 0
   16mscounter = 0
   Do
      Xmagsum = 0
      Ymagsum = 0
      For I = 1 To 20
         Gosub Get_mag_data
         Xmagsum = Xmagsum + Xmag
         Ymagsum = Ymagsum + Ymag
      Next
      Xmagsum = Xmagsum / 20
      Ymagsum = Ymagsum / 20

      If Xmagsum > Xmagmax Then Xmagmax = Xmagsum
      If Ymagsum > Ymagmax Then Ymagmax = Ymagsum

      If Xmagsum < Xmagmin Then Xmagmin = Xmagsum
      If Ymagsum < Ymagmin Then Ymagmin = Ymagsum

      Incr Calibcounter
   Loop Until 16mscounter >= 800                            '9,6s
   Stop Timer0

   Xmagoff = Xmagmin + Xmagmax
   Ymagoff = Ymagmin + Ymagmax
   Xmagoff = Xmagoff / 2
   Ymagoff = Ymagoff / 2

T0isr:
   Incr 16mscounter
Return

                                                      '