View previous topic :: View next topic 
Author 
Message 
snipsnip
Joined: 10 Feb 2014 Posts: 66 Location: Melbourne

Posted: Tue Dec 23, 2014 2:20 am Post subject: GPS Functions 


hi all,
just want to share and collect GPS functions
only got a couple of rough ones at the moment  haven't really optimised them yet (WIP)
please if you have GPS functions to share, post them here
1st one below sucks out data fields between commas  needs header info and the start comma number
Code: 
Function Commapos (startcomma As Byte , Header As String) As String
Local N As Byte , Q As Byte , Comma As Byte , X As String * 1 , Endcomma As Byte , Ch As String * 1 , Ggps As String * 5
Local Gps1 As String * 80
'CLEAR BUFFER, LOOPS 'TILL NOTHING LEFT
While Ischarwaiting() > 0
Ch = Inkey()
Wend
'SCROLL BUFFER AND GET GET THE SENTENCE THAT STARTS WITH "HEADER" IE  GPRMC,GPGGL......
Ggps = "" 'CLEAR GGPS JUST IN CASE
While Ggps <> Header
Do
Ch = Inkey()
Loop Until Ch = "$"
Input , Gps1 Noecho 'GET A FULL SENTENCE
Ggps = Left(gps1 , 5) 'CHECK IT FOR A MATCH
Wend
'n=start posistion q=end posistion
'q also = grab string length ie mid(var,n,q)
'find index(N) for start comma posistion
Comma = 0 : N = 0 : Q = 0 'INITIALISE VARIABLES
Endcomma = Startcomma + 1
While Comma < Startcomma 'COMMA HOLD THE COMMA COUNT AMOUNT
Incr N
X = Mid(gps1 , N , 1)
If X = "," Then Incr Comma
Wend
'find index(Q) for end comma posistion
Q = N
While Comma < Endcomma
Incr Q
X = Mid(gps1 , Q , 1)
If X = "," Then Incr Comma
Wend
'not quite rightTWO COMMAS IN A ROW CAUSE PROBLEMS  fix it here
N = N + 1
If Q = N Then
Cls 'START COUNT AND END COUNT NEED A SEPERATION OF MORE THAN 0
Lcd "GOT A DOUBLE" 'just for debugging  maybe a good way to check for a VALID sentence
Wait 1
End If
N = N  1
Q = Q  N
Q = Q  1
N = N + 1
'suck out the needed string componant
Commapos = Mid(gps1 , N , Q )
End Function 
second one below converts nmea lat lon to signed degree format
field 1 is the lat or lon to convert, field 2 is N,S,E or W
inputs are strings, output is a single.
Code: 
Function Convert(field1 As String , Field2 As String , ) As Single
Local Y As String * 7 , X As String * 3 , Z As Byte , W As Single
Z = 0 : X = "" : Y = ""
Z = Len(field1)
Y = Right(field1 , 7)
If Z = 9 Then X = Left(field1 , 2)
If Z = 10 Then X = Left(field1 , 3)
W = Val(x)
Convert = Val(y)
Convert = Convert / 60
Convert = Convert + W
'make it neg for south or west
If Field2 = "S" Then Convert = convert
If Field2 = "W" Then Convert = convert
Convert = Convert
End Function

Last edited by snipsnip on Sun Dec 28, 2014 11:18 am; edited 1 time in total 

Back to top 


snipsnip
Joined: 10 Feb 2014 Posts: 66 Location: Melbourne


Back to top 


snipsnip
Joined: 10 Feb 2014 Posts: 66 Location: Melbourne

Posted: Tue Dec 23, 2014 12:40 pm Post subject: 


