
' ************************ IMPORTANT!!!!!   **********************************
' * Use these values in BASCOM at "Options, Compiler, Chip" at the beginning *
' *       HWSTACK-64                                                         *
' *       SWSTACK-128                                                        *
' *       FRAMESIZE-16                                                       *
' ****************************************************************************
'
'
$regfile = "m2561def.dat"                                   'ATMega 2561, change if using different processors
$crystal = 16000000                                         '16 MHz
$hwstack = 256
$swstack = 256
$framesize = 256
$baud1 = 9600

Const Roottwomgovercda = 2.9

Declare Sub I2cread()
Declare Sub I2cwrite()
Declare Sub Onewiretemperature()
Declare Sub Sendsms()
Declare Function Checkposition(byval Xpos As Single , Byval Ypos As Single)as Byte
Declare Sub Video(byval Timeinterval As Byte)
Declare Sub Processgps()
Declare Sub Updatepredict()
Declare Sub Cutdown()
Declare Sub Landingvideo()

Dim Temp1 As Byte , Temp2 As Byte ,
Dim Bitstring(140) As Byte , Numb As Byte
Dim Jordancurvexpoints(18) As Single
Dim Jordancurveypoints(18) As Single
Dim Numberofpoints As Byte
Dim Videookay As Byte

 Dim G As Byte , De As Byte , Cutdown As Byte
 Dim North As Single
 Dim East As Single
 Dim Altitude As Single
 Dim Storage As Single
 Dim Gpstring As String * 15
 Dim S As String * 90
 Dim B As String * 6
 Dim Degrees As String * 3
 Dim Minutes As String * 7
 Dim Contents(17) As String * 11
 Dim V As String * 1
 Dim K As Single
 Dim Storebyte As Byte
 Dim Up As Byte
 Dim Across As Byte
 Dim Bd(9) As Byte
 Dim I As Integer
 Dim Iterations As Word
 Dim Writepermission As Byte
 Dim Lock As Bit
 Dim Videocount As Byte
 Dim Videos As Byte
 Dim Btemp1 As Byte
 Dim Seconds As Long
 Dim Timestring As String * 10
 Dim Two As String * 2
 Dim Oldeast As Single
 Dim Oldnorth As Single
 Dim Oldaltitude As Single
 Dim Oldseconds As Single
 Dim Predictedn As Single , Predictede As Single
 Dim Deltae As Single , Deltan As Single , Deltaa As Single , Deltat As Single
 Dim Averagealt As Single
 Dim Cuttingdown As Eram Byte

 Open "COM2:" For Binary As #1
 Open "coma.0:9600,8,n,1,inverted" For Output As #3
 Open "comd.0:4800,8,n,1" For Input As #2
 Open "comd.1:4800,8,n,1" For Output As #4

 Numberofpoints = 17
 Jordancurvexpoints(1) = -2.166
 Jordancurvexpoints(2) = 0
 Jordancurvexpoints(3) = 0
 Jordancurvexpoints(4) = 0.333
 Jordancurvexpoints(5) = 1.000
 Jordancurvexpoints(6) = 1.250
 Jordancurvexpoints(7) = 1.500
 Jordancurvexpoints(8) = 1.500
 Jordancurvexpoints(9) = 1.166
 Jordancurvexpoints(10) = 0.166
 Jordancurvexpoints(11) = -0.666
 Jordancurvexpoints(12) = -0.666
 Jordancurvexpoints(13) = -2.166
 Jordancurvexpoints(14) = -2.166
 Jordancurvexpoints(15) = -1.666
 Jordancurvexpoints(16) = -1.666
 Jordancurvexpoints(17) = -2.166
 Jordancurvexpoints(18) = -2.166
 Jordancurveypoints(1) = 53.166
 Jordancurveypoints(2) = 53.166
 Jordancurveypoints(3) = 52.833
 Jordancurveypoints(4) = 52.75
 Jordancurveypoints(5) = 52.833
 Jordancurveypoints(6) = 52.833
 Jordancurveypoints(7) = 52.666
 Jordancurveypoints(8) = 52.416
 Jordancurveypoints(9) = 52.083
 Jordancurveypoints(10) = 51.666
 Jordancurveypoints(11) = 51.666
 Jordancurveypoints(12) = 51.166
 Jordancurveypoints(13) = 51.166
 Jordancurveypoints(14) = 52.333
 Jordancurveypoints(15) = 52.333
 Jordancurveypoints(16) = 52.666
 Jordancurveypoints(17) = 52.666
 Jordancurveypoints(18) = 53.166

 Iterations = 0
 Cutdown = 0
 Videookay = 0

