Main Site Documentation

FEZ Mini, LS20031 GPS, Wrong Coordinates (for RC Car)


#1

I am using the “66 Channel LS20031 GPS 10Hz Receiver”.
I’ve downloaded the driver from TinyWiki.
GPS is connected to COM2 with the following connection:
GPS -> FEZ
Pin1(VDD) -> 3v3
Pin2(Rx) -> TTL Tx
Pin3(Tx) -> TTL Rx
GND -> GND
GND -> GND

The GPS is working, the led on the gps component is flashing when he has enough satellites.

This is the coordinates i am receiving (in string).
Lon: 00525.1767
Lat: 5137.5706

Not the place where i am living. First, it’s 50 km off… Second, the dots are wrong placed, or is it me? Thought it should be something like this:

Lat: 5.251765
Lon: 51.375703

This is the driver code i am using now. Can someone look at this place

//
// Import Namespaces
//
using System;
using System.IO.Ports;
using System.Threading;
using Microsoft.SPOT;
//
// FEZ NETMF Namespace
//
namespace GHIElectronics.NETMF.FEZ
{
    //
    // FEZ_Components Partial Class
    //
    public static partial class FEZ_Components
    {
        //
        // Helper class to work with GPS
        //
        public class GPS
        {
            //
            // Default Variables
            //
            char[] split_char = new char[] { ',' };
            string[] sentence_fields;
            SerialPort _port;
            byte[] buffer = new byte[200];
            bool _data_is_valid;
            private static GPSStateMachine gps_decoder_state;
            char[] pree_buffer = new char[10];
            char[] pree = { 'G', 'P', 'R', 'M', 'C' };
            int pree_index = 0;
            char[] sentence = new char[200];
            char[] GPRMC_sentence_array = new char[200];
            int GPRMC_sentence_array_length = 0;
            int sentence_index = 0;
            string sentence_string = "";
            //
            // Location Variables
            //
            string _date;
            string _time;
            public string _longitude;
            public string _latitude;
            string _valid;
            string _longitudeDir;
            string _latitudeDir;
            string _speed;
            string _direction;
            //
            // GPSStateMachine
            //
            private enum GPSStateMachine
            {
                FindPre,
                CopyingSentence,
            }
            //
            // Sub New
            //
            public GPS(string PortName)
            {
                Initialize(PortName);
            }
            //
            // Initialize
            //
            public void Initialize(string portName)
            {
                _port = new SerialPort(portName, 57600, Parity.None, 8, StopBits.One);
                _port.ReadTimeout = 0;
                _port.ErrorReceived += new SerialErrorReceivedEventHandler(_port_ErrorReceived);
                _port.Open();
                _port.DataReceived += new SerialDataReceivedEventHandler(_port_DataReceived);
            }
            //
            // DataReceived
            //
            void _port_DataReceived(object sender, SerialDataReceivedEventArgs e)
            {
                int buffer_index = 0;
                int data_received = _port.Read(buffer, 0, buffer.Length);
                while (buffer_index < data_received)
                {
                    switch (gps_decoder_state)
                    {
                        case GPSStateMachine.FindPre:
                            if (buffer[buffer_index] == pree[pree_index])
                            {
                                pree_index++;
                                if (pree_index >= pree.Length)
                                {
                                    gps_decoder_state = GPSStateMachine.CopyingSentence;
                                    pree_index = 0;

                                }
                            }
                            else
                                pree_index = 0;
                            break;
                        case GPSStateMachine.CopyingSentence:
                            sentence[sentence_index] = (char)buffer[buffer_index];
                            if (sentence[sentence_index] == '*' || sentence[sentence_index] == '\r' || sentence[sentence_index] == '\n')
                            {
                                if (sentence_index > 10)
                                {
                                    if (sentence[12] == 'A')
                                    {
                                        lock (GPRMC_sentence_array)
                                        {
                                            Array.Copy(sentence, GPRMC_sentence_array, sentence_index);
                                            GPRMC_sentence_array_length = sentence_index;
                                            _data_is_valid = true;
                                        }
                                    }
                                    else
                                    {
                                        _data_is_valid = false;
                                    }
                                }
                                sentence_index = 0;
                                gps_decoder_state = GPSStateMachine.FindPre;
                            }
                            else
                            {
                                sentence_index++;
                                if (sentence_index >= sentence.Length)
                                {
                                    sentence_index = 0;
                                    gps_decoder_state = GPSStateMachine.FindPre;
                                }
                            }
                            break;
                    }
                    buffer_index++;
                }
            }
            //
            // ErrorReceived
            //
            void _port_ErrorReceived(object sender, SerialErrorReceivedEventArgs e)
            {
            }
            //
            // GetLongitude
            //
            public string GetLongitude()
            {
                return _longitude;
            }
            //
            // GetLatitude
            //
            public string GetLatitude()
            {
                return _latitude;
            }
            //
            // GetDirection
            //
            public string GetDirection()
            {
                return _direction;
            }
            //
            // GetSpeed
            //
            public string GetSpeed()
            {
                return _speed;
            }
            //
            // IsValid
            //
            public bool IsValid()
            {
                if (_valid == "A")
                {
                    return (true);
                }
                else
                {
                    return (false);
                }
            }
            //
            // ParseSentence
            //
            private void ParseSentence()
            {
                if (sentence_fields.Length == 13)
                {
                    _date = sentence_fields[9];
                    _time = sentence_fields[1];
                    _longitude = sentence_fields[5];
                    _latitude = sentence_fields[3];
                    _valid = sentence_fields[2];
                    _longitudeDir = sentence_fields[6];
                    _latitudeDir = sentence_fields[4];
                    _speed = sentence_fields[7];
                    _direction = sentence_fields[8];
                }
                else
                {
                }
            }
            //
            // GetPosition
            //
            public bool GetPosition()
            {
                if (_data_is_valid == true)
                {
                    lock (GPRMC_sentence_array)
                    {
                        sentence_string = new string(GPRMC_sentence_array, 0, GPRMC_sentence_array_length);
                        sentence_fields = sentence_string.Split(split_char);
                        ParseSentence();
                        _data_is_valid = false;
                        return true;
                    }
                }
                else
                {
                    return false;
                }
            }
        }
    }
}

