'initiate I2C
SetPin GP0,GP1,I2C
I2C OPEN 100, 1000
'initiate Serial 1 for RTK GPS input
SetPin GP12, GP13, COM1
' initiate Serial 2 for Telemetry
setpin GP9, GP8, COM2
'initilise the motor drive pins
setpin GP2, DOUT
setpin GP3, PWM[1]
setpin GP4, DOUT
setpin GP5, PWM[2]
'initilise the batt voltage analogue input
SETPIN GP28, AIN
'Initiage compass
I2C WRITE &H1E, 0,2,&H02, &H00
Dim MagData(6)
'Open the com ports
Open "COM1:38400" As GPS
Open "COM2:57600" As #2
'initilise pins for reading PPM data
SETPIN GP6, DIN
DIM RCChan(8)

Do
 ' Compass
I2C WRITE &H1E,0,1,&H03
I2C READ &H1E, 0, 6, MagData()
X=(MagData(0)*256)+MagData(1)
Y=(MagData(2)*256)+MagData(3)
Z=(MagData(4)*256)+MagData(5)
If X>32768 Then X=X-65536
If Y>32768 Then Y=Y-65536
If Z>32768 Then Z=Z-65536
 ' Added the +190 And +127 to calibrate the sensor for my location
CompassInRad = Atan2(Z+190, X+127) + DeclinationAngle
CompassInDeg=Int(CompassInRad * 180/Pi)
Print "Compas ";CompassInDeg;" degrees"
Print #2, "<COM>";CompassInDeg;"</COM>]"

 ' Distance
D=Distance(24,25)
Print "Distance to object ";D;"cm"
Print #2, "<DIS>";D;"</DIS>"

 ' GPS
print
Print "GPS Valid ";GPS(VALID)
Print "Lat ";GPS(LATITUDE)
Print "Lng ";GPS(LONGITUDE)
Print "DOP ";GPS(DOP)
Print "FIX ";GPS(FIX)
print #2, "<LAT>";GPS(Latitude);"</LAT>"
print #2, "<LNG>";GPS(LONGITUDE);"</LNG>"
print #2, "<FIX>";GPS(FIX);"<DOP>"

 ' RC Receiver, using PPM
 ' https://github.com/rc-hacks/mpx-ppm-spec
c=0
hp=0
lp=pulsin(GP6,0,50000)' get low pulse duration.
do while hp<4000 and c<10 ' Wait for a sync pulse
 hp=pulsin(GP6,1,50000)
 c=c+1
loop
if hp>0 then
 for t=1 to 4 ' only need the first 4 channels
   RCChan(t) = pulsin(GP6,1)+lp
 next
 print "RC Channel Data ";
 for t=1 to 4
     print str$(RCChan(t))+"  ";
     print #2, "<CH1>";str$(RCChan(t));"</CH1>"
     print #2, "<CH2>";str$(RCChan(t));"</CH2>"
     print #2, "<CH3>";str$(RCChan(t));"</CH3>"
     print #2, "<CH4>";str$(RCChan(t));"</CH4>"
 next
 print
else
 Print "No PPM signal"
endif

 ' Battery Voltage
 ' 10:1 resisitor diver, calibrated in software
 BattV=Pin(GP28)
 print "Batt voltage ";BattV*12.51
 
Pause (500)

Loop

PRINT #2, Txd$

if GPS(VALID)=0 then BadGPSData=BadGPSData+1
if BadGPSData>5 then
 close GPS
 SetPin GP12, GP13, COM1
 Open "COM1:38400" As GPS
 BadGPSData=0
end if