
$regfile = "m168def.dat"
$crystal = 16000000
$baud = 16000
$hwstack = 32
$swstack = 64
$framesize = 96
Open "comc.4:125000,8,N,1" For Output As #1
Open "comc.3:125000,8,N,1" For Input As #2
Open "comd.2:4800,8,n,1" For Input As #3                    'rx from pc
Open "comb.4:19200,8,n,1" For Output As #4                  'Logger
Config Adc = Single , Prescaler = Auto , Reference = Internal
Config Portd.7 = Output
Config Portb.0 = Output
Config Portb.2 = Output
Config Portb.1 = Output
Config Portd.2 = Input                                      'rx from pc
Config Portd.6 = Output
Config Portd.3 = Input
Config Portc.2 = Input
Config Portd.4 = Input
Config Portc.1 = Output
Config Portc.0 = Input
Config Portc.5 = Input
Config Watchdog = 4096
'Config Portc.4 = Output
'Config Portc.3 = Input
Ledone Alias Portd.7
Ledtwo Alias Portb.0
Radiocts Alias Pind.3
'Cts Alias Portd.2
Groundcontrol Alias Pind.4
Statusled Alias Portd.6
Requestsecond Alias Portc.1
Clearsecond Alias Pinc.2
Cutdownpinone Alias Portb.1
Cutdownpintwo Alias Portb.2
Cutdownpinone = 0
Cutdownpintwo = 0
Baud = 4800
Declare Sub Checkvoltage()
Declare Sub Cutdown(byval Channel As Byte , Byval Duration As Byte)
Declare Sub Radiocomms()
Echo Off
Start Adc
Dim Rate As Integer
Dim Temperature As Integer , Error As Byte , Heading As Single , Filteredrate As Single , Pulselenght As Word
Dim Pulses As Byte
Dim S As String * 100
Dim T As String * 15
Dim Printstring As String * 20
Dim K As Single
Dim Iterations As Byte , Flashcount As Byte
Dim Windxs(251) As Eram Byte
Dim Windys(251) As Eram Byte , Placemark As Eram Byte
Dim Target_es As Eram Single
Dim Target_ns As Eram Single
Dim Lock As Bit
Dim Gpfloatone As Single , Gpsheading As Single
Dim U As String * 100
Dim B As String * 6
Dim Contents(17) As String * 15
Dim Transfer As String * 11
Dim N As Byte
Dim I As Byte
Dim Statuscode As Byte
Dim Heightupto As Byte , Rxenable As Byte , Servoenable As Byte
Dim Gpbyteone As Byte , Gpbytetwo As Byte , Telcounter As Byte , Landcount As Byte
Dim Gpintegerone As Integer
Dim Intcount As Integer , Timeelapsed As Integer
Dim Store As Integer
Dim Test As Integer
Dim L As Integer
Dim Arrayat As Integer
Dim Rudder As Single
Dim Integral As Single
Dim Derivative As Single
Dim Target As Single
Dim Target_e As Single
Dim Target_n As Single
Dim Offset As Single
Dim Windy_total As Single
Dim Windx_total As Single
Dim Wind_speed As Single
Dim Airspeed As Single
Dim X As Single
Dim Y As Single
Dim V As Single , Altitude As Single
Dim Wind_x As Single
Dim Wind_y As Single
Dim North As Single
Dim East As Single
Dim Store_east As Single
Dim Store_north As Single
Dim V_x As Single
Dim V_y As Single
Dim Minutes As String * 7
Dim Degrees As String * 3
Dim Command As String * 10
Statuscode = 0
Lock = 0
Iterations = 0
Timeelapsed = 0
Wind_x = 0
Wind_y = 0
Landcount = 0
Telcounter = 0
Portd.3 = 1
Waitms 100
Gpintegerone = Getadc(5)
Gpfloatone = Gpintegerone
Gpintegerone = Getadc(0)
Gpfloatone = Gpfloatone / Gpintegerone
If Gpfloatone < 0.5 And Gpfloatone > 0.4 Then
 Print "Button pressed"
Else
 Print "Watchdog reset"
 Start Watchdog
 Target_n = Target_ns
 Target_e = Target_es
 Select Case Placemark
 Case 1:
 Goto Cutdownlbl                                            'cutdown if error during ascent
 Case 2:
 Goto Descentloop
 Case Else:
 Goto Cutdownlbl
 End Select
End If
Waitms 50
Call Checkvoltage()
'Call Cutdown(1 , 4)
Print "wind records:"
For Gpbyteone = 1 To 251
 Gpbytetwo = Windxs(gpbyteone)
 Print Gpbyteone ; "," ; Gpbytetwo ; "," ;
 Gpbytetwo = Windys(gpbyteone)
 Print Gpbytetwo
Next
Print Version()
Print "Demo flight: 4km/20 minute"
Print "Now check gps"
Do
 Input S
 B = Mid(s , 2 , 6)
 If B = "$GPGGA" Then
  Print S                                                   'shows us the GGA string
  I = 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 "Okay we have GPS"
Print "check rc ground control, then"
Print "Enter target north and east"
Input #3 , Target_n
Print Target_n
Input #3 , Target_e
Print Target_e
Gpfloatone = Target_ns
If Target_n <> Gpfloatone Then Target_ns = Target_n         'store in eeprom
Gpfloatone = Target_es
If Target_e <> Gpfloatone Then Target_es = Target_e
Portd.3 = 1
Print "Launch!"                                             'radiocts pullup
Start Watchdog
Placemark = 1                                               'write eeprom to say in ascent
Do
 Reset Watchdog
 Do
  Input U
  B = Mid(u , 2 , 6)
 Loop Until B = "$GPGGA"
 Do
  Input S
  B = Mid(s , 2 , 6)
 Loop Until B = "$GPRMC"
   I = Split(s , Contents(1) , ",")                         'process rmc
   K = Val(contents(9))
   V = Val(contents(8))
   K = Deg2rad(k)
   Wind_x = Sin(k)                                          'WORK OUT WIND IN Y=NORTH frame - the trig library works in radians
   Wind_x = Wind_x * V
   Wind_y = Cos(k)
   Wind_y = Wind_y * V
   Windx_total = Windx_total + Wind_x                       'add it to our total wind
   Windy_total = Windy_total + Wind_y
   Incr L                                                   'incrament the denominator for our averaging
   Incr Timeelapsed
   I = Split(u , Contents(1) , ",")                         'now process gga
   K = Val(contents(10))
   K = K / 100
   Arrayat = Int(k)                                         'find the altitude in 100m incraments
   If Arrayat > Heightupto Then
    Incr Heightupto                                         'do we now fall into another (higher) 100m incrament ?
    Windx_total = Windx_total / L
    Gpbyteone = Windx_total + 127
    Windxs(heightupto) = Gpbyteone                          'we are storing as a signed byte
    Windy_total = Windy_total / L
    Gpbyteone = Windy_total + 127                           'we find the average and shove it in the eeprom
    Windys(heightupto) = Gpbyteone
    L = 0                                                   'this was forgotten in the flight code doh
   End If
   If Arrayat > 40 Then
    Print "Altitude cutdown" ; Arrayat
    Goto Cutdownlbl                                         '4Km/20 minute cutdown
   End If
   If Timeelapsed > 1200 Then
    Print "Time cutdown"
    Goto Cutdownlbl
   End If
   Requestsecond = 1
   While Clearsecond = 0
    Waitus 10
   Wend
   Requestsecond = 0
   Waitus 50
   Printbin #1 , Statuscode
   Waitus 150                                               'tells slave we are going up
   Inputbin #2 , Rate , Temperature , Error
   Incr Telcounter
   If Telcounter > 10 Then Call Checkvoltage()              ' our callsign is UKHAS
   If Radiocts = 1 Then                                     'is CTS=1 from the radio modem?
    Call Radiocomms()
   End If
Loop

Cutdownlbl:
If L > 0 Then                                               'set a valid wind for cutdown altitude
 Wind_x = Windx_total / L
 Wind_y = Windy_total / L
