UKHAS Wiki

This is an old revision of the document!

---

$regfile = "m168def.dat"
$crystal = 16000000
$baud = 16000
$hwstack = 40
$swstack = 40
$framesize = 40
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
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 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)
Echo OFF
Start Adc
DIM S AS STRING * 100
DIM T AS STRING * 15
DIM K AS SINGLE
DIM Iterations AS Byte
DIM Windxs(256) AS Eram Byte
DIM Windys(256) AS Eram Byte
DIM LOCK AS Bit
DIM Gpfloatone AS SINGLE , Gpsheading AS SINGLE
DIM R AS STRING * 90
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 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
LOCK = 0
Iterations = 0
Timeelapsed = 0
Wind_x = 0
Wind_y = 0
Waitms 500
CALL Checkvoltage()
Waitms 500
'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 "Enter target north and east"
INPUT #3 , Target_n
PRINT Target_n
INPUT #3 , Target_e
PRINT Target_e
PRINT "Launch!"
DO
 WHILE B <> "$GPGGA"
  INPUT R
  B = Mid(r , 2 , 6)
 WEND
 WHILE B <> "$GPRMC"
  INPUT S
  B = Mid(s , 2 , 6)
 WEND
   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(r , 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(arrayat) = 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(arrayat) = Gpbyteone
    L = 0                                                   'this was forgotten in the flight code doh
   END IF
   IF Arrayat > 40 THEN
    GOTO Cutdownlbl                                         '4Km/20 minute cutdown
   END IF
   IF Timeelapsed > 900 THEN
    GOTO Cutdownlbl
   END IF
   Requestsecond = 1
   WHILE Clearsecond = 0
    Waitus 100
   WEND
   Requestsecond = 0
   PRINT #1 , "#,ascent"
   Waitus 200
   INPUT #2 , S
   Incr Telcounter
   IF Telcounter > 10 THEN CALL Checkvoltage()
                                                             ' our callsign is UKHAS
   IF Radiocts = 1 THEN                                     'is CTS=1 from the radio modem?
    PRINT "UKHAS" ; Contents(3) ; Contents(4) ; Contents(5) ; Contents(6) ; Contents(10) ; S
   END IF
LOOP
 
Cutdownlbl:
CALL Cutdown(1 , 12)
 
 
 
DO
 'insert
 WHILE B <> "$GPGGA"
  INPUT R
  B = Mid(r , 2 , 6)
 WEND
 WHILE B <> "$GPRMC"
  INPUT S
  B = Mid(s , 2 , 6)
 WEND
   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(r , Contents(1) , ",")                         'now we do the gga
  K = VAL(contents(10))
  Altitude = K
 IF V = 0 AND Altitude < 200 THEN                           'turn servo off on landing
   Servoenable = 0
 ELSE
   Servoenable = 1
 END IF                                                     'enable ground control near the ground
 IF Altitude < 200 THEN
   Rxenable = 1
 ELSE
   Rxenable = 0
 END IF
 Requestsecond = 1
 WHILE Clearsecond = 0
  Waitus 100
 WEND
 Requestsecond = 0
 PRINT #1 , "#," ; Gpsheading ; "," ; Target ; "," ; Servoenable ; "," ; Rxenable
 Waitus 200
 INPUT #2 , S
 K = K / 100
 Arrayat = INT(k)
 Gpbyteone = Windxs(arrayat) - 127
 Wind_x = Gpbyteone                                         'gives us the correct wind for our altitude
 Gpbyteone = Windys(arrayat) - 127
 Wind_y = Gpbyteone
 Incr Telcounter
 IF Telcounter > 10 THEN CALL Checkvoltage()
 IF Radiocts = 1 THEN                                       'is CTS=1 from the radio modem?
  PRINT "UKHAS" ; Contents(3) ; Contents(4) ; Contents(5) ; Contents(6) ; Contents(10) ; S
 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)
PRINT Gpintegerone
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
WAIT Duration
Cutdownpinone = 0
Cutdownpintwo = 0
END SUB