#2

step back and show us what serial output the GPS is generating. This is pretty clearly an interpretation issue.


#3

Actually it’s pretty easy…

Lon is HHHMM.MMMM and Lat is HHMM.MMMM

For lon, the first 3 numbers are the hours (or degrees), the remaining is the minutes. Same with Lat except it’s just the first two numbers for hours, and the remaining is minutes. This is the standard NMEA data string.

So the coordinates you are actually getting is

5’ 25.251765" / 51’ 37.5706"


#4

Brett, will do, back to basics :wink:

Ron2, i think i also need SS (or SSSS?) for more accuracy, but i think when i go back to basics, reading the output string, i will get more info.

Will check this tonight.


#5

I will post my code for this on here in a second - Might take me a while to find it.

I use the same gps module, with the GPGLL sentence


#6

Xarren, thanks! Looking forward to your next post!


#7
using System;
using System.IO.Ports;
using System.Text;
using System.Threading;

using GHIElectronics.NETMF.System;

using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;

namespace DaveKoz
{
    public class GpsApi
    {
        private SerialPort gpsUart;

        public double[] pos;
        public bool fix;

        public GpsApi()
        {
            gpsUart = new SerialPort("COM1", 57600);
            fix = false;
            pos = new Double[2];
        }

        public Double[] GetPosData()
        { 
            return pos;
        }

        public void UpdateData()
        {
            byte[] buffer = new byte[100];
            string[] data;

            gpsUart.Open();

            data = ReadLine(gpsUart);

            if (GetFixStatus(data[5], data[6]))
            {
                pos[1] = GetDecimalDeg(data[0], 2) * GetSign(data[1]);
                pos[0] = GetDecimalDeg(data[2], 3) * GetSign(data[3]);
                fix = true;
            }
            else
                fix = false;

            gpsUart.Close();
        }

