Commit f976dc71 authored by Benoit ZERR's avatar Benoit ZERR
Browse files

compute distances on gpx track

parent e91bf78e
DDBOAT python3 drivers version 2
# DDBOAT python3 drivers version 2
The drivers are :
* IMU (MAG, ACCEL, GYRO) : imu9_driver_v2.py
* GPS (serial line, GPGLL message) : gps_driver_v2.py
* Encoders on propeller rotation (serial line) : encoders_driver_v2.py
* Arduino motors command (serial line) : arduino_driver_v2.py
a small test program can be launched to test all the drivers :
## testing the drivers on your ddboat
First, you will have to clone this repo on your laptop
```
$ git clone https://gitlab.ensta-bretagne.fr/zerrbe/drivers-ddboat-v2.git
```
Then you will have to upload the drivers on your ddboat
We assume we are on ddboat 7 and the folder of your team is mywrk (replace with your own values, mywrk should be a different name for all teams to avoid interferences)
```
$ scp -r drivers-ddboat-v2 ue32@172.20.25.207:mywrk
```
Now we open another terminal on the ddboat:
```
$ ssh ue32@172.20.25.207
$ cd mywrk
```
Now, the drivers are all tested using the tst_ddboat_v2.py python script :
```
$ python3 tst_ddboat_v2.py
```
## using the screen command to work outside of the WIFI Network
When using WIFI, the terminal is locked when the WIFI connection is lost.
The ddboat wil stop working properly.
A solution is to use the **screen** command to detach the session from the terminal.
First, we start a screen session called sesddboat (or whatsoever name)
```
$ screen -S sesddboat
```
Note that screen creates a new terminal (and you loose the command history for recent commands)
We can now check that the session is attached
```
$ screen -ls
```
should give :
```
There is a screen on:
1404.sesddboat (09/29/21 16:42:04) (Attached)
1 Socket in /run/screen/S-pi.
```
The screen commands start with Ctrl+A. You can see a list of command here : https://guides.github.com/features/mastering-markdown/
Now, we can detach the screen session from the terminal by typing Ctrl+A d
and we can check that the session is actually detached :
```
$ screen -ls
```
should give :
There is a screen on:
1404.sesddboat (09/29/21 16:42:04) (Detached)
1 Socket in /run/screen/S-pi.
```
Now we can work outside the WIFI coverage. To get access back (resume access) to the session when are back in WIFI area, type :
```
screen -r
```
we are back on line with the ddboat !
A last usefull command is to stop screen, one way to do it is :
```
$ screen -X -S sesddboat quit
```
where sesddboat is the session name the has been used at the start.
......@@ -33,20 +33,27 @@ class GpsIO:
def read_gll(self,n_try_max=20):
val=[0.,'N',0.,'W',0.]
for i in range(n_try_max):
v=self.ser.readline().decode("utf-8")
if str(v[0:6]) == "$GPGLL":
vv = v.split(",")
if len(vv[1]) > 0:
val[0] = float(vv[1])
if len(vv[2]) > 0:
val[1] = vv[2]
if len(vv[3]) > 0:
val[2] = float(vv[3])
if len(vv[4]) > 0:
val[3] = vv[4]
if len(vv[5]) > 0:
val[4] = float(vv[5])
break # GPGLL found ! exit !
rdok = True
try:
v=self.ser.readline().decode("utf-8")
except:
print ("error reading GPS !!")
rdok = False
break # go out
if rdok:
if str(v[0:6]) == "$GPGLL":
vv = v.split(",")
if len(vv[1]) > 0:
val[0] = float(vv[1])
if len(vv[2]) > 0:
val[1] = vv[2]
if len(vv[3]) > 0:
val[2] = float(vv[3])
if len(vv[4]) > 0:
val[3] = vv[4]
if len(vv[5]) > 0:
val[4] = float(vv[5])
break # GPGLL found ! exit !
return val
if __name__ == "__main__":
......
<?xml version="1.0" encoding="UTF-8"?>
<gpx xsi:schemaLocation="http://www.topografix.com/GPX/1/0 http://www.topografix.com/GPX/1/0/gpx.xsd" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.topografix.com/GPX/1/0" version="1.0" creator="gpx.py -- https://github.com/tkrajina/gpxpy">
<trk>
<trkseg>
<trkpt lat="48.4188" lon="-4.4725"></trkpt>
<trkpt lat="48.4185" lon="-4.4739"></trkpt></trkseg></trk></gpx>
This diff is collapsed.
import time
import sys
import gps_driver_v2 as gpsdrv
import gpxpy.gpx
# requires install of gpxpy
# sudo apt install python3-gpxpy
def cvt_gll_ddmm_2_dd (st):
sts =st.split(" ")
stlat = sts[0]
stlon = sts[2]
stlats = stlat.split(".")
stlons = stlon.split(".")
lat = double(stlats[0][0:len(stlats[0])-2])
lon = double(stlons[0][0:len(stlons[0])-2])
print (lat,lon)
ilat = st[0]
ilon = st[2]
olat = float(int(ilat/100))
olon = float(int(ilon/100))
olat_mm = (ilat%100)/60
olon_mm = (ilon%100)/60
olat += olat_mm
olon += olon_mm
if st[3] == "W":
olon = -olon
return olat,olon
gps = gpsdrv.GpsIO()
# debug with USB GPS
gps.init_line_devname_baudrate("/dev/ttyUSB0",9600)
#gps.init_line_devname_baudrate("/dev/ttyUSB0",9600)
tmax = 25.0
# open gpx buffer
gpx = gpxpy.gpx.GPX()
# Create first track in our GPX:
gpx_track = gpxpy.gpx.GPXTrack()
gpx.tracks.append(gpx_track)
# Create first segment in our GPX track:
gpx_segment = gpxpy.gpx.GPXTrackSegment()
gpx_track.segments.append(gpx_segment)
tmax = 10*60.0 # 10 minutes max
t0 = time.time()
while True:
print ("---------------------------------------------------")
......@@ -24,5 +40,17 @@ while True:
# test GPS
gps_data_string = gps.read_gll()
print ("GPS:",gps_data_string)
if gps_data_string[0] == 0.0:
break
lat,lon = cvt_gll_ddmm_2_dd (gps_data_string)
print (lat,lon)
gpx_segment.points.append(gpxpy.gpx.GPXTrackPoint(lat, lon))
if (time.time()-t0) > tmax:
break
fp = open("tst.gpx","w")
fp.write(gpx.to_xml())
fp.write("\n")
fp.close()
#
# requires : sudo apt install python3-pyproj
# requires : gpxpy and pyproj
# sudo apt install python3-pyproj
# sudo apt install python3-gpxpy
# test project lat,lon (deg) to meters
import gpxpy
import gpxpy.gpx
from pyproj import Proj, transform
inProj = Proj("+init=EPSG:2263",preserve_units=True)
outProj = Proj("+init=EPSG:4326") # WGS84 in degrees and not EPSG:3857 in meters)
x2,y2 = -74.2700000001129, 40.46999999990434
print (x2,y2)
x1,y1 = transform(outProj,inProj,x2,y2)
print (x1,y1)
print(transform(inProj,outProj,x1,y1))
import numpy as np
projMeter = Proj("+init=EPSG:2263",preserve_units=True)
projDegre = Proj("+init=EPSG:4326") # WGS84 in degrees and not EPSG:3857 in meters)
gpx = gpxpy.gpx.GPX()
# Create first track in our GPX:
gpx_track = gpxpy.gpx.GPXTrack()
gpx.tracks.append(gpx_track)
# Create first segment in our GPX track:
gpx_segment = gpxpy.gpx.GPXTrackSegment()
gpx_track.segments.append(gpx_segment)
# reference point "the flag"
reference_lat = 48.4188
reference_lon = -4.4725
gpx_segment.points.append(gpxpy.gpx.GPXTrackPoint(reference_lat, reference_lon))
reference_lat = 48.4185
reference_lon = -4.4739
gpx_segment.points.append(gpxpy.gpx.GPXTrackPoint(reference_lat, reference_lon))
reference_x,reference_y = transform(projDegre,projMeter,reference_lat,reference_lon)
fp = open("reference.gpx","w")
fp.write(gpx.to_xml())
fp.write("\n")
fp.close()
gpx_file = open('tst_ensta_stadium_20210929.gpx', 'r')
gpx = gpxpy.parse(gpx_file)
for track in gpx.tracks:
for segment in track.segments:
for point in segment.points:
#print('Point at ({0},{1}) -> {2}'.format(point.latitude, point.longitude, point.elevation))
x,y = transform(projDegre,projMeter,point.latitude, point.longitude)
lat,lon = transform(projMeter,projDegre,x,y)
#print (x,y,lat,lon)
dx = x - reference_x
dy = y - reference_y
distance = np.sqrt(dx*dx+dy*dy)
print (dx,dy,distance)
# test project lat,lon (deg) to meters
#from pyproj import Proj, transform
#inProj = Proj("+init=EPSG:2263",preserve_units=True)
#outProj = Proj("+init=EPSG:4326") # WGS84 in degrees and not EPSG:3857 in meters)
#x2,y2 = -74.2700000001129, 40.46999999990434
#print (x2,y2)
#x1,y1 = transform(outProj,inProj,x2,y2)
#print (x1,y1)
#print(transform(inProj,outProj,x1,y1))
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment