Precise GPS navigation

Python Help
#1

Good day all

I am new here and not a programmer at all.

I have asked the following on other forums with very little success. Most of the replies suggested that I use C++. I have no idea what C++ is all about and have very limited knowledge of Python but prefer Python as the little I do in Python makes sense. Live data streams however is out of my league

I have developed a low cost sub decimeter accurate navigation device using GNSS satellites.

A basic robotic vehicle has been build and consists of a Raspberry Pi 3b, a Sense Hat (hopefully temporary), L298NHbridge, 4 12 Volt DC motors and employs Skid Steering. Components are a problem in South Africa and the vehicle was built with what was available locally. Some components were sourced from Ebay and Aliexpress, the delivery times were from 2 weeks to 7 months.

The navigation device provides accurate Position Navigation and Time (PNT) data to the Raspberry via a USB port.

The data provided is Latitude, Longitude (Decimal format), PPS pulse, Track and altitude
The device is capable of providing ECEF X, Y and Z data as well, (long term it would be better to use ECEF according to the survey community) How to do so in Python is next on the list.

The idea is to write a Python program to do the following:

  1. Calculate the distance and bearing between the vehicle position and a user provided GPS target coordinate. (Or is could be a CSV file with a list of coordinates)
  2. Align the vehicle to the target coordinates
  3. Walk / drive the vehicle to the target coordinates
  4. Do minor course corrections
  5. Stop when the vehicle is less than 40mm from the target coordinates.

The calculation is straight forward

Python 3

Anton Strydom

2022/08/01

Part of Autonomous Robot GPS Navigation System

Routine to calculate Distance and Bearing Between 2 sets of Coordinates

import math

x1 = -26.1704925
y1 = 28.17704589
x2 = -26.1705061
y2 = 28.17719069

dist = math.sqrt((x2 - x1)**2 + (y2 - y1)**2) *100000
bearing = math.atan2( y2 - y1, x2 - x1 ) * ( 180 / math.pi )

if bearing < 0:
bearing = (bearing + 360)
elif bearing > 0:
bearing = bearing

print (‘Distance:’, dist)
print (‘Bearing:’, bearing)

Controlling the motors is also straight forward and I am using GPIO 6,13,19 and 26 to control the motors connected to IN1, IN2, IN3 and IN4 respectively and GPIO 18 and 12 connected to ENA and ENB respectively to try and use PWM for more accurate control of the motors.

Also the thinking is that in order to achieve sub decimeter accuracy, the device’s time synchronization with the GNSS satellites must be sub nano second taken into account that 1 nano second in time equals just on 300 mm. The device has a PPS port therefor the Raspberry time can be synchronized to the navigation device.

The reason is that using PWM to control the motors requires stable frequency in order to be accurately controlled.

Frequency equals time therefor the better the time pulse the better the frequency the better the control

PWM control has not been implemented as yet and is a definite for the future.

I have never worked with live data and the use of live data in calculations as well as applying the results to effect course directions autonomously.

My knowledge of python is restricted to the above.

What would be most simplistic way in Python be capturing live data, latitude, longitude and track values from the navigation device data stream and store it temporally. Acquire the target coordinates (either user entered or read from CSV type file) and calculate the distance and bearing from the present position of the vehicle (the vehicle position is determined by the GNSS antenna mounted in the front of the vehicle and aligned with the center of the vehicle) to the target.

Turn the vehicle to align the “track” value with the “bearing” value to ± 0.5 of a degree. Once aligned the vehicle must start moving towards the target and at the same time continuously calculate bearing and distance values at set intervals (the navigation device is capable of 20Hz update rates) using the target coordinates and the vehicle’s then present position applying course corrections through slowing and speeding up either port or starboard motors to change the course of the vehicle to align to the target.

Once the vehicle is within 40 mm’s from the target it must automatically stop. For now that would totally suffice for demonstration of the navigation device’s capabilities.

Any assistance or collaboration would be greatly appreciated

Thank you in advance

Sincerely

Anton

#2

Part of why you’re getting sparse response on this is that your question is a bit all over the place. You’re talking a lot about robotic steering, PWM control, and various components related to this, none of which seem relevant to the core of the question. If you try to keep your question to the bare minimum information necessary, you increase the likelihood of getting good answers.

As I understand your question, you want to communicate with a GPS device from a Raspberry Pi over a USB connection. To help you with that, we would need to know what protocol the GPS device uses.

You also mention that you need sub-nanosecond timing accuracy. Unfortunately you will not be able to achieve that with a pure Python solution. In fact, you won’t be able to do it with a Raspberry Pi at all. To get that kind of timing accuracy, you need a real time operating system. On a Raspberry Pi running some kind of Linux, the best you can do is around millisecond resolution.

So unfortunately you will need to delve into C/C++ to get the kind of performance you’re looking for.

1 Like
#3

Hi Alexander.

Thank you, your reply is appreciated

My apologies, yes I am all over the place I know.

I have a working navigation device that provides me sub-decimeter accuracy and sub nano second time synchronization. I am trying to use it in real time.

The robotics side of it has been taken care of.

It is the real time side that I require assistance with

Please feel free to ask any other relevant questions

Sincerely

Anton

#4

OK, so you already have some kind of device which outputs a position with sufficient accuracy? How does it communicate?

#5

Hi that was quick

It communicates either usb or i2c

#6

Using what protocol? Does it have a datasheet or a programmer’s reference manual?

#7

My apologies the device also has a PPS output port.
The information I have is that GPIO 4 is the PPS port on the Raspberry
The device also provides NMEA and TPV data
Anything else please feel free to ask

#8

I’m not familiar with GPS navigation so you will have to clarify what those terms mean and in what way they are communicated.

“PPS” I take it is some kind of clock synchronization.

“NMEA”: From a brief web search this seems to be a serial protocol. The pyserial package might be what you need.

“TPV” I can’t figure out. Third party verification?

#9

PPS is Pulse per second, your GPS device synchronizes time with the atomic clocks carried by the GPS satellites. The more precise the synchronization the better the position. It then provides a PPS pulse that is sub nano second accurate. It is what is used for stratum 1 ntp servers. This is normally achieved using survey grade dual frequency GPS devices and post processing the data. Look at this device as doing post processing on the fly.

NMEA consists of strings that provide time, position, heading and velocity information. This is enough information to navigate with. All the information required is contained in the various NMEA strings such as the GGA string for instance. However NMEA appears to be archaic and the tendency is the Time, Position and Velocity string created by GPSD.

The device is capable of providing either or does not matter, the TPV string looks easier to work with. gpsd_json(5)

#10

By protocol are you referring to the GPS standard information messages?

#11

By protocol I mean how the data sent by the device is meant to be interpreted by a receiving device, in your case a Raspberry Pi. From the link you provided, a TPV object might look like this:

{
	"class": "TPV",
	"device": "/dev/pts/1",
	"time": "2005-06-08T10:34:48.283Z",
	"ept": 0.005,
	"lat": 46.498293369,
	"lon": 7.567411672,
	"alt": 1343.127,
	"eph": 36.000,
	"epv": 32.321,
	"track": 10.3788,
	"speed": 0.091,
	"climb": -0.085,
	"mode": 3
}

Awesome, that’s JSON which is easy to work with. But how is the JSON object sent from the device to the Raspberry Pi? You mention that it has USB and I2C connections. Neither of those are capable of sending JSON directly, the JSON must first be encoded as bytes. What is the encoding? Is the entire JSON object encoded as ASCII, like this?

b'{\n\t"class": "TPV",\n\t"device": "/dev/pts/1",\n\t"time": "2005-06-08T10:34:48.283Z",\n\t"ept": 0.005,\n\t"lat": 46.498293369,\n\t"lon": 7.567411672,\n\t"alt": 1343.127,\n\t"eph": 36.000,\n\t"epv": 32.321,\n\t"track": 10.3788,\n\t"speed": 0.091,\n\t"climb": -0.085,\n\t"mode": 3\n}'