End If
Call Cutdown(1 , 12)
Placemark = 2                                               'write eeprom
Descentloop:
Statuscode = 1
Requestsecond = 1
While Clearsecond = 0
 Waitus 10                                                  'I did commented this out as we should give the slave a message 1 second before the first valid gps
Wend                                                        'but that was a stupid idea as in the slave code, when if first enters the decent loop
Requestsecond = 0                                           'it waits for a new gps before doing anything other than running off the gyro
Waitus 50                                                   ' the first update after this then goes into the gps lag compensation stage, but NOT YET !!!!
Printbin #1 , Statuscode                                    'we are going down so let the slave know


Do
 Reset Watchdog
 'insert
    Do
     Input U
     B = Mid(u , 2 , 6)
    Loop Until B = "$GPGGA"
    Do
     Input S
     B = Mid(s , 2 , 6)
    Loop Until B = "$GPRMC"
   I = Split(s , Contents(1) , ",")                         'process rmc
   Degrees = Left(contents(4) , 2)
   Minutes = Right(contents(4) , 7)
   North = 0
   North = Val(minutes)
   North = North / 60
   K = 0
   K = Val(degrees)
   North = K + North
   Degrees = Left(contents(6) , 3)
   Minutes = Right(contents(6) , 7)
   East = 0
   East = Val(minutes)
   East = East / 60
   K = 0
   K = Val(degrees)
   East = K + East
   If Contents(7) = "W" Then
    East = -east
   End If
   East = Target_e - East
   K = Deg2rad(north)
   K = Cos(k)
   East = East * K                                          'distance to target in equatorial degree units
   North = Target_n - North
   K = East / North
   Target = Atn(k)
   Target = Rad2deg(target)
   If North < 0 Then                                        'direction to target
    Target = Target - 180
   End If
   If Target < -180 Then
    Target = Target + 360                                   'gets it in +-180 degree range
   End If
   K = 0
   K = Val(contents(9))
   V = 0
   V = Val(contents(8))
   If V < 0.5 Then                                          'off if we crashed
    If V = 0 Then
     V = 0.01
    End If
    Statusled = 0
   Else                                                     'indicator LED
    'Incr Timeelapsed
    Toggle Statusled
   End If
   K = Deg2rad(k)
   X = Sin(k)
   X = X * V
   Y = Cos(k)
   Y = Y * V
   X = X - Wind_x                                           'wind compensation  in Y=NORTH frame
   Y = Y - Wind_y
   Airspeed = X * X
   K = Y * Y
   Airspeed = Airspeed + K
   Airspeed = Sqr(airspeed)
   If Y = 0 Then
    X = 0
    Else
    X = X / Y                                               'stops us getting infinity
   End If
   K = Atn(x)
   K = Rad2deg(k)
   If Y < 0 Then
    K = K - 180
   End If                                                   'all to keep us in +-180 degree range
   If K < -180 Then
    K = K + 360                                             'K is now our air vector heading
   End If
   Gpsheading = K
   K = K - Target                                           'k is now our heading offset (from now on is just for
   If K < -180 Then                                         'the pretty leds)
    K = K + 360
   End If
   If K > 180 Then
    K = K - 360
   End If
   If K > 0 Then
    Ledone = 0                                              'left/right indictor LEDs
    Ledtwo = 1
   Else
    Ledone = 1
    Ledtwo = 0
   End If
 'end insert
  I = Split(u , Contents(1) , ",")                          'now we do the gga
  K = Val(contents(10))
  Altitude = K
 If V < 0.5 And Altitude < 250 Then                         'we have landed ?
  Incr Landcount
 Else
  Landcount = 0
 End If
 If Landcount > 4 Then
   Landcount = 5                                            'ie we had 5 stationary gps updates
   Incr Flashcount                                          'flashes leds
   If Flashcount > 9 Then
    Cutdownpintwo = 1
    Flashcount = 0
   Else
    Cutdownpintwo = 0                                       'turn off landing lights
   End If
   Servoenable = 0                                          'turn servo off on landing
 Else
  If Altitude < 300 Then                                    'so we are moving but low
    Cutdownpintwo = 1                                       'turn on landing lights
    Rxenable = 1                                            'enable ground control near the ground
  Else
    Cutdownpintwo = 0
    Rxenable = 0
  End If
  Flashcount = 0
  Servoenable = 1                                           'always enable servo
 End If
 Requestsecond = 1
 While Clearsecond = 0
  Waitus 10
 Wend
 Requestsecond = 0
 Waitus 50                                                  'make sure slave is ready
 Printbin #1 , Statuscode ; Gpsheading ; Target ; Servoenable ; Rxenable
 Waitus 200
 Inputbin #2 , Rate , Temperature , Error , Heading , Filteredrate , Pulselenght , Pulses
 K = K / 100
 Arrayat = Int(k)
 Incr Arrayat                                               'as minimum altitude incrament=0 but array starts at 1
 Gpbyteone = Windxs(arrayat)                                'gives us the correct wind for our altitude
 If Gpbyteone <> 255 Then Wind_x = Gpbyteone - 127          'handles an unrecorded eeprom byte
 Gpbyteone = Windys(arrayat)
 If Gpbyteone <> 255 Then Wind_y = Gpbyteone - 127          'handles unrecorded
 Incr Telcounter
 If Telcounter > 10 Then Call Checkvoltage()
 If Radiocts = 1 Then                                       'is CTS=1 from the radio modem?
  Call Radiocomms()
 End If