        private string[] ReadLine(SerialPort port)
        {
            byte[] rxBytes = new byte[1];
            byte[] stringTypeBytes = new byte[5];

            string stringType;

            while (true)
            {
                rxBytes[0] = 0;
                int curPos = 0;

                byte[] buffer = new byte[82];

                while (rxBytes[0] != 36) // $ Sign
                    port.Read(rxBytes, 0, 1);

                port.Read(stringTypeBytes, 0, 5);

                stringType = new string(Encoding.UTF8.GetChars(stringTypeBytes));

                if (stringType != "GPGLL")
                    continue;

                while (curPos < 82)
                {
                    int read = port.Read(buffer, curPos, 1);
                    if (read == 0) break;
                    if (buffer[curPos] == 42) break;
                    curPos++;
                }

                string data = new string(Encoding.UTF8.GetChars(buffer));
                return data.Substring(1).TrimEnd('*').Split(',');
            }
        }

        private double GetDecimalDeg(string degrees, int degLength)
        {
            return double.Parse(
                degrees.Substring(0, degLength)) +
                    (double.Parse(degrees.Substring(2)) / 60);
        }

        private int GetSign(string sign)
        {
            if (sign == "W" || sign == "S")
                return -1;
            else
                return 1;
        }

        private bool GetFixStatus(string fixStatus, string fixType)
        {
            if (fixStatus == "A")
                if (fixType == "A" || fixType == "D")
                    return true;
                else
                    return false;
            else
                return false;
        }


    }
}

Feel free to copy and paste straight into your solution. It will work right out of the box, so you can play around with it until you get the hang of it, and then deface it to your hearts contents :wink:

Quick summary: It is defaulted to COM1 - You can easily put that in the constructor and make it adjustable. Baud rate is set 57000. Have a timer or a thread running “Update Data” method every x amount of miliseconds. Method “Get Pos Data” will return a double array [lat,long] - It will be in digital hour format, so ready to use in any mathematics. The variable “fix” which is a bool will be set to true when a fix is available, and back to false when there is no fix, as long as the main loop is running.

Edit: PS resolution is 1.1m lat and 1.1cos(lat) long. Accuracy is 2m rms. So if you’re getting anything more than a couple meters off you’ve messed something up


#8

Xarren, do you have any information (in your code) about heading? (need this too)…

And, how can i “convert” this coordinates:
Lon: 00525.1767
Lat: 5137.5706

To correct double coordinates like this one (in Amsterdam ;-));

Lon: 52.374761
lat: 4.905396

It is not easy as:

Lon: 52.51767
Lat: 51.375706

Correct?

Thanks for replying.

Ps, Xarren, will test your code, didn’t got any time yet…

[edit] Never mind the conversion to decimal, got it: Degrees + (Minutes/60)


#9

Indeed, thats what the following function is for

        private double GetDecimalDeg(string degrees, int degLength)
        {
            return double.Parse(
                degrees.Substring(0, degLength)) +
                    (double.Parse(degrees.Substring(2)) / 60);
        }

And no, I’m using an IMU for heading.


#10

I managed to get the coordinates in Decimal.
Just checked the code with gps signal.

Copy/paste the coordinates to google maps, and, BOOM my house :smiley:


#11

Nice one, and btw to save you some time - 1 decimal degree long = 1 decimal degree lat x cos(long), when it comes to comparing distances.


#12

Hi Xarren,

Got some functions converted from vb.net to c#.net to calculate distance and bearing.

Could you check these for me?

        //
        // Calculate radian from degrees
        //
        private static double ToRadian(double deg)
        {
            return (deg * System.MathEx.PI / 180);
        }
        //
        // Calculate degrees from radians
        //
        private static double ToDegree(double rad)
        {
            return (rad * 180 / System.MathEx.PI);
        }
        //
        // Calculate Bearing
        //
        public static double CalculateBearing(double latitude1, double longitude1, double latitude2, double longitude2)
        {
            //
            // Set Latitude and Longitude
            //
            latitude1 = ToRadian(latitude1);
            latitude2 = ToRadian(latitude2);
            //
            // Calculate Difference
            //
            double longitudeDifference = ToRadian(longitude2 - longitude1);
            //
            // Returns the Sine and the Cosine of the specified angle
            //
            double y = System.MathEx.Sin(longitudeDifference) * System.MathEx.Cos(latitude2);
            double x = System.MathEx.Cos(latitude1) * System.MathEx.Sin(latitude2) - System.MathEx.Sin(latitude1) * System.MathEx.Cos(latitude2) * System.MathEx.Cos(longitudeDifference);
            //
            // Return Bearing
            //
            return (ToDegree(System.MathEx.Atan2(y, x)) + 360) % 360;
        }
        //
        // Calculate Distance
        //
        public static double CalculateDistance(double latitude1, double longitude1, double latitude2, double longitude2)
        {
            //
            // Calculate Difference
            //
            double difference = longitude1 - longitude2;
            difference = ToRadian(difference);
            //
            // Set Latitude and Longitude
            //
            latitude1 = ToRadian(latitude1);
            latitude2 = ToRadian(latitude2);
            //
            // Returns the Sine and the Cosine of the specified angle
            //
            double distance = System.MathEx.Sin(latitude1) * System.MathEx.Sin(latitude2) + System.MathEx.Cos(latitude1) * System.MathEx.Cos(latitude2) * System.MathEx.Cos(difference);
            distance = System.MathEx.Acos(distance);
            distance = ToDegree(distance);
            distance = distance * 60 * 1.1515;
            distance = distance * 1.609344;
            distance = distance * 1000;
            //
            // Return Distance
            //
            return distance;
        }

#13

Looks good to me,

If I’m honest, you don’t need the sine rules etc for your distance and bearing… It only matters over large distances. For my non-commercial version I’m just using atan and a^2 + b^2 = c^2. But if you’ve done it, and understand it/not got any problems with it, then thats all good.

One thing I’m not sure you’ve addressed. Atan returns the acute angle. Have you compensated for that? E.g. Atan of 270 will equal to atan of 90, etc.

The absolute best way to check that this works is just to run it manually with a couple of values, thats what I did with mine to make sure it all worked.


#14

The functions are working fine for me :wink: Thanks for checking.

I know, I have a seperate c#.net windows application project on my computer where I check several functions first, before implementing in the Fez project. Works like a charm 8)

Tomorrow evening, more test, more response.


#15

I’m going to be building an autonomous ground vehicle myself soon - A desert raster scanning meteorite finder :).


#16

Awesome. I am going the create a wiki page for my robot asap. So, you can check my hardware, software etc…


#17

After some more tests, here are the results:

  • Heading, is correct, the gps gives me the correct degrees of the heading i am walking.
  • Current location, it’s about 40 meters off. tested 3 times on different locations (about 2km away from eachother).
    After waiting, for about 1 minute, for valid signal, i check the coordinates with google maps, but it’s about 40 meters off the right place. Do i need to wait longer for accurate position, or is it something else? Do i need a code change for better accuracy?

Please advice.


#18

I have done some more tests, at work, open space :wink:
The coordinates are correct now. I think i didn’t got good quality signals from the satellites yesterday.

But, i’ve found out that the heading isn’t updating fast enough. I am checking the gps signal (coordinates) in a while loop every second now. The coordinates are correct, but when i turn around and walk the other way, the heading is sloooooow with updating.
For example. I am heading north, 5 degrees, and when i turn around and walking south, you should say the heading is 180 degrees, but i am receiving the following (not exactly, but for exampel) heading degrees from the gps (comma seperated):
5,24,41,65,80,96,101,120,135,140,155,162,etc.
How comes that the heading isn’t updating correctly?

Anyone?


#19

The GPS probably uses an internal filter that has 12-16 “taps”, so until the filter fills up with the new direction that you are walking, it will take some time for the filter to clear out and fill up with valid data.

You can do the heading calculations yourself using the last two positions, but because the GPS signal can bounce around quite a bit you will have to do the same thing the GPS is doing internally. The only way around it is to increase the frequency of position/heading updates so the filter turns over faster.


#20

Ron, thanks, will check it out. I allready have a function to calculate bearing between 2 coordinates. So…
Will check it out tomorrow or so.

Thanks again.