'----------------------------------------------------------------------

Config 1wire = Portc.1                                      'connects to our temperature sensor
Config Scl = Portc.3                                        ' we need to provide the SCL pin name
Config Sda = Portc.5
Config Portc.4 = Output
Config Portc.6 = Output
Config I2cdelay = 10                                        ' we need to provide the SDA pin name
Config Portc.0 = Output
Config Portc.2 = Output
Config Porta.1 = Output
Config Clock = Soft
Config Portd.0 = Input
Config Watchdog = 2048

I2cinit                                                     ' we need to set the pins in the proper state

Cutdownpin Alias Porta.1
Cam_shutter Alias Portc.2
Cam_power Alias Portc.0
Cam_right Alias Portc.4
Cam_ok Alias Portc.6
Cam_right = 1
Cam_ok = 1                                                  'Port b.1

Config Porta = Output                                       'need to work out WTF connects here
Porta = 0
Config Portc.7 = Output
Portc.7 = 0                                                 'this is dead man button
Portd.4 = 1
Config Portd.4 = Input

'------------------------------------------------------------------start of main program
 Stop Watchdog
 Cutdownpin = 0
 Print #1 , "Hello from ULTRAHAB"
 Wait 1

$include "Config_MMC.BAS"                                   'code off the avr-dos site
$include "CONFIG_AVR-DOS.BAS"

If Gbdriveerror <> 0 Then
 Print #1 , "card error " ; Gbdriveerror
End If

Btemp1 = Initfilesystem(1)
If Btemp1 <> 0 Then
 Print #1 , "File system error" ; Btemp1
Else
 Print #1 , "Card setup okay"
End If
 Print #1 , "sms time"
 S = "hello test"
 Call Sendsms()
 Print #1 , "temperture sensor test"
 Call Onewiretemperature()
 Print #1 , "GPS test"
 Do
  Input #2 , S
  B = Mid(s , 2 , 6)
  If B = "$GPGGA" Then
   Print #1 , S                                             'shows us the GGA string
   Temp1 = Split(s , Contents(1) , ",")
   K = 0
   K = Val(contents(10))
   If K <> 0 Then                                           'if nonzero altitude then we've got something
    Goto Gotlock
   End If
  End If
 Loop
Gotlock:
Print #1 , "Okay we have GPS"
Call Processgps()
Call Updatepredict()
Temp1 = Checkposition(predictede , Predictedn)
Print #1 , "check says:" ; Temp1
Print #1 , "press the button you fool!!!!"
For Temp1 = 1 To 10                                         ' dead man button
 Wait 1
 If Pind.4 = 0 Then
  Goto Userpresent
 End If
Next Temp1
Iterations = 241
Print #4 , "ULTRAHAB bad news: forced reeboot, cutting down"
Print #1 , "bad news: forced reeboot, cutting down"
Wait 4
If Cuttingdown = 15 Then
 Print #4 , "ULTRAHAB insufficient power to cutdown"
 Print #1 , "not enought power!!!"
 Cutdown = 1
End If
Goto Theflightloop:
Userpresent:
If Cuttingdown <> 0 Then
 Cuttingdown = 0
End If
Print #1 , "Filesystem: " ; Gbfilesystem
Print #1 , "FAT Start Sector: " ; Glfatfirstsector
Print #1 , "Root Start Sector: " ; Glrootfirstsector
Print #1 , "Data First Sector: " ; Gldatafirstsector
Print #1 , "Max. Cluster Nummber: " ; Glmaxclusternumber
Print #1 , "Sectors per Cluster: " ; Gbsectorspercluster
Print #1 , "Root Entries: " ; Gwrootentries
Print #1 , "Sectors per FAT: " ; Glsectorsperfat
Print #1 , "Number of FATs: " ; Gbnumberoffats
S = Dir( "*.*")
  While Len(s) > 0                                          ' if there was a file found?
   Print #1 , S ; "  ";                                     'file name
   Print #1 , Filedate() ; "  ";
   Print #1 , Filetime() ; "  ";
   Print #1 , Filelen()
   S = Dir()                                                ' print file , the date the fime was created/changed , the time and the size of the file
  Wend

