' {$STAMP BS2pe}
' {$PBASIC 2.5}
' iridium modem should be set to 600 baud/8 bit/no parity
' GPS should be configured to output a GLL string at 2400 baud/8 bit/no parity
' MVC communications are at 2400 baud/8 bit/no parity

OUTPUT 3
OUTPUT 14
OUTPUT 15
INPUT 11
INPUT 4
INPUT 0
OUTPUT 1
OUTPUT 7
OUTPUT 12
INPUT 10

gpsTimeHr VAR Byte
gpsTimeMin VAR Byte
gpsTimeSec VAR Byte
gpsTImeFrac VAR Byte
gpsLatDeg VAR Byte
gpsLatMin VAR Byte
gpsLatMinFrac VAR Word
latSect VAR Byte(2)
gpsLonDeg VAR Byte
gpsLonMin VAR Byte
gpsLonMinFrac VAR Word
lonSect VAR Byte(2)
gpsQual VAR Byte(2)
gpsMode VAR Byte(2)
checksum VAR Byte
updateCount VAR Word
moStatus VAR Byte
cnt VAR Byte
mvcInput VAR Nib
gpstimeout VAR Nib

HIGH 14    'put watchdog in 500x mode

gpsTimeout = 0
moStatus=2
updateCount = 910
pwrChkLoop:
TOGGLE 15
DEBUG "sleep count is ", DEC updateCount,CR
IF IN4 = 1 THEN SEROUT 12,396,["sleep count is ", DEC updateCount,13,10]
DEBUG "12V good is ", DEC IN4,CR
IF IN4 = 1 THEN SEROUT 12, 396,["12V good is ", DEC IN4,13,10]
DEBUG "iridium modem net avail is ", DEC IN0, CR
IF IN4 = 1 THEN SEROUT 12,396,["iridium modem net avail is ", DEC IN0, 13,10]
IF IN4 = 1 THEN  'turn on power converter if 12V input is good
  HIGH 3
  HIGH 7
  DEBUG "in mainLoop, pwr on",CR
  SEROUT 12,396,["in mainLoop, pwr on",13,10]
ELSE 'if 12V power is off, then turn off the power converter (it gets turned back on when its time to beacon)
  LOW 3
  LOW 7
  DEBUG "in mainLoop, pwr off",CR
ENDIF
updateCount = updateCount+22
TOGGLE 15
IF IN4 = 1 THEN SEROUT 12,396,["Send Command Now",CR]
cnt=0
mvcInput=0
IF IN4 = 1 THEN SERIN 10,396,2000,noInputFromMVC,[WAIT("cmd:"),DEC mvcInput]
IF mvcInput=1 THEN updateCount=910
noInputFromMVC:
IF IN4=0 OR moStatus=1 THEN
  DEBUG "sleeping", CR
  SLEEP 20
  DEBUG "waking up", CR
  ENDIF
TOGGLE 15 'toggle watchdog on way out of sleep
IF moStatus<>1 AND IN4=1 THEN updateCount = 910
IF updateCount > 900 THEN

  HIGH 7    'turn on modem
  DEBUG "pwr on", CR
  IF IN4 = 1 THEN SEROUT 12,396,["pwr on", 13,10]
  HIGH 3
  FOR cnt=1 TO 60 '2 minutes of trying to get a gps fix
    GOSUB getGPSFix
    IF gpsQual(0)<>"A" THEN
      gpsLatDeg = 0
      gpsLatMin = 0
      gpsLatMinFrac = 0
      latSect = "W"
      gpsLonDeg = 0
      gpsLonMin = 0
      gpsLonMinFrac = 0
      lonSect = "N"
    ENDIF
    IF IN4 = 1 THEN SEROUT 12,396,["timeout ", DEC gpstimeout, " cnt ",DEC cnt, " $GPGLL,", DEC2 gpsLatDeg, DEC2 gpsLatMin,".", DEC5 gpsLatMinFrac,",", STR latSect,",", DEC3 gpsLonDeg, DEC2 gpsLonMin,".", DEC5 gpsLonMinFrac,",", STR lonSect,",", DEC2 gpsTimeHr,DEC2 gpsTimeMin, DEC2 gpsTimesec, ".",DEC2 gpsTimeFrac,",", STR gpsQual, ",", STR gpsMode, "*", HEX2 checksum, 13,10]
    DEBUG "$GPGLL,", DEC2 gpsLatDeg, DEC2 gpsLatMin,".", DEC5 gpsLatMinFrac,",", STR latSect,",", DEC3 gpsLonDeg, DEC2 gpsLonMin,".", DEC5 gpsLonMinFrac,",", STR lonSect,",", DEC2 gpsTimeHr,DEC2 gpsTimeMin, DEC2 gpsTimesec, ".",DEC2 gpsTimeFrac,",", STR gpsQual, ",", STR gpsMode, "*", HEX2 checksum, CR
    IF gpsQual(0)="A" THEN EXIT
  NEXT
  IF IN4 = 1 THEN SEROUT 12,396,["$GPGLL,", DEC2 gpsLatDeg, DEC2 gpsLatMin,".", DEC5 gpsLatMinFrac,",", STR latSect,",", DEC3 gpsLonDeg, DEC2 gpsLonMin,".", DEC5 gpsLonMinFrac,",", STR lonSect,",", DEC2 gpsTimeHr,DEC2 gpsTimeMin, DEC2 gpsTimesec, ".",DEC2 gpsTimeFrac,",", STR gpsQual, ",", STR gpsMode, "*", HEX2 checksum, 13,10]
  IF IN4 = 1 AND gpsQual(0)<> "A" THEN 'if 12V power is good but we didn't get a GPS fix, set moStatus to failed and try again for GPS
    moStatus = 2
    DEBUG "pwr good, no gps, try again", CR
    GOTO pwrChkLoop
  ENDIF
  FOR cnt=1 TO 30 'roughly half a minute of trying to send an SBD
    TOGGLE 15
    DEBUG "iridium netavail ", DEC IN0, CR
    IF IN4 = 1 THEN SEROUT 12,396,["iridium netavail ", DEC IN0, " cnt is ", DEC cnt, 13,10]
    GOSUB sendIridiumUpdate
    IF moStatus = 1 THEN
   updateCount = 0
   EXIT
    ENDIF
  NEXT
