UKHAS Wiki

This is an old revision of the document!

---

$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
DIM Windxs(255) AS Eram Byte
DIM Windys(255) AS Eram Byte , Placemark AS Eram Byte
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
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 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 Target_e AS SINGLE
DIM Target_n 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
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
 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 255
 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
Portd.3 = 1                                                 'radiocts pullup
PRINT "Launch!"
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 > 900 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:
CALL Cutdown(1 , 12)
Placemark = 2                                               'write eeprom
Descentloop:
Statuscode = 1
Requestsecond = 1
WHILE Clearsecond = 0
 Waitus 10
WEND
Requestsecond = 0
Waitus 50
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 AND Altitude < 300 THEN                           'turn servo off on landing
   Servoenable = 0
 ELSE
   Servoenable = 1
 END IF                                                     'enable ground control near the ground
 IF Altitude < 300 THEN
   Rxenable = 1
 ELSE
   Rxenable = 0
 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
 Wind_x = Gpbyteone - 127
 IF Wind_x = 128 THEN Wind_x = 0                            'handles an unrecorded eeprom byte
 Gpbyteone = Windys(arrayat)
 Wind_y = Gpbyteone - 127
 IF Wind_y = 128 THEN Wind_y = 0                            'handles unrecorded
 Incr Telcounter
 IF Telcounter > 10 THEN CALL Checkvoltage()
 IF Radiocts = 1 THEN                                       'is CTS=1 from the radio modem?
  CALL Radiocomms()
  Printstring = Str(gpsheading)
  Printstring = Left(printstring , 8)
 
 END IF
LOOP
 
SUB Checkvoltage()
Gpintegerone = Getadc(5)
Gpfloatone = Gpintegerone
Gpfloatone = Gpfloatone * 9.503
Gpfloatone = Gpfloatone / 1000
IF Radiocts = 1 THEN PRINT "Servo voltage=" ; Gpfloatone ; ",";
Gpintegerone = Getadc(0)
Gpfloatone = Gpintegerone
Gpfloatone = Gpfloatone * 4.250
Gpfloatone = Gpfloatone / 1000
IF Radiocts = 1 THEN PRINT "Supply voltage=" ; Gpfloatone
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
 Incr Gpbyteone
WEND
PRINT "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()
 PRINT "UKHAS>" ; Contents(3) ; Contents(4) ; Contents(5) ; Contents(6) ; Contents(10) ;       'nmea data
 PRINT #4 , "UKHAS>" ; Contents(3) ; Contents(4) ; Contents(5) ; Contents(6) ; Contents(10) ;
 Printstring = Fusing(gpsheading , "#.#")
 PRINT "," ; Printstring;
 PRINT #4 , "," ; Printstring;
 Printstring = Fusing(target , "#.#")
 PRINT "," ; Printstring;
 PRINT #4 , "," ; Printstring;
 Printstring = Str(rate)
 PRINT "," ; Printstring;
 PRINT #4 , "," ; Printstring;
 Printstring = Str(temperature)
 PRINT "," ; Printstring;
 PRINT #4 , "," ; Printstring;
 PRINT "," ; ERROR;
 PRINT #4 , ERROR;
 Printstring = Fusing(heading , "#.#")
 PRINT "," ; Printstring;
 PRINT #4 , "," ; Printstring;
 Printstring = Fusing(filteredrate , "#.#")
 PRINT "," ; Printstring;
 PRINT #4 , "," ; Printstring;
 Printstring = Str(pulselenght)
 PRINT "," ; Printstring;
 PRINT #4 , "," ; Printstring;
 Printstring = Str(pulses)
 PRINT "," ; Printstring
 PRINT #4 , "," ; Printstring                               'we copy everything to the logger
END SUB