''******************************************** ''* GPS Driver v2.51 * ''* By: Jim Miller, 0404/2014 * ''******************************************** CON _clkmode = xtal1 + pll16x _xinfreq = 5_000_000 StackSpace = 60 OBJ GPS : "FullDuplexSerialExtended" f : "F32" fs : "FloatString" Var LONG Stack[StackSpace] long TimeZone ' long LastAzm, LastAlt long SolarDec long DayOfYear ' old location long LeapYear long Day, dDay long Month, dMonth long Year,dYear long Hour,Minute long dHour,dMinute,dSecond ' Do not change the order of these! long DelayMinute long Second long Lat,LatD, Lon,LonD ' Lattitude and Longitude in Degrees and Floating Point ' Signed E = - S = - long LatM,LonM ' Lat and Lon mantissa's long HardLat,HardLon ' HardCode Lat and Long - Signed integers long HourAngle,SolarAlt,SolarAzm ',AngleOTime,EoTime long ErrorCount,QAzm,QAlt BYTE CheckSum byte Fix byte NewAAData byte RTClk,RTDat,TimeSource byte HoldFlag byte seed byte GPSString[70] byte TimeString[12] byte DateString[12] byte LatString[12] byte LongString[12] byte LonEW[5] byte LatNS[5] byte gpsType[10] byte SOGstring[8] byte CMGstring[8] byte gpsNRW[5] byte RT_bytes[7] PUB start(GPS_Rx, GPS_Tx,GPS_Baud,GPS_Reset,GPS_Time,dbg,RT_CLK,RT_Data,TimeSourc) : okay | i seed := Seed? ' Random number into seed (a byte) bytefill(@Stack,seed, StackSpace * 4) ' Fill stackspace: a constant Repeat i from 0 to 4 ' Launch the cog @Stack gpsNRW[i] := 0 f.Start ' Spawns 1 cog floating point routines HourAngle:= 0 ' initialize lat/lon variables until controller takes over SolarAlt := 0 SolarAzm := 0 Lat := 0 LatD := 0 Lon := 0 LonD := 0 Fix := 0 ' GPS Fix assumed bad until adjusted. RTClk := RT_CLK ' RTDat := RT_Data ' dira[RTClk] := 0 ' dira[RTDat] := 0 ' ' dira[GPS_Reset] := 1 ' dira[GPS_Time] := 0 ' outa[GPS_Reset] := 1 ' Reset is active LOW so Pull it up TimeSource := TimeSourc ' 0 for GPS - 1 for realtime clock ' RTClockData ' Load data from rt clock system ' Populate ' cognew(GPSMain(GPS_Rx, GPS_Tx,GPS_Baud), @Stack) ' Start the GPS system waitcnt (Cnt + 80_000_000) ' Let the cog load pub TestStack | i,j '' Diagnostic routine for testing stack size. j:= seed ' Initialize j i := @Stack + (StackSpace * 4) - 1 ' Position i at 'top' of stack repeat while j == seed and i > @Stack - 1 ' Work down from top looking for non-seed value bytemove(@j,i,1) i -- ' Return answer in longs +- 1 for round-off i := (i - @Stack) / 4 return i PUB GPSMain(GPS_Rx, GPS_Tx,GPS_Baud) '' Main loop for GPS GPS.start(GPS_Rx, GPS_Tx,0,GPS_Baud) 'Start GPS Driver object spawns 1 cog waitcnt(cnt + 80_000_000) ' Wait for eFDS to laod NewAAData := 0 Repeat If TimeSource == 0 GPSSource HoldFlag := 0 if TimeSource == 1 RTClockData HoldFlag := 1 Calculate HoldFlag := 0 Waitcnt(cnt + 80_000_000) Pub GPSSource | data,i,c '' Gets GPS data string and parses it out to individual i := 0 c := 0 Repeat while NewAAData == 0 ' This repeat builds entire string from $ to $0D Terminator Data := GPS.rx if Data == $24 ' Is it a $ sign i := 0 ' If it is, zero the string position counter GPSString[i] := Data ' Put it into the buffer i ++ ' Increment the position counter if data == "*" ' If it's a * then transmission is complete, next two are checksum GPSString[i++] := "<" ' Add String Terminator instead of $0D GPSString[i] := 0 ' Add String Terminator CheckSumCalc ' add this byte to checksum if TypeGPSData == 1 HoldFlag := 1 Parse Calculate HoldFlag := 0 if i > 69 i := 0 Pub TypeGPSData | i repeat i from 0 to 5 GPSType[i] := GPSString[i] i := 0 if strcomp(@GPSType,string("$GPRMC")) i := 1 if strcomp(@GPSType,string("$GPGSA")) i := 2 if strcomp(@GPSType,string("$GPGSV")) i := 3 return i Pub Parse | data,i,j,k,flag i := 0 ' GPSString position counter k := 0 ' k = comma counter (variable counter) j := 0 ' j = sub string position counter flag := 1 repeat while flag == 1 data := GPSString[i] if data == $2C ' Is it a comma data := 0 ' set data as String Terminator case k 0: GPSType[j] := data 1: TimeString[j] := data 2: gpsNRW[j] := data 3: LatString[j] := data 4: LatNS[j] := data 5: LongString[j] := data 6: LonEW[j] := data 7: SOGstring[j] := data 8: CMGstring[j] := data 9: DateString[j] := data j ++ i ++ if data == 0 ' End of a string k ++ ' Yes so next substring j := 0 ' zero local substring if i == 99 or k > 9 flag := 0 ' Set to get out of repeat Pub CheckSumCalc | i,flag CheckSum := 0 Repeat i from 0 to strsize(@GPSString) if GPSString[i] == "$" Flag := 1 i ++ if GPSString[i] == "*" Flag := 0 if Flag == 1 Checksum := CheckSum ^ GPSString[i] pub GetGPSSource return @GPSString Pub GetCheckSum return CheckSum Pub GetGPSType return @GPSType Pub GetTimeString return @TimeString Pub GetgpsNRW return @gpsNRW Pub GetLatString return @LatString Pub GetLatNS return @LatNS Pub GetLongString return @LongString Pub GetLonEW return @LonEW Pub GetSOGstring return @SOGstring Pub GetCMGstring return @CMGstring Pub GetDateString return @DateString pub Calculate '' Top of calculation loop - calls all major math routines in sequence Populate LatM:=0 LonM:=0 ' Get numbers out of Time Base arrival ' if dMinute <> DelayMinute ' If a minute has rolled over then calculate new stuff DelayMinute := dMinute DayOfYear := DayOfYr(dYear,dMonth,dDay) ' Function Verified - Returns Integer SolarDec := SunDec(DayOfYear,dHour,dMinute) ' Function Verified - Returns radian unit as floating point HourAngle := HourAng(dHour,dMinute) ' Function Verified - Returns radian unit as floating point SolarAlt := SolarAltitude(Lat,SolarDec,HourAngle) ' SolarAzm := SolarAzmuth(SolarDec,HourAngle,SolarAlt,Lat) If ||(f.fround(RadToDeg(QAzm)) - f.fround(RadToDeg(SolarAzm))) > 2 ErrorCount ++ If ||(f.fround(RadToDeg(QAlt)) - f.fround(RadToDeg(SolarAlt))) > 2 ErrorCount ++ if f.fround(RadToDeg(SolarAzm)) > 360 or f.fround(RadToDeg(SolarAzm)) < 0 ErrorCount ++ if f.fround(RadToDeg(SolarAlt)) > 360 or f.fround(RadToDeg(SolarAlt)) < -180 ErrorCount ++ if f.fround(Lat) > 90 or f.fround(Lat) < -90 ErrorCount ++ if f.fround(Lon) > 180 or f.fround(Lon) < -180 ErrorCount ++ If ErrorCount == 0 QAzm := SolarAzm QAlt := SolarAlt NewAAData := 1 ' All done so set new altitude azimuth data arrival If ErrorCount == 2 QAzm := SolarAzm QAlt := SolarAlt ErrorCount := 0 pub GetErrorCount Return ErrorCount pub TimeStringLoc '' Returns address of Time String Return @dHour pub Populate | work '' Populate all key time, lat, lon variables if TimeSource == 0 ' If TimeSource is 0 then use GPS Time - Lat - Lon Data dDay := AscToInt(@DateString[0],2) dMonth := AscToInt(@DateString[2],2) dYear := 2000 + AscToInt(@DateString[4],2) dHour := AscToInt(@TimeString[0],2) dMinute := AscToInt(@TimeString[2],2) dSecond := AscToInt(@TimeString[4],2) Second := f.ffloat(AscToInt(@TimeString[4],2)) LatD := ((AscToInt(@LatString[0] ,2))#> 0) <# 180 LatM := f.ffloat(AscToInt(@LatString[2] ,2)) ' Lat Mantissa LatM := f.fdiv(LatM,60.0) Lat := f.ffloat(LatD) Lat := f.fadd(Lat,LatM) LonD := ((AscToInt(@LongString[0],3))#> -181)<# 181 LonM := f.ffloat(AscToInt(@LongString[3],2)) LonM := f.fdiv(LonM,60.0) Lon := f.ffloat(LonD) Lon := f.fadd(Lon,LonM) If LatNS[0] == $53 ' Is it an 'S' * -1 Lat:= f.FMul(Lat,-1.0) LatD:= LatD * -1 If LonEW[0] == $57 ' Is it an 'W' * -1 Lon:= f.FMul(Lon,-1.0) LonD:= LonD * -1 Fix := 0 if strcomp(@gpsNRW,string("A")) Fix := 4 if strcomp(@gpsNRW,string("V")) Fix := 3 if TimeSource == 1 ' If TimeSource is 1 then use RT Time - HardLat - HardLong work := RT_bytes[0] dSecond := 10 * ((Work >> 4) & %0000_0111) + (Work & %0000_1111) work := RT_bytes[1] dMinute := 10 * ((work >> 4)& %0000_0111) + (work & %0000_1111) work := RT_bytes[2] dHour := 10 * ((work >> 4)& %0000_0011) + (work & %0000_1111) work := RT_bytes[4] dDay := 10 * ((work >> 4)& %0000_0011) + (work & %0000_1111) work := RT_bytes[5] dMonth := 10 * ((work >> 4)& %0000_0001) + (work & %0000_1111) work := RT_bytes[6] dYear := 2000 + (10 * ((work >> 4)& %0000_1111) + (work & %0000_1111)) Second := f.ffloat(dSecond) Lat := f.ffloat(HardLat) ' Use the hard code values for Lat and Lon Lon := f.ffloat(HardLon) Fix := 4 ' Set GPS fix to fool startup Pub GetHourAngle '' Return Hour Angle in degrees Repeat while HoldFlag == 1 waitcnt(cnt + 5_000) if f.FRound(RadtoDeg(HourAngle)) > 360 or f.FRound(RadtoDeg(HourAngle)) < -360 Return 60 else Return f.FRound(RadtoDeg(HourAngle)) Pub GetSolarAlt | work '' Return Solar Altitude in Integer Degrees Repeat while HoldFlag == 1 waitcnt(cnt + 5_000) 'work := (f.fround(RadToDeg(SolarAlt)) #> 0) <# 360 return f.fround(RadToDeg(QAlt)) ' work Pub GetSolarAzm |SA,HA '' Return Solar Azmuth in Integer Degrees Repeat while HoldFlag == 1 waitcnt(cnt + 5_000) SA := f.fround(RadToDeg(QAzm)) HA := f.fround(RadToDeg(HourAngle)) if Ha > -180 and Ha =< 0 ' Solar AM if SA => 180 SA := SA - 180 Else ' Solar PM if SA < 180 SA := SA + 180 If SA > 359 SA := SA - 180 IF HA == 0 SA := 180 'SA := (SA #> 0) <# 90 return SA Pub GetAddressLat return @Lat Pub GetAddressLon return @Lon Pub GetAddressAlt return @SolarAlt Pub GetFix Repeat while HoldFlag == 1 waitcnt(cnt + 5_000) return Fix Pub GetLat '' Return Local Lattitude in Integer Degrees Repeat while HoldFlag == 1 waitcnt(cnt + 5_000) return f.fround(Lat) Pub GetLon '' Return Local Longitude in Integer Degrees Repeat while HoldFlag == 1 waitcnt(cnt + 5_000) return f.fround(Lon) Pub GetHardLat '' Return Local Hard coded Lattitude in Integer Degrees return HardLat Pub GetHardLon '' Return Local Hard coded Longitude in Integer Degrees return HardLon Pub SetHardLat(x) '' Set Local Hard coded Lattitude in Integer Degrees HardLat := x Pub SetHardLon(x) '' Set Local Hard coded Longitude in Integer Degrees HardLon := x pub GetDayIndexAddress '' Returns address of the DayOfYear index as integer Repeat while HoldFlag == 1 waitcnt(cnt + 5_000) return @DayOfYear Pub GetSecond '' Return Integer Second Repeat while HoldFlag == 1 waitcnt(cnt + 5_000) return dSecond Pub GetMinute '' Return Integer Minute Repeat while HoldFlag == 1 waitcnt(cnt + 5_000) return dMinute Pub GetHour '' Return Integer Hour Repeat while HoldFlag == 1 waitcnt(cnt + 5_000) return dHour pub GetDay '' Return Integer Day in current month Repeat while HoldFlag == 1 waitcnt(cnt + 5_000) return dDay pub GetMonth '' Return Integer Month Repeat while HoldFlag == 1 waitcnt(cnt + 5_000) return dMonth pub GetYear '' Return Integer Year Repeat while HoldFlag == 1 waitcnt(cnt + 5_000) Return dYear Pub GetDayOfYear '' Return Integer that is the index of the day of the year Repeat while HoldFlag == 1 waitcnt(cnt + 5_000) return DayOfYear Pub GetNewAAData '' Returns status of New Azm & Alt data flag Repeat while HoldFlag == 1 waitcnt(cnt + 5_000) return NewAAData Pub ZeroNewAAData '' Zeros New Azm & Alt data flag NewAAData := 0 Pub SolarAzmuth(Dec_R,Hour_R,SAlt_R,Lat_D) | x_azm, y_azm1, y_azm2, azm ''Returns solar azimuth in radians floating point x_azm := f.fmul(f.sin(Hour_R),f.cos(Dec_R)) y_azm1 := f.fmul(f.fmul(f.fmul(-1.0,f.cos(Hour_R)),f.cos(Dec_R)),f.sin(DegToRad(Lat_D))) y_azm2 := f.fmul(f.cos(degToRad(Lat_D)),f.sin(Dec_R)) azm := f.fsub(3.1415,f.atan(f.fdiv(x_azm,f.fadd(y_azm1,y_azm2)))) return azm Pub SolarAltitude(Lt,Dec,HAngle) |L,Sa1,Sa2,Sa3,Sa4 '' Returns Solar Altitude in Radians above horizon floating point L := DegToRad(Lt) Sa1 := f.fmul(f.fmul(f.cos(L),f.cos(Dec)),f.cos(HAngle)) Sa2 := f.fmul(f.sin(L),f.sin(Dec)) Sa3 := f.fadd(Sa1,Sa2) Sa4 := f.asin(Sa3) return Sa4 Pub SunDec(N,dH,dM) | z,z1,z2,z3,p2,dr '' Returns Suns Declination in Radians N := N - 80 ' N == day of year Jan 1 = 1, Dec 31 =356 z := f.fadd(f.fadd(f.ffloat(N),f.fdiv(f.ffloat(dH),24.0)),f.fdiv(f.ffloat(dM),1440.0)) ' z == decimil day of year to minute z1 := f.fdiv(z,365.0) p2 := f.fmul(pi,2.0) z2 := f.fmul(p2,z1) z1 := f.sin(z2) dr := DegtoRad(23.43) z3 := f.fmul(dr,z1) return z3 pub HourAng(dH,dM) | Ha,Ha1,Ha2,Ha3 '' Returns floating point Hour angle in Radians dH := dH - 12 ' Subtract off noon Ha := f.fdiv(f.ffloat(dM),60.0) ' Define minutes as decimil hour Ha1 := f.fadd(f.ffloat(dH),Ha) ' Add minutes to hour Ha1 is decimil Hour Ha2 := f.fmul(Ha1,15.0) ' 15 deg per hour Ha3 := f.fadd(Ha2,Lon) ' Add the longitude Ha := DegToRad(Ha3) ' Turn it to radians return Ha ' Hour angle in radians is returned. Pub RadToDeg(Rad)|Deg '' Converts radians to degrees returns fp Deg := f.fdiv(f.fmul(180.0,Rad),pi) Return Deg Pub DegToRad(Deg)|Rad '' Converts degrees to Radians returns fp Rad := f.fdiv(f.fmul(Deg,pi),180.0) Return Rad Pub DayOfYr(Y,M,D) | Dyr '' Returns Integer Day of Year from date If ((10*Y)/4) & $00000001 > 0 ' Determine Leap year if even divisible by 4 LeapYear := 0 else LeapYear := 1 Dyr := D + LeapYear ' add the day of the month and LeapYear Case M ' Now add in days for months 2: ' In Feb, Add Jan and so on Dyr := Dyr + 31 3: Dyr := Dyr + 59 4: Dyr := Dyr + 90 5: Dyr := Dyr + 120 6: Dyr := Dyr + 151 7: Dyr := Dyr + 181 8: Dyr := Dyr + 212 9: Dyr := Dyr + 243 10: Dyr := Dyr + 273 11: Dyr := Dyr + 304 12: Dyr := Dyr + 334 return Dyr Pub AscToInt(StrLoc,StrLen): Val | i, Get '' Converts an ASCII string into an integer Val := 0 Get := 0 Repeat i from 0 to StrLen - 1 ByteMove(@Get,StrLoc + i,1) ' Move ASCII byte to location of get Get := (Get - $30) #> 0 ' Shift it to Numerical Value but not less than 0 Case StrLen - i 1: Val := Val + Get 2: Val := Val + (Get * 10) 3: Val := Val + (Get * 100) 4: Val := Val + (Get * 1_000) pub SetTimeSource(s) '' Sets flag to look at real time clock, not GPS TimeSource := s If s == 1 Fix := 4 pub GetTimeSource '' Returns value of TimeSource Return TimeSource pub SetRTByte(Bt,Value) '' Return the location of the first byte in the array RT_Bytes[Bt] := Value Pub WriteTimeData '' Transfer function from PRI to PUB SetRTClock PRI RTClockData | i '' Loads real time clock data from chip to memory RTInit RTstart RTwrite(%1101_0000) ' Slave Address, write RTwrite(%0000_0000) ' Sub-Address of starting register RTstart ' do start again RTwrite(%1101_0001) ' Slave Address again read mode Repeat i from 0 to 7 RT_Bytes[i] := RTread(0) RTstop PRI SetRTClock | i '' Writes real time clock data from memory to clock RTInit RTstart RTwrite(%1101_0000) ' Slave Address, write RTwrite(%0000_0000) ' Sub-Address of starting register Repeat i from 0 to 6 ' Write the Bytes RTwrite(RT_Bytes[i]) RTstop ' Send stop sequence to clock - does not stop the clock just tranmission to it PRI RTinit | okay '' Initializes RT clock chip '' Define I2C SCL (clock) and SDA (data) pins dira[RTDat] := 0 ' float buss pins dira[RTClk] := 0 PRI RTwait(id) | ackbit,ACK '' Delay cycle for real time clock ACK := 0 '' Waits for I2C device to be ready for new command repeat RTstart ackbit := RTwrite(id & $FE) until ackbit == ACK PRI RTstart '' Start sequence to real time clock '' Create I2C start sequence '' -- will wait if I2C buss SDA pin is held low dira[RTDat] := 0 ' float SDA (1) dira[RTClk] := 0 ' float SCL (1) repeat while (ina[RTClk] == 0) ' allow "clock stretching" outa[RTDat] := 0 dira[RTDat] := 1 ' SDA low (0) PRI RTwrite(i2cbyte) | ackbit '' Write one byte to RT clock '' Write byte to I2C buss outa[RTClk] := 0 dira[RTClk] := 1 ' SCL low i2cbyte <<= constant(32-8) ' move msb (bit7) to bit31 repeat 8 ' output eight bits dira[RTDat] := ((i2cbyte <-= 1) ^ 1) ' send msb first dira[RTClk] := 0 ' SCL  (float to p/u) dira[RTClk] := 1 ' SCL  dira[RTDat] := 0 ' relase SDA to read ack bit dira[RTClk] := 0 ' SCL  (float to p/u) ackbit := ina[RTDat] ' read ack bit dira[RTClk] := 1 ' SCL  return (ackbit & 1) PRI RTread(ackbit) | i2cbyte '' Read one byte from RT clock '' Read byte from I2C buss outa[RTClk] := 0 ' prep to write low dira[RTDat] := 0 ' make input for read repeat 8 dira[RTClk] := 0 ' SCL  (float to p/u) i2cbyte := (i2cbyte << 1) | ina[RTDat] ' read the bit dira[RTClk] := 1 ' SCL  dira[RTDat] := !ackbit ' output ack bit dira[RTClk] := 0 ' SCL  (float to p/u) dira[RTClk] := 1 ' SCL  return (i2cbyte & $FF) PRI RTstop '' Stop sequence to RT clock '' Create I2C stop sequence outa[RTDat] := 0 dira[RTDat] := 1 ' SDA low dira[RTClk] := 0 ' float SCL repeat while (ina[RTClk] == 0) ' hold for clock stretch dira[RTDat] := 0 ' float SDA