Or does it have some other way to encode the Time Position Velocity data?

#12

You certainly understand more about that than I do and you are absolutely correct.

The device provides binary (each gps receiver manufacturer have their own binary message structure.

A gps is a special radio with a processor that processes the satellite information to usable information that is provided by the likes of GPSD

In this case I am using a Ublox receiver that provides UBX binary that is “converted” to JASON by GPSD providing the TPV string.

Does this make sense?

#13

So this ublox thing is already connected to the rpi and you can already get the TPV string from gpsd_json?

#14

That is correct yes

It connects to ttyACM0

I can see the data stream but to use it in real time navigating a vehicle autonomously is above my knowledge of Python.

I know the procedure but to have it do it automatically with real time data I have no idea

#15

Still not entirely clear on how the communication works. Is the Ublox streaming data to /dev/ttyACM0 continously, or do you need to query it? Can you read directly from the ttyACM0, or does gpsd handle that and you read data from gpsd in some way?

Does your device come with any documentation? Please provide a link, it may make it easier to help you.

Edit: OK, I think I understand what you need to do. Have you installed gpsd? If not, do that. Then, from a command line, run this:

$ gpsd /dev/ttyACM0

Next, you need to install a gpsd client for Python:

$ pip3 install gpsdclient

Now, you should be able to read the streamed data from within Python:

>>> from gpsdclient import GPSDClient
>>> with GPSDClient(host="127.0.0.1") as client:
...    for result in client.json_stream():
...        print(result)

You should now see some GPS data being printed. Let me know if it works and we can go from there.

#16

The device streams continuously to ttyACM0 and I van read from it directly if I kill gpsd
Minicom or cat ttyACM0 allows me to see the data

GPSD is started on startup /etc/default.gpsd

# Default settings for the gpsd init script and the hotplug wrapper.

# Start the gpsd daemon automatically at boot time

START_DAEMON="true"

`# Use USB hotplugging to add new USB devices automatically to the daemon`
USBAUTO="true"

`# Devices gpsd should collect to at boot time.`
#` They need to be read/writeable, either by user gpsd or the group dialout.`
DEVICES="/dev/ttyACM0"

#` Other options you want to pass to gpsd`
GPSD_OPTIONS="-G"

gpsd client has also been installed 


pi@raspberrypi:~ $ python3
Python 3.7.3 (default, Jan 22 2021, 20:04:44)
[GCC 8.3.0] on linux
Type "help", "copyright", "credits" or "license" for more information.
>>> from gpsdclient import GPSDClient
>>> with GPSDClient(host="127.0.0.1") as client:
...    for result in client.json_stream():
...        print(result)
...
I did not get any data with the above 

When I run cgps I get the following, telling me the gps is working

lqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqklqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqk
x    Time:       2022-10-11T18:32:42.000Z   xxPRN:   Elev:  Azim:  SNR:  Used: x
x    Latitude:    26.17026966 S             xx   2    30    219    29      Y   x
x    Longitude:   28.17692933 E             xx   5    51    241    17      Y   x
x    Altitude:   5813.648 ft                xx   6    34    087    17      Y   x
x    Speed:      0.02 mph                   xx  11    57    126    25      Y   x
x    Heading:    0.0 deg (true)             xx  12    14    303    40      Y   x
x    Climb:      0.00 ft/min                xx  13    51    003    38      Y   x
x    Status:     3D FIX (20 secs)           xx  15    29    329    36      Y   x
x    Longitude Err:   +/- 25 ft             xx  19    19    026    38      Y   x
x    Latitude Err:    +/- 28 ft             xx  20    54    171    27      Y   x
x    Altitude Err:    +/- 135 ft            xx  29    21    221    32      Y   x
x    Course Err:      n/a                   xx   7    07    111    12      N   x
x    Speed Err:       n/a                   xx  25    08    269    00      N   x
x    Time offset:     0.082                 xx  30    14    078    00      N   x
x    Grid Square:     KG43ct                xx                                 x
mqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqjmqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqj
,{"PRN":20,"el":54,"az":171,"ss":21,"used":true},{"PRN":25,"el":8,"az":269,"ss":
13,"used":false},{"PRN":29,"el":21,"az":221,"ss":32,"used":true},{"PRN":30,"el":
14,"az":78,"ss":16,"used":true}]}
{"class":"TPV","device":"/dev/ttyACM0","mode":3,"time":"2022-10-11T18:32:32.000Z
","ept":0.005,"lat":-26.170286167,"lon":28.176935667,"alt":1774.400,"epx":7.903,
"epy":9.054,"epv":38.640,"track":0.0000,"speed":0.013,"climb":0.000}

Thank you your input is appreceated
#17

No output at all? What if you run it from command line:

python3 -m gpsdclient

Do you know at what port number gpsd is running? The default is 2947, and it doesn’t look like it’s been changed from the config script you provided, but best double check.

#18

Good day, we had a power outage, called load shedding here

This is the result running python3 -m gpsdclient

pi@raspberrypi:~ $ python3 -m gpsdclient

Connected to gpsd v3.17

Devices: /dev/ttyACM0

timed out

The port is 2947, gpsmon starts up fine

pi@raspberrypi:~ $ gpsmon
tcp://localhost:2947 JSON slave driver>
(82) {“class”:“VERSION”,“release”:“3.17”,“rev”:“3.17”,“proto_major”:3,“proto_min
or”:12}
(202) {“class”:“DEVICES”,“devices”:[{“class”:“DEVICE”,“path”:“/dev/ttyACM0”,“dri
ver”:“NMEA0183”,“activated”:“2022-10-12T05:57:42.071Z”,“flags”:5,“native”:0,"bps
":9600,“parity”:“N”,“stopbits”:1,“cycle”:1.00}]}
(122) {“class”:“WATCH”,“enable”:true,“json”:false,“nmea”:false,“raw”:2,“scaled”:
false,“timing”:false,“split24”:false,“pps”:true}

The above is what is received when gpsmon is started up
Interesting in the last line I see “timing”:false it should be true as far as I can determine

Thank you for your assistance it is appreciated

#19

Hmm. I guess gpsdclient might just be broken then. I have no experience with it.

Try using the gps package instead, installed as part of gpsd-clients (sudo apt install gpsd-clients on Debian-based systems). The gpsd website provides an example script for use with this package:

#! /usr/bin/env python3
"""
example  Python gpsd client
run this way: python3 example1.py.txt
"""

import gps               # the gpsd interface module

session = gps.gps(mode=gps.WATCH_ENABLE)

try:
    while 0 == session.read():
        if not (gps.MODE_SET & session.valid):
            # not useful, probably not a TPV message
            continue

        print('Mode: %s(%d) Time: ' %
              (("Invalid", "NO_FIX", "2D", "3D")[session.fix.mode],
               session.fix.mode), end="")
        # print time, if we have it
        if gps.TIME_SET & session.valid:
            print(session.fix.time, end="")
        else:
            print('n/a', end="")

        if ((gps.isfinite(session.fix.latitude) and
             gps.isfinite(session.fix.longitude))):
            print(" Lat %.6f Lon %.6f" %
                  (session.fix.latitude, session.fix.longitude))
        else:
            print(" Lat n/a Lon n/a")

except KeyboardInterrupt:
    # got a ^C.  Say bye, bye
    print('')

# Got ^C, or fell out of the loop.  Cleanup, and leave.
session.close()
exit(0)

Save this script as ‘gpstest.py’ and try running it with python3 gpstest.py.

#20

Hi the example code runs perfectly, the lack of a time stamp appears to be due to the time set to false

pi@raspberrypi:~ $ python3 example1.py
Mode: 3D(3) Time: n/a Lat -26.170198 Lon 28.177173
Mode: 3D(3) Time: n/a Lat -26.170198 Lon 28.177173
Mode: 3D(3) Time: n/a Lat -26.170221 Lon 28.177147