' This is the UAV altitude hold program
' Released under a Creative Commons attribution licence by Chris Anderson, DIYDrones.com

' {$STAMP BS2p}
' {$PBASIC 2.5}

' -----[ I/O Definitions ]-------------------------------------------------

Sio             PIN     15     ' connects to GPS Module SIO pin


' -----[ Constants ]-------------------------------------------------------

RX5in           CON     5
T4800           CON     500
Open            CON     $8000
Baud            CON     Open | T4800    ' Open mode to allow daisy chaining
GetAlt          CON     $07
pin2            CON     9

' -----[ Variables ] ------------------------------------------------------

Alt       VAR      Word(3)
AltOriginal VAR Word: Position VAR Word
Lbyte VAR Byte: Ubyte VAR Byte: interim VAR Word
Temp VAR Word

'------[ Main ]------------------------

Main:
  PULSIN RX5in,1,Temp
  IF Temp > 1900 THEN RUN 0 '1900 is a safe value for a mid-point (if the value is greater than 1900, then the
    'gear switch has been moved to the “manual” position).

    ' ------- Get three readings. First...-------
  SEROUT Sio, Baud, ["!GPS", GetAlt]
  SERIN  Sio, Baud, 3000, No_Response, [alt.HIGHBYTE, alt.LOWBYTE]
' ------- Second... -------
  SEROUT Sio, Baud, ["!GPS", GetAlt]
  SERIN  Sio, Baud, 3000, No_Response, [alt.HIGHBYTE(1), alt.LOWBYTE(1)]
' ------- Third...-------
  SEROUT Sio, Baud, ["!GPS", GetAlt]
  SERIN  Sio, Baud, 3000, No_Response, [alt.HIGHBYTE(2), alt.LOWBYTE(2)]
' ------- Average them ----------------
  alt = (alt + alt(1) + alt(2)) / 3

  ' move elevator as required to stay within +-10 meters of original altitude
  GET 6, Word AltOriginal
  IF alt < (AltOriginal + 10) OR alt > (AltOriginal - 10) THEN GOSUB Center
  IF alt >= (AltOriginal + 10) THEN GOSUB Down
  IF alt <= (AltOriginal - 10) THEN GOSUB Up
  DEBUG "Going to program 2"
  RUN 2

' --------[ Subroutines]--------
Center:
  Position = 128 'adjust this so it's centered
  Interim = Position & %00001111 'keep last 4 bits
  Lbyte = Interim | %00100000 'create lower nibble for servo 3 (0010 means “lower nibble for servo 3” in FT639-speak
  Interim = Position & %11110000
  Ubyte = (Interim >> 4) | %10100000 'create upper nibble for servo 3 (1010 means “upper nibble for servo 3” in FT639-speak
  SEROUT pin2,baud,[Lbyte,Ubyte]:PAUSE 10 'sends command to FT639
  RETURN

Down:
  Position = 100 'adjust this so it's a bit down
  Interim = Position & %00001111
  Lbyte = Interim | %00100000
  Interim = Position & %11110000
  Ubyte = (Interim >> 4) | %10100000
  SEROUT pin2,baud,[Lbyte,Ubyte]:PAUSE 10
  RETURN

Up:
  Position = 150 'adjust this so it's a bit up
  Interim = Position & %00001111
  Lbyte = Interim | %00100000
  Interim = Position & %11110000
  Ubyte = (Interim >> 4) | %10100000
  SEROUT pin2,baud,[Lbyte,Ubyte]:PAUSE 10
  RETURN

No_Response:
  PAUSE 5000
  GOTO Main