found a site with some good gps formulas 
http://www.movabletype.co.uk/scripts/latlong.html
see below for a basic 3d distance calc based on formula from above site (flat earth), not mega accurate but should suitable for small distance, ie logging
only given it a quick test, but google earth agrees with it. (checked distance of 15 kilometres north heading)
I still need to prove it properly
Code: 
'####################################################################################################################
'3d distance calculation function
'altitude = in meters
'lat lon must be in decimal
'output in kilometers
Function 3ddistance (startlat , Startlon , Endlat , Endlon , Startalt , Endalt )as Single
Local R As Single , D As Single , X As Single , Y As Single
Local Tmp1 As Single , Tmp2 As Single , Tmp3 As Single , Height As Single
'first convert decimal lat lon to radians
'r=d*pi/180
Startlat = Startlat * 3.14158 : Startlat = Startlat / 180
Startlon = Startlon * 3.14158 : Startlon = Startlon / 180
Endlat = Endlat * 3.14158 : Endlat = Endlat / 180
Endlon = Endlon * 3.14158 : Endlon = Endlon / 180
'now lets butcher some maths
'2d distance  simple method ok for small distance
'ripped from this site  http://www.movabletype.co.uk/scripts/latlong.html
'x=(startlonendlon)*cos((startlat+endlat)/2)
'y=(endlatstartlat)
'distance=sqrt(x*x+y*y)*6371 6371=earths radius in k's
Tmp1 = 0 : Tmp2 = 0 : Tmp3 = 0
Tmp1 = Startlon  Endlon
Tmp2 = Startlat + Endlat
Tmp2 = Tmp2 / 2
Tmp3 = Cos(tmp2 )
X = Tmp1 * Tmp3
Y = Endlat  Startlat
Tmp1 = X * X
Tmp2 = Y * Y
Tmp3 = Tmp1 + Tmp2
Tmp3 = Sqr(tmp3 )
'd=2d distance in kilometers
D = Tmp3 * 6371
'now the 3d distance
'3d*3d=(d*d)+(height*height)
Height = Endalt  Startalt
'convert from meters to kilometers
Height = Height / 1000
Tmp1 = D * D
Height = Height * Height
Tmp2 = Tmp1 + Height
3ddistance = Sqr(tmp2 )
End Function

Last edited by snipsnip on Sun Dec 28, 2014 11:04 am; edited 2 times in total 

Back to top 


nicofer
Joined: 01 May 2013 Posts: 83 Location: GRJ

Posted: Wed Dec 24, 2014 5:58 pm Post subject: 


Hi
I am playing with gps info at the moment and have a question or 2  I repost it here .
I am busy with a gps runner watch ( mega128 with a GTOP 16x16 mm gps )  no fun in buying a working one Wink
I can get all the parts working , oled display , sd card for track log , and all the other bits.
I do have a problem with the gps data .
When standing still the lat and long values tend to drift a bit ,as expected , and I have a filter to detect if the delta distance between 2 readings exceed the limit.
This seems to help to prevent false distance readings due to small changes when not moving.
The bigger problem is that at times the distance reading jump quite high when the signal is low or for what ever reason and then the total distance jumps up quite fast .
I notice Kalman filters are used for GPS but I cant seem to find a good working sample .
I also notice the MCS gps distance sample make use of DOUBLE precision , and code from your program above make use of SINGLE precision . Which is better , good enough , for this application?
Any help to prevent this false readings would be highly appreciated.
Best wishes.
Nico 

Back to top 


snipsnip
Joined: 10 Feb 2014 Posts: 66 Location: Melbourne

Posted: Wed Dec 24, 2014 9:27 pm Post subject: 


Hi Nico,
I'm thinking about the same problem,
maybe using a accel/gyro to check for rubbish gps jumps?
or maybe use $GPGGA field6 "Horizontal Dilution of Precision (HDOP)"
keep a running average of this and check its current value against the average, then recheck the position data if HDOP is high?
should only need to run the function when not moving?
Would there be enough samples / time for a Kalman filter?
re single. Seems ok so far. I cant fault the distance measurement (yet). dead on over 15klms  according to google earth
I think using longs should be ok to?? not sure how the whole math/floating point thing works


Back to top 


nicofer
Joined: 01 May 2013 Posts: 83 Location: GRJ

Posted: Wed Dec 24, 2014 10:33 pm Post subject: 


Hi
I have looked at the HDOP and speed values and by adding them in some sort of filter will help.
I do however notice LARGE false readings even when the HDOP value is low.
The Gtop gps module is hooked up to an external mag mount antenna to get signal while the sensor is hooked to the programmer and serial port.
Normally one would be moving so the small error should not be a problem . I also have an upper limit for values based based on previous distance values. The idea is to reject any sudden
large distance jumps since it will be used in this case only for running but I might want to take it for a cycle ride later with larger changes in speed / distance so it have to be self adjusting.
The filter limit small changes but now there is another problem , what if you are moving very slow, so that the filter rejects the values , you still need to update your current position and distance.
I run with an android phone with a good performance gps sensor and the STRAVA app. But even the gps and subsequent distance values tend to drift a bit when not moving .
The TomTom Runner watch however does not increment while stationary and the final track is more smooth so they have a working filter algorithm .
The Runner has a accel/gyro sensor since it can be used "off road " so this might be the best to use this extra movement sensor since normally your arm is moving when running.
Cheers 

