GPS Destination Heading Calculating Software
SRLM
Posts: 5,045
I thought I'd post this in case anybody else needed something like it. I was working on a robot (BS2px based) that navigates from one GPS coordinate to another autonomously. However, I've found that the two BS2s that I had on there weren't really powerful enough for what I wanted, so I've transitioned to the Propeller. So, as this section of code is very generic, I thought I'd post it in case anybody else had a need for something like it.
The Datasheet:
This code determines if the current GPS coordinates match the destination coordinates, and if they don't it calculates a heading relative to the robot that it needs to travel in to get to the destination.
Hardware Requirements:
GPS
Compass
BS2e or Higher
The GPS coordinates (both destination and current) and compass heading are stored in the SCRAM of the BS2, and this program takes a single slot (due to the large amount of variables).
I have included DEBUG and DEBUGIN statements (commented out) that allow you to run the first part of the program by itself for testing purposes. I have tested it with a set of data, but it has not been field tested. I designed this to be used in north America, if you are elsewhere, then you'll probably have to change some direction variables.
The first half of the program tests and calculates on the coordinates. Due to the limitations of the BS2 processor (namely, a maximum variable value of ~64,000), I had to break the heading calculation into two parts.
--First, it tests to see if the coordinates match in the Degree category
If not, then it calculates a heading based on a triangle using the current coordinate degrees and the destination coordinate degrees
--Next (if it is at the destination degrees), it checks to see if it is at the destination minutes and decimal minutes.
·
If not, then it calculates a heading based on a triangle using the current coordinate degrees & decimal degrees and the destination coordinate degrees & decimal degrees.
Once it has calculated the heading the GPS says it should take, it goes into the _CalculateDesiredHeding function, which compares it to the heading measured by the compass and creates a relative heading for the robot. This relative heading ranges from 1 to 7,
·1
4
7
Left---Center ---Right
This relative heading can be sent to the servo or to an obstacle detection and avoidance algorithm.
The Datasheet:
This code determines if the current GPS coordinates match the destination coordinates, and if they don't it calculates a heading relative to the robot that it needs to travel in to get to the destination.
Hardware Requirements:
GPS
Compass
BS2e or Higher
The GPS coordinates (both destination and current) and compass heading are stored in the SCRAM of the BS2, and this program takes a single slot (due to the large amount of variables).
I have included DEBUG and DEBUGIN statements (commented out) that allow you to run the first part of the program by itself for testing purposes. I have tested it with a set of data, but it has not been field tested. I designed this to be used in north America, if you are elsewhere, then you'll probably have to change some direction variables.
The first half of the program tests and calculates on the coordinates. Due to the limitations of the BS2 processor (namely, a maximum variable value of ~64,000), I had to break the heading calculation into two parts.
--First, it tests to see if the coordinates match in the Degree category
If not, then it calculates a heading based on a triangle using the current coordinate degrees and the destination coordinate degrees
--Next (if it is at the destination degrees), it checks to see if it is at the destination minutes and decimal minutes.
·
If not, then it calculates a heading based on a triangle using the current coordinate degrees & decimal degrees and the destination coordinate degrees & decimal degrees.
Once it has calculated the heading the GPS says it should take, it goes into the _CalculateDesiredHeding function, which compares it to the heading measured by the compass and creates a relative heading for the robot. This relative heading ranges from 1 to 7,
·1
4
7
Left---Center ---Right
This relative heading can be sent to the servo or to an obstacle detection and avoidance algorithm.