Open "VERB.TXT" For Input As #14
Do
 Do
   Line Input #14 , S                                       ' read a line
   Print #1 , S                                             ' print on terminal
 Loop Until Eof(#14) <> 0
 Print #1 , "deleting, okay? (Y/N)"
 Input #1 , B
Loop Until B = "Y"
Close #14
Kill "VERB.TXT"


Open "LOG.TXT" For Input As #14
Do
 Do
   Line Input #14 , S                                       ' read a line
   Print #1 , S                                             ' print on terminal
 Loop Until Eof(#14) <> 0
Print #1 , "deleting, okay? (Y/N)"
Input #1 , B
Loop Until B = "Y"
Close #14
Kill "LOG.TXT"
Print #1 , "camera on"
 Cam_power = 1
 Waitms 500
 Cam_power = 0



Theflightloop:
Do
 Open "VERB.TXT" For Append As #11                          'these are all our files
 Open "LOG.TXT" For Append As #12                           'main flight loop
 Incr Iterations
 Print #1 , "Main loop iteration number:" ; Iterations
 Cam_shutter = 1
 Waitms 200                                                 'photo
 Cam_shutter = 0
 Call Onewiretemperature()
 Print #12 , Seconds ; "," ; Storage ; ",";
 Print #11 , "temperature is:" ; Storage
 Wait 30
 Do
  Input #2 , S
  B = Mid(s , 2 , 6)                                        'check stuff and update
 Loop Until B = "$GPGGA"
 S = Mid(s , 2 , 90)
 Print #11 , S
 Print #12 , S
 Call Processgps()
 Call Updatepredict()
 If Deltaa < 0 And Videookay = 1 And Altitude < 300 Then
  Call Landingvideo()
 End If
 Print #11 , "Predicted at " ; Seconds ; "was" ; Predictede ; " " ; Predictedn
 Call Sendsms()
 Print #4 , "ULTRAHAB" ; Contents(3) ; Contents(4) ; Contents(5) ; Contents(6);
 If Cutdown = 0 Then
  Print #4 , "PRED:" ; Predictede ; Predictedn
 Else
  Print #4 , "cut"
 End If
 Temp1 = Checkposition(predictede , Predictedn)
 If Cutdown = 0 Then
  If Temp1 = 1 Then
   Print #11 , "Geofence cutdown"
   Call Cutdown()
   Print #4 , "geofence cutdown"
  End If
  If Iterations > 180 Then                                  ' 3 hour cutdown approx
   Print #1 , "Time limit cutdown"
   Print #11 , "Time limit cutdown"
   Call Cutdown()
   Print #4 , "time limit cutdown"
  End If
 End If
 Cam_shutter = 1                                            'photo
 Waitms 200
 Cam_shutter = 0
 Call Onewiretemperature()
 Print #12 , Seconds ; "," ; Storage ; ",";
 Print #11 , "temperature is:" ; Storage
 Wait 10                                                    'video
 Call Video(8)
 Wait 10
 Do
  Input #2 , S
  B = Mid(s , 2 , 6)
 Loop Until B = "$GPGGA"
 S = Mid(s , 2 , 90)
 Print #11 , S
 Print #12 , S
 Call Processgps()
 Call Updatepredict()
 If Deltaa < 0 And Videookay = 1 And Altitude < 300 Then
  Call Landingvideo()
 End If
 Print #11 , "Predicted at " ; Seconds ; "was" ; Predictede ; " " ; Predictedn
 Print #4 , "ULTRAHAB" ; Contents(3) ; Contents(4) ; Contents(5) ; Contents(6);
 If Cutdown = 0 Then
  Print #4 , "PRED:" ; Predictede ; Predictedn
 Else
  Print #4 , "cut"
 End If
 Temp1 = Checkposition(predictede , Predictedn)
 If Cutdown = 0 Then
  If Temp1 = 1 Then
   Print #11 , "Geofence cutdown"
   Call Cutdown()
   Print #4 , "geofence cutdown"
  End If
 End If
 Close #11
 Close #12

Loop



'---------------------------------------------------------- prediction

Sub Updatepredict()
 If Iterations = 0 Then
  Predictedn = North
  Predictede = East
  Oldeast = East                                            'start
  Oldnorth = North
  Oldaltitude = Altitude
  Oldseconds = Seconds
 End If
 Deltae = East - Oldeast
 Deltan = North = Oldnorth
 Deltaa = Altitude - Oldaltitude
 Deltat = Seconds - Oldseconds
 Averagealt = Oldaltitude + Altitude
 Averagealt = Averagealt / 2
 Oldeast = East
 Oldnorth = North
 Oldaltitude = Altitude
 Oldseconds = Seconds
 If Deltaa > 0 Then
  K = Averagealt / 16000
  K = Exp(k)
  K = 0.90909 * K
  K = Roottwomgovercda * K                                  'we now have the velocity of decent
  Print #1 , "Descent velocity=" ; K
  K = Deltaa / K                                            'time of decent for this layer
  Print #1 , "descent time=" ; K
  K = K / Deltat                                            'weighting for this layer
  Print #1 , "Layer weighting=" ; K
  K = K + 1                                                 'need to account for ascent drift
  Deltae = K * Deltae
  Deltan = K * Deltan
  Predictedn = Predictedn + Deltan
  Predictede = Predictede + Deltae
 End If
 Print #1 , "predicted north=" ; Predictedn
 Print #1 , "predicted east=" ; Predictede
End Sub

'---------------------------------------------------------- GPS parsing

Sub Processgps()
   Temp1 = Split(s , Contents(1) , ",")
   Altitude = Val(contents(10))
   Degrees = Left(contents(3) , 2)
   Minutes = Right(contents(3) , 7)
   North = 0
   North = Val(minutes)
   North = North / 60
   K = 0
   K = Val(degrees)
   North = K + North
   Degrees = Left(contents(5) , 3)
   Minutes = Right(contents(5) , 7)
   East = 0
   East = Val(minutes)
   East = East / 60
   K = 0
   K = Val(degrees)
   East = K + East
   If Contents(6) = "W" Then
    East = -east
   End If
   Timestring = Contents(2)
   Two = Mid(timestring , 1 , 2)
   I = Val(two)
   Seconds = I * 3600
   Two = Mid(timestring , 3 , 2)
   I = Val(two)
   I = 60 * I
   Seconds = Seconds + I
   Two = Mid(timestring , 5 , 2)
   I = Val(two)
   Seconds = Seconds + I
   Print #1 , "Lat=" ; North
   Print #1 , "Long=" ; East
   Print #1 , "Altitude=" ; Altitude
   Print #1 , "Time=" ; Seconds
   If Altitude > 500 Then
    Videookay = 1
   End If
End Sub

'---------------------------------------------------------- temperature

Sub Onewiretemperature()
 1wreset
 1wwrite &HCC
 1wwrite &H44
 Wait 1
 1wreset
 1wwrite &HCC
 1wwrite &HBE
 Bd(1) = 1wread(9)
 1wreset
 Storage = Bd(1)
 If Bd(2) = 255 Then                                        'bd(2) is 255 for -ive temp
  Storage = -storage
 End If
 Storage = Storage / 2
 Gpstring = Str(storage)
 Print #1 , "T=" ; Gpstring
End Sub


'----------------------------------------------------------- phone


Sub Sendsms()
Local E As Byte , D As Byte , M As Byte                     ' this is a for one sms
E = 1
D = 0
  For G = 1 To 80
   V = Mid(s , G , 1)
   Numb = Asc(v)
   For M = 0 To 6
    Bitstring(e).d = Numb.m                                 'd,ti,m ,g and e Are Bytes,
    Incr D
    If D = 8 Then
     D = 0
    Incr E
    End If
   Next M
  Next G
Print #1 , "SMS"
Print #3 , "AT+CMGS=154"
Wait 1
Print #3 , "079144775810065011000C914497313345270000AA";    'PDU string to Mobile
Print #3 , "A0";
For G = 1 To 140
 Print #3 , Hex(bitstring(g));
Next G
Print #3 , Chr(026);
End Sub

'----------------------------------------------------------- poscheck

Function Checkposition(byval Xpos As Single , Byval Ypos As Single) As Byte
 Local Count As Byte , Skip As Byte , Gplus As Byte
 Local Grad As Single , Gradtwo As Single , Delta As Single , Deltatwo As Single , Xone As Single , Xtwo As Single , Yone As Single , Ytwo As Single
 Count = 0
 For G = 1 To Numberofpoints
   Gplus = G + 1
   Xone = Jordancurvexpoints(g)
   Xtwo = Jordancurvexpoints(gplus)
   Yone = Jordancurveypoints(g)
   Ytwo = Jordancurveypoints(gplus)
  'first we check to see if both points north or both south
   Skip = 0
   If Yone > Ypos Then
    If Ytwo > Ypos Then
     Skip = 1
    End If
   End If
   If Yone < Ypos Then
    If Ytwo < Ypos Then
     Skip = 1
    End If
   End If
   If Skip = 0 Then
    'then we work out if the x axis intercept is at positive or negative x
    Delta = Xone - Xpos
    'now the gradient
    Grad = Xtwo - Xone
    Gradtwo = Ytwo - Yone
    Grad = Grad / Gradtwo
    'we've got the gradient
    Deltatwo = Ypos - Yone
    Deltatwo = Deltatwo * Grad
    Delta = Delta + Deltatwo
    'is the crossing to the left or right?
    If Delta > 0 Then
     If Count = 0 Then
      Count = 1
     Else
      Count = 0
     End If
    End If
   End If
  Next G
If Count = 0 Then
 Print #1 , "outside"
 Checkposition = 1
Else
 Print #1 , "inside"
 Checkposition = 0
End If

End Function

'----------------------------------------------------------- eeprom


Sub I2cread()
I2cstart
I2cwbyte &B1010_0000
I2cwbyte Up
I2cwbyte Across
I2cstart
I2cwbyte &B1010_0001
I2crbyte De , Nack
I2cstop
End Sub

Sub I2cwrite()
If Writepermission = 1 Then
 I2cstart                                                   'start condition
 I2cwbyte &B1010_0000                                       'slave address
 I2cwbyte Up                                                'high address
 I2cwbyte Across                                            'low address
 I2cwbyte De                                                'value to write
 I2cstop                                                    'stop condition
 Waitms 10
End If
End Sub

'--------------------------------------------------------------------------  camera

Sub Video(byval Timeinterval As Byte)
Toggle Cam_right
Waitms 200
Toggle Cam_right
Waitms 300
Toggle Cam_right
Waitms 200
Toggle Cam_right
Waitms 200
Toggle Cam_ok
Waitms 200
Toggle Cam_ok
Waitms 500
Toggle Cam_shutter
Waitms 200
Toggle Cam_shutter
Wait Timeinterval
Toggle Cam_shutter
Waitms 200
Toggle Cam_shutter
Wait 1
Toggle Cam_right
Waitms 200
Toggle Cam_right
Waitms 300
Toggle Cam_right
Waitms 200
Toggle Cam_right
Waitms 200
Toggle Cam_right
Waitms 200
Toggle Cam_right
Waitms 200
Toggle Cam_right
Waitms 200
Toggle Cam_right
Waitms 200
Toggle Cam_ok
Waitms 200
Toggle Cam_ok
End Sub

Sub Landingvideo()
Print #1 , "Landing video"
Videookay = 0
Toggle Cam_right
Waitms 200
Toggle Cam_right
Waitms 300
Toggle Cam_right
Waitms 200
Toggle Cam_right
Waitms 200
Toggle Cam_ok
Waitms 200
Toggle Cam_ok
Waitms 500
Toggle Cam_shutter
Waitms 200
Toggle Cam_shutter
End Sub

'------------------------------------------------------------------------------cutdown

Sub Cutdown()
 Print #1 , "Cutting down !!!!"
 Cuttingdown = 15
 Start Watchdog
 Cutdownpin = 1
 For Temp1 = 1 To 12
  Wait 1
  Reset Watchdog
 Next Temp1
 Cutdownpin = 0
 Cutdown = 1                                                'so we know we've cutdown
 Stop Watchdog
 Cuttingdown = 0
End Sub


'---------------------------------------------------------------------------

Close #1
Close #2
Close #3

End