Back to top 


snipsnip
Joined: 10 Feb 2014 Posts: 66 Location: Melbourne

Posted: Wed Dec 24, 2014 11:58 pm Post subject: 


Quote:  The Runner has a accel/gyro sensor since it can be used "off road " so this might be the best to use this extra movement sensor since normally your arm is moving when running.

there would be a way to use a 6DOF (stand alone) mems to pretty accurately map distance and direction travelled. Reckon the maths and integration would be pretty full on
There will be some code somewhere on google for this  i'll see if I can track some down and convert it.
Last edited by snipsnip on Thu Dec 25, 2014 12:57 am; edited 1 time in total 

Back to top 


snipsnip
Joined: 10 Feb 2014 Posts: 66 Location: Melbourne

Posted: Thu Dec 25, 2014 12:06 am Post subject: 


Also,
what about taking a lat lon  offset this by say a kilometre N
take the (average of last 10 reading) offset this 100m south
final reading is the halfway point between the two 450m?
Just quick thoughts, could be very flawed


Back to top 


snipsnip
Joined: 10 Feb 2014 Posts: 66 Location: Melbourne

Posted: Thu Dec 25, 2014 12:30 am Post subject: 


just found the mcs gps distance calculator in the samples.
Code: 
'
' (c) 19952008
' MCS Electronics
' This program will calculate the distance between GPS coordinates
' It also calculates course
'
$regfile = "m128def.dat"
$framesize = 320
$hwstack = 160
$swstack = 160
$crystal = 16000000
'
Const Pi = 3.14159265358979
Const Mpi = Pi / 180
Const Radiusearth = 6371000
Dim Calc_distance As Double
Dim Calc_course_d As Double
Dim Calc_course_m As Double
Dim Calc_course_s As Double
Declare Sub Get_distance_course(byval Lat1 As Double , Byval Lon1 As Double , Byval Lat2 As Double , Byval Lon2 As Double)
'
Call Get_distance_course( 9.9666 , 18.0374 , 52.8273 , 8.4409)
If Calc_distance > 1000 Then Calc_distance = Calc_distance / 1000
Print "Distance: " ; Calc_distance
Print "Course: " ; Calc_course_d ; "D " ; Calc_course_m ; "M " ; Calc_course_s ; "S"
End
'
Sub Get_distance_course(byval Lat1 As Double , Byval Lon1 As Double , Byval Lat2 As Double , Byval Lon2 As Double)
Local Tmp As Double , A As Double , B As Double , C As Double , T2 As Double
' 1 Degree is 69.096 miles, 1 mile is 1609.34 m
A = Lat1 * Mpi 'Mpi
A = Cos(a)
T2 = Lat2 * Mpi : T2 = Cos(t2) : A = A * T2
T2 = Lon1 * Mpi : T2 = Cos(t2) : A = A * T2
T2 = Lon2 * Mpi : T2 = Cos(t2) : A = A * T2
B = Lat1 * Mpi : B = Cos(b)
T2 = Lon1 * Mpi : T2 = Sin(t2) : B = B * T2
T2 = Lat2 * Mpi : T2 = Cos(t2) : B = B * T2
T2 = Lon2 * Mpi : T2 = Sin(t2) : B = B * T2
C = Lat1 * Mpi : C = Sin(c)
T2 = Lat2 * Mpi : T2 = Sin(t2) : C = C * T2
T2 = A + B
T2 = T2 + C
A = Abs(t2)
If A >= 1 Then
Calc_distance = 0
Else
T2 = Acos(t2) : Tmp = T2 : T2 = T2 * Radiusearth
Calc_distance = T2
End If
'Calculate bearing course
A = Lat2 * Mpi : A = Sin(a)
B = Lat1 * Mpi : B = Sin(b)
T2 = Cos(tmp) : B = B * T2
C = Lat1 * Mpi : C = Cos(c)
T2 = Sin(tmp) : C = C * T2
A = A  B
A = A / C
A = Acos(a)
A = Rad2deg(a)
B = Lon2 * Mpi
C = Lon1 * Mpi
Tmp = B  C
If Tmp < 0.0 Then A = 360  A
Calc_course_d = Int(a)
Tmp = Frac(a)
Tmp = Tmp * 60
Calc_course_m = Int(tmp)
Tmp = Frac(tmp)
Tmp = Tmp * 60
Calc_course_s = Round(tmp)
End Sub 