Loop

Sub Checkvoltage()
S = "UKHASD,"
Gpintegerone = Getadc(5)
Gpfloatone = Gpintegerone
Gpfloatone = Gpfloatone * 9.503
Gpfloatone = Gpfloatone / 1000
Printstring = Str(gpfloatone )                              'servo
S = S + Printstring
S = S + ","
Gpintegerone = Getadc(0)
Gpfloatone = Gpintegerone
Gpfloatone = Gpfloatone * 4.250
Gpfloatone = Gpfloatone / 1000
Printstring = Str(gpfloatone )
S = S + Printstring
S = S + ","
Printstring = Str(pulselenght)
S = S + Printstring
S = S + ","
Printstring = Str(pulses)
S = S + Printstring
Print S
Print #4 , S
Telcounter = 0
End Sub


Sub Cutdown(byval Channel As Byte , Byval Duration As Byte)
Gpbyteone = 0
While Radiocts = 0 And Gpbyteone < 30                       'wait for radio with timeout
 Waitms 500
 Reset Watchdog
 Incr Gpbyteone
Wend
Print "UKHASC,Cutdown channel=" ; Channel ; " Time=" ; Duration
If Channel = 1 Then Cutdownpinone = 1
If Channel = 2 Then Cutdownpintwo = 1
For Gpbyteone = 0 To Duration
 Wait 1
 Reset Watchdog
Next
Cutdownpinone = 0
Cutdownpintwo = 0
End Sub

Sub Radiocomms()
 If Statuscode = 0 Then
  S = "UKHAS>"
 Else
  S = "UKHAS<"
 End If
 S = S + Contents(3)
 S = S + Contents(4)
 S = S + Contents(5)
 S = S + Contents(6)
 S = S + Contents(10)                                       'nmea data
 Printstring = Fusing(gpsheading , "#.#")
 Printstring = "," + Printstring
 S = S + Printstring
 Printstring = Fusing(target , "#.#")
 Printstring = "," + Printstring
 S = S + Printstring
 Printstring = Str(rate)
 Printstring = "," + Printstring
 S = S + Printstring
 Printstring = Str(temperature)
 Printstring = "," + Printstring
 S = S + Printstring
 Printstring = Str(error)
 Printstring = "," + Printstring
 S = S + Printstring
 Printstring = Fusing(heading , "#.#")
 Printstring = "," + Printstring
 S = S + Printstring
 Printstring = Fusing(filteredrate , "#.#")
 Printstring = "," + Printstring
 S = S + Printstring
 Gpbyteone = Checksum(s)
 S = S + "*"
 Printstring = Hex(gpbyteone)
 S = S + Printstring
 Print S
 Print #4 , S
End Sub