ENDIF
GOTO pwrChkLoop

getGPSFix:

latSect(1) = 0
lonSect(1) = 0
gpsQual(1) = 0
gpsMode(1) = 0
waitForFix:
TOGGLE 15
SERIN 11,396,1200,timeoutFromGPS,[WAIT("GPGLL,"),DEC2 gpsLatDeg,DEC2 gpsLatMin,SKIP 1,DEC5 gpsLatMinFrac,SKIP 1,STR latSect\1,SKIP 1,DEC3 gpsLonDeg,DEC2 gpsLonMin,SKIP 1,DEC5 gpsLonMinFrac,SKIP 1,STR lonSect\1,SKIP 1,DEC2 gpsTimeHr,DEC2 gpsTimeMin,DEC2 gpsTimeSec,SKIP 1,DEC2 gpsTimeFrac,SKIP 1,STR gpsQual\1, SKIP 1,STR gpsMode\1, SKIP 1, HEX2 checksum]
gpstimeout = 0
RETURN
timeoutFromGps:
gpstimeout=1
  gpsLatDeg = 0
  gpsLatMin = 0
  gpsLatMinFrac = 0
  latSect = "W"
  gpsLonDeg = 0
  gpsLonMin = 0
  gpsLonMinFrac = 0
  lonSect = "N"
RETURN

sendIridiumUpdate:
SEROUT 1,1646,["AT+SBDWT=", DEC2 gpsLatDeg, DEC2 gpsLatMin,".",DEC5 gpsLatMinFrac,",",STR latSect,",",DEC3 gpsLonDeg, DEC2 gpsLonMin,".",DEC5 gpsLonMinFrac,",",STR lonSect,",", DEC2 GPSTimeHr,DEC2 gpsTimeMin, DEC2 gpsTimesec,".",DEC2 gpsTimeFrac, ",", STR gpsQual, 10, 13]
DEBUG "AT+SBDWT=",DEC2 gpsLatDeg, DEC2 gpsLatMin,".",DEC5 gpsLatMinFrac,",",STR latSect,",",DEC3 gpsLonDeg, DEC2 gpsLonMin,".",DEC5 gpsLonMinFrac,",",STR lonSect,",", DEC2 GPSTimeHr,DEC2 gpsTimeMin, DEC2 gpsTimesec,".",DEC2 gpsTimeFrac,",", STR gpsQual, CR
IF IN4 = 1 THEN SEROUT 12,396,["AT+SBDWT=",DEC2 gpsLatDeg, DEC2 gpsLatMin,".",DEC5 gpsLatMinFrac,",",STR latSect,",",DEC3 gpsLonDeg, DEC2 gpsLonMin,".",DEC5 gpsLonMinFrac,",",STR lonSect,",", DEC2 GPSTimeHr,DEC2 gpsTimeMin, DEC2 gpsTimesec,".",DEC2 gpsTimeFrac,",", STR gpsQual, 13,10]
SERIN 2,1646,5000,noIridiumOK, [WAIT("OK")]
noIridiumOK:
SEROUT 1,1646,["AT+SBDI", 10, 13]
SERIN  2,1646,20000,noIridium,[WAIT("SBDI:"),DEC1 moStatus]
DEBUG "moStatus ", DEC1 moStatus, CR
IF IN4 = 1 THEN SEROUT 12,396,["moStatus ", DEC1 moStatus, 13,10]
returnFromSBD:
RETURN

noIridium:
DEBUG "timeout sending iridium ", CR
IF IN4 = 1 THEN SEROUT 12,396,["timeout sending iridium ", 13,10]
moStatus = 2
GOTO returnFromSBD