Back to top 


snipsnip
Joined: 10 Feb 2014 Posts: 66 Location: Melbourne

Posted: Thu Dec 25, 2014 2:11 am Post subject: 


anchor / proximity alarm, not tested yet
Code: 
'anchor / proximity alarm returns "anchor" in meters from target when setdistance is exceded
'lat,lon in decimal meters,
'to_from.. 0=to 1=from ie 1=anchor and 0=proximity alarm conviguration
Function Anchor (anchlat , Anchlon , Curlat , Curlon , Setdistance , To_from ) As Single
Local Tmp As Single
Setdistance = Setdistance / 1000
'couldnt be stuffed rewiting the dist function here. if stack / portability is an issue rewrite it
Tmp = 3ddistance (anchlat , Anchlon , Curlat , Curlon , 1 , 1)
If To_from = 1 And Tmp > Setdistance Then
Tmp = tmp
Elseif To_from = 0 And Tmp < Setdistance Then
Tmp = tmp
End If
'convert to meters
Tmp = Tmp * 1000
Anchor = Tmp
End Function
End Function



Back to top 


snipsnip
Joined: 10 Feb 2014 Posts: 66 Location: Melbourne

Posted: Sun Dec 28, 2014 10:42 am Post subject: 


Another that returns the bearing from two points  Thanks to AdrianJ's code
needs lat / lon in decimal degrees
returns angle from north in degrees
Code: 
Function Bearing(lat1 , Lon1 , Lat2 , Lon2)as Single
'using arcsin formula rather than arccos
'calc dist and bearing in radians of pos2 from pos1
'positions in de degrees converted here to radians
'result = true bearing from north
'x1,y1 are long,lat of source, x2, y2 long, lat of dest
Const Cpi = 3.14159265
Const C2pi = 6.2831854
Const Crad2nm = 3437.746771 ' radians to nautical miles
Const Cnm2ft = 6080.0 ' nautical miles to feet
Local Bear As Single
Local X1 As Single
Local X2 As Single
Local Y1 As Single
Local Y2 As Single
Local Dx As Single
Local Dy As Single
Local Cosd As Single
Local Nu As Single
Local De As Single
Local Be As Single
Local Ad As Single
Local Dq As Single
'conver decimal lat,lons to radians
Lat1 = Lat1 * Cpi : Lat1 = Lat1 / 180
Lat2 = Lat2 * Cpi : Lat2 = Lat2 / 180
Lon1 = Lon1 * Cpi : Lon1 = Lon1 / 180
Lon2 = Lon2 * Cpi : Lon2 = Lon2 / 180
Dx = Lon2  Lon1 : Dx = Dx / 2 : Dx = Sin(dx) : Dx = Dx * Dx
Dy = Lat2  Lat1 : Dy = Dy / 2 : Dy = Sin(dy) : Dy = Dy * Dy
'the arc cos function fails for very small dx
'cosd = Sin(Y1) * Sin(Y2) + Cos(Y1) * Cos(Y2) * Cos(X2  X1)
'so use:
'd=2*asin(sqrt((sin((lat1lat2)/2))^2 + cos(lat1) * cos(lat2) *(sin((lon1 lon2) / 2)) ^ 2))
'be=atan((sin(lon1lon2)*cos(lat2))/(cos(lat1)*sin(lat2)sin(lat1)*cos(lat2)*cos(lon1lon2))
X1 = Sin(lat1) : Y1 = Cos(lat1)
X2 = Sin(lat2) : Y2 = Cos(lat2)
Dx = Dx * Y1 : Dx = Dx * Y2
De = Dy + Dx : De = Sqr(de)
Dq = Asin(de) : Dq = Dq * 2.0
'now calc bearing
Dx = Lon2  Lon1
Nu = sin(Dx)
Nu = nu * Y2
De = Cos(dx) : De = De * Y2 : De = De * X1 : X2 = X2 * Y1 : De = X2  De
Ad = 0.0
If Nu <> 0.0 Then
If De <> 0.0 Then
Be = Nu / De
Be = Atn(be) ' bearing in radians
If De < 0.0 Then
Ad = Cpi
End If
If Nu < 0.0 And De > 0.0 Then
Ad = C2pi
End If
Else
If Lon2 > Lon1 Then
Ad = Cpi / 2.0
Else
Ad = 1.5 * Cpi
End If
End If
Else
If Lat2 > Lat1 Then
Be = 0.0
Else
Be = Cpi
End If
End If
Be = Be + Ad
Bear = Be ' in radians
'to convert from radians to degrees, multiply by 180/pi.
Tmp1 = 180 / Cpi
Bear = Bear * Tmp1
Bearing = Bear
End Function 


Back to top 


nicofer
Joined: 01 May 2013 Posts: 83 Location: GRJ

Posted: Mon Dec 29, 2014 10:44 pm Post subject: 


I focused more on data logging to upload to STRAVA since they do filtering and smoothing and provide you with impressive graphical displays . Strava is not the only or the best for this but ,
since I have history with them ,I use them .
I now save the lon / lat / elev / time and date direct as a GPX format file that afterwards I use the upload page from Strava to upload the file , file syntax very critical.
The results are quite impressive  just as good as TomTom Runner  or Android Strava app.
So the bottom line is that for a few $$, a few scrap parts and some BASCOM code you can have a GPS runners watch that is on par with the best , for the logging part of it at least.
The embedded software , display , battery life and other bells and whistles is what sets the units apart.
I had some problems with my prototype Nokia 128x129 type lcd display  not good for use outside . So at the moment I have only logging  zero display.
The quick and dirty pcb has a 0.96 " OLED but still not sure if I should run it via I2c or SPI  I prefer SPI since it should be faster but I prefer to keep the display and sd card separated.
Any suggestions about this ?
Regards
Nico 

Back to top 


snipsnip
Joined: 10 Feb 2014 Posts: 66 Location: Melbourne

Posted: Thu Jan 01, 2015 1:15 am Post subject: 


Hi, Nico
Sorry mate, I've got no suggestions. I haven't yet got logging or a glcd setup yet. Was hoping you'd tell me the best way to go about it
But its pretty common to share card and screen on spi?
Got any pics of your watch setup that you want to share. Sounds pretty cool. 

Back to top 


nicofer
Joined: 01 May 2013 Posts: 83 Location: GRJ

Posted: Fri Jan 02, 2015 9:23 am Post subject: 


Hi
Did a 10 km run and the gps log of the track is 100 % with my temporary setup .
Biggest issue is the final external hardware and mounting since unit is visible and needs to be functional.
Busy with the proto pcb with the oled display  will post files soon.
To limit power drain I do not use the lcd display with led backlight  not readable outdoors anyway so it is only logging for now  plan is to have short vibration every km done and the display to
light up for a few seconds.
Circuit diagram not 100 % cast in stone.
Cheers 

Back to top 


snipsnip
Joined: 10 Feb 2014 Posts: 66 Location: Melbourne

Posted: Fri Feb 20, 2015 6:26 am Post subject: 


just another basic function.
converts Knots (string) to KPH(string) 2 decimal points with a trailing "Kph"
Code: 
Function Speed(knots As String)as String
Local Oput As String * 10 : Local X As Single : Local Y As Byte : Local Z As Byte
X = Val(knots)
X = X * 1.852
Oput = Str(x)
Y = Charpos(oput , ".")
Z = Y + 3 : Insertchar Oput , Z , " "
Z = Y + 4 : Insertchar Oput , Z , "K"
Z = Y + 5 : Insertchar Oput , Z , "p"
Z = Y + 6 : Insertchar Oput , Z , "h"
Oput = Left(oput , Z)
Speed = Oput
End Function 


Back to top 




You cannot post new topics in this forum You cannot reply to topics in this forum You cannot edit your posts in this forum You cannot delete your posts in this forum You cannot vote in polls in this forum You cannot attach files in this forum You cannot download files in this forum

