Code:
/*
* ****************************************************************************
* RP6 ROBOT SYSTEM - RP6 CONTROL M32 Examples
* ****************************************************************************
* Example: NL-552ETTL
* Author(s): Dirk
* ****************************************************************************
* Description:
* Now we will use the UART to read data from the NL-552ETTL GPS module
* (ELV No. 94241) with the u-blox5 UBX-G5000-BT GPS chipset.
*
* The NL-552ETTL TX pin (4) has to be connected to RP6 CONTROL pin RXD
* (PROG_UART: 2). The internal pullup resistor of RXD should be
* deactivated!
*
* ****************************************************************************
* NMEA 0183 short description:
* 1. GPRMC:
* $GPRMC,213938.000,A,5108.3356,N,00951.0774,E,0.00,79.64,061012,,,A*53
* Index: 1 2 3 4 5 6 7 8 9 12CS
* 1 = UTC of position [hhmmss.sss]
* 2 = Data status (A=ok, V=void)
* 3 = Latitude of fix [ddmm.mmmm]
* 4 = N or S
* 5 = Longitude of fix [dddmm.mmmm]
* 6 = E or W
* 7 = Speed over ground in knots
* 8 = Track made good, degrees true
* 9 = UT date [ddmmyy]
* 10 = Magnetic variation degrees (Easterly var. subtracts from true course)
* 11 = E or W
* 12 = Mode indicator (NMEA V2.3):
* A=Autonomous mode
* D=Differential mode
* E=Estimated (dead-reckoning) mode
* M=Manual input mode
* S=Simulated mode
* N=Data not valid
* CS = Checksum
*
* 2. GPVTG:
* $GPVTG,79.64,T,,M,0.00,N,0.0,K,A*31
* Index: 1 2 4 5 6 7 8 9 CS
* 1 = Track made good
* 2 = Fixed text 'T' indicates that track made good is relative to true north
* 3 = Track degrees
* 4 = M=Magnetic
* 5 = Speed over ground in knots
* 6 = Fixed text 'N' indicates that speed over ground is in knots
* 7 = Speed over ground in kilometers/hour
* 8 = Fixed text 'K' indicates that speed over ground is in kilometers/hour
* 9 = Mode indicator (A,D,E,M,S,N)
* CS = Checksum
*
* 3. GPGGA:
* $GPGGA,213938.000,5108.3356,N,00951.0774,E,1,04,5.3,158.5,M,46.2,M,,0000*52
* Index: 1 2 3 4 5 6 7 8 9 11 12 14 CS
* 1 = UTC of position
* 2 = Latitude of fix
* 3 = N or S
* 4 = Longitude of fix
* 5 = E or W
* 6 = GPS quality indicator:
* 0=invalid
* 1=GPS fix (SPS)
* 2=DGPS fix
* 3=PPS fix
* 4=Real Time Kinematic
* 5=Float RTK
* 6=estimated (dead reckoning) (NMEA V2.3)
* 7=Manual input mode
* 8=Simulation mode
* 7 = Number of satellites in use [not those in view]
* 8 = Horizontal dilution of position (HDOP)
* 9 = Antenna altitude above/below mean sea level (geoid)
* 10 = Meters (Antenna height unit)
* 11 = Geoidal separation (Diff. between WGS-84 earth ellipsoid and
* mean sea level. -=geoid is below WGS-84 ellipsoid)
* 12 = Meters (Units of geoidal separation)
* 13 = Age in seconds since last update from diff. reference station
* 14 = Differential reference station ID#
* CS = Checksum
*
* 4. GPGSA:
* $GPGSA,A,3,02,04,10,07,,,,,,,,,7.4,5.3,5.1*33
* Index: 1 2 3 4 5 6 15 16 17 CS
* 1 = Selection mode:
* M=Manual, forced to operate in 2D or 3D
* A=Automatic 2D/3D
* 2 = Mode:
* 1=no fix
* 2=2D
* 3=3D
* 3-14 = IDs of SVs used in position fix (null for unused fields)
* 15 = PDOP
* 16 = HDOP
* 17 = VDOP
* CS = Checksum
*
* 5. GPGSV:
* $GPGSV,3,1,12,02,36,301,37,04,53,235,21,05,03,294,,07,35,170,29*79
* $GPGSV,3,2,12,08,04,187,28,10,56,290,36,13,88,106,20,16,12,076,*7D
* $GPGSV,3,3,12,20,23,118,11,23,50,067,,29,03,351,12,30,16,048,22*70
* Index: 1 2 3 4 5 6 7 8 9 10 12 13 14 15 16 17 18 19 CS
* 1 = Total number of messages of this type in this cycle
* 2 = Message number
* 3 = Total number of SVs in view
* 4 = SV PRN number
* 5 = Elevation in degrees, 90 maximum
* 6 = Azimuth, degrees from true north, 000 to 359
* 7 = SNR, 00-99 dB (null when not tracking)
* 8-11 = Information about second SV, same as field 4-7
* 12-15= Information about third SV, same as field 4-7
* 16-19= Information about fourth SV, same as field 4-7
* CS = Checksum
*
* 6. GPGLL:
* $GPGLL,5108.3356,N,00951.0774,E,213938.000,A,A*5C
* Index: 1 2 3 4 5 6 7 CS
* 1 = Latitude of fix
* 2 = N or S
* 3 = Longitude of fix
* 4 = E or W
* 5 = UTC of position
* 6 = Data status (A=ok, V=void)
* 7 = Mode indicator (A,D,E,M,S,N)
* CS = Checksum
*
* COMMENTS:
* a) Each sentence (line) begins with '$'.
* b) Each sentence (line) ends with '*', followed by the checksum (two
* chars building an 8-bit HEX number) and terminated by <CR><LF>.
* c) The checksum (CS) is calculated by XOR of all chars between '$' and '*'.
* d) The max. sentence (line) length is 82 chars (including <CR><LF>).
* e) Further infos:
* -> http://www.nmea.org/
* -> http://www.kowoma.de/gps/zusatzerklaerungen/NMEA.htm
* -> http://de.wikipedia.org/wiki/NMEA_0183
* -> http://aprs.gids.nl/nmea/
*
* ############################################################################
* The Robot does NOT move in this example! You can simply put it on a table
* next to your PC and you should connect it to the PC via the USB Interface!
* ############################################################################
* ****************************************************************************
*/
/*****************************************************************************/
// Includes:
#include "RP6ControlLib.h"
#include <stdint.h>
// Always needs to be included!
/*****************************************************************************/
// NL-552ETTL functions:
char parseBuffer[256];
uint16_t parseBuffer_length = 0;
uint8_t parseBuffer_pos = 0;
/**
* Clears the parseBuffer
*/
void clearParseBuffer(void)
{
parseBuffer_pos = 0;
parseBuffer_length = 0;
}
/**
* Writes received byte data into buffer parseBuffer and returns if a full
* NMEA sentence has been received. Then the buffer is terminated with 0.
* Returns 0 if a sentence has been detected and buffer is ready.
* Returns 1 if no sentence has been received yet
* Returns 2 if buffer overflow occured
*/
uint8_t parseNMEAsentence(char *sentence, uint8_t data)
{
static uint8_t startflag = false;
static char last_data[10];
uint16_t length = 0;
uint8_t i = 0;
if(data == 0)
return 1;
while(sentence[i++])
length++;
if(length > 10)
length = 10;
for(i = 0; i < 9; i++)
last_data[i] = last_data[i + 1];
last_data[9] = data;
parseBuffer[parseBuffer_pos] = data;
parseBuffer_pos++;
parseBuffer_length++;
if(parseBuffer_pos > 254)
{
clearParseBuffer();
startflag = false;
return 2;
}
if(!startflag && (strstr(last_data, sentence) != NULL))
{ // Chosen sentence start
for(i = 0; i < length; i++)
parseBuffer[i] = sentence[i];
parseBuffer_pos = length;
parseBuffer_length = length;
startflag = true;
}
if(startflag)
{
if(data == '\r')
{ // Chosen sentence end
parseBuffer[parseBuffer_pos] = 0;
last_data[9] = 0;
startflag = false;
return 0;
}
}
return 1;
}
/**
* Outputs true, if the NMEA sentence in parseBuffer is intact,
* else returns false.
*/
uint8_t checkNMEAsentence(void)
{
uint8_t i, cs;
char cs_str[3];
if(parseBuffer_length < 18) return false;
if(parseBuffer_length > 82) return false;
if(parseBuffer[0] != '$') return false;
if(parseBuffer[parseBuffer_length - 4] != '*') return false;
cs = parseBuffer[1];
for(i = 2; i < (parseBuffer_length - 4); i++)
{
cs ^= parseBuffer[i]; // Calculate checksum
}
itoa(cs, &cs_str[0], HEX);
strupr(&cs_str[0]); // cs as uppercase chars
if((cs_str[0] != parseBuffer[parseBuffer_length - 3])
|| (cs_str[1] != parseBuffer[parseBuffer_length - 2])) return false;
return true;
}
/**
* Outputs a result string containing the data with index of the NMEA
* sentence in parseBuffer.
*/
void parseNMEAitem(char *result, uint8_t index)
{
uint8_t index_cnt, buffer_idx, result_idx;
index_cnt = index;
buffer_idx = 6;
while(index_cnt)
{
if(parseBuffer[buffer_idx++] == ',')
index_cnt--;
}
result_idx = 0;
while((parseBuffer[buffer_idx] != ',')
&& (parseBuffer[buffer_idx] != '*'))
{
result[result_idx++] = parseBuffer[buffer_idx++];
}
result[result_idx] = 0;
}
typedef struct
{
int32_t latitude;
int32_t longitude;
} geographic_t;
/* mapping of location difference ratios to the range [0; 45[ degrees: */
static const int16_t ATAN[45] = {
8, 26, 43, 61, 78, 96, 113, 131, 149, 167,
185, 203, 221, 240, 258, 277, 296, 315, 334, 354,
373, 393, 414, 434, 455, 476, 498, 520, 542, 565,
589, 612, 637, 661, 687, 713, 739, 767, 795, 824,
854, 884, 916, 948, 982 };
/* the destination coordinates times 1000 */
static const geographic_t DESTINATION = {52011, 11686}; //schönebeck start
/* convert location difference ratio to an angle.
*
* ratio -- ratio of location differences
*
* returns angle in degrees
*/
static int16_t ratio2angle(int32_t ratio)
{
uint8_t i;
for(i=0; i<45; i++)
{
if(ratio < ATAN[i])
return i;
}
return 45;
}
/* get the direction to the destination
*
* current -- current location
*
* returns course angle in degrees
* 0 <-> N
* 90 <-> W
* 180 <-> S
* 270 <-> E
* -1 <-> destination reached
*/
static int16_t get_direction(geographic_t * current)
{
int32_t d_latitude, d_longitude, ratio;
/* compute the coordinate differences: */
d_latitude = DESTINATION.latitude - current->latitude;
d_longitude = DESTINATION.longitude - current->longitude;
/* compute the course depending on the 8th we are in: */
if(d_longitude < 0) // 2nd or 3rd quadrant
{
d_longitude = -d_longitude;
if(d_latitude < 0) // 3rd quadrant
{
d_latitude = -d_latitude;
if(d_longitude > d_latitude) // upper 8th
{
ratio = (d_latitude*1000)/d_longitude;
return 90 + ratio2angle(ratio);
}
else // lower 8th
{
ratio = (d_longitude*1000)/d_latitude;
return 180 - ratio2angle(ratio);
}
}
else // 2nd quadrant
{
if(d_longitude > d_latitude) // lower 8th
{
ratio = (d_latitude*1000)/d_longitude;
return 90 - ratio2angle(ratio);
}
else // upper 8th
{
ratio = (d_longitude*1000)/d_latitude;
return ratio2angle(ratio);
}
}
}
else // 1st or 4th quadrant
{
if(d_latitude < 0) // 4th_quadrant
{
d_latitude = -d_latitude;
if(d_longitude > d_latitude) // upper 8th
{
ratio = (d_latitude*1000)/d_longitude;
return 270 - ratio2angle(ratio);
}
else // lower 8th
{
ratio = (d_longitude*1000)/d_latitude;
return 180 + ratio2angle(ratio);
}
}
else // 1st quadrant
{
if(d_longitude > d_latitude) // lower 8th
{
ratio = (d_latitude*1000)/d_longitude;
return 270+ratio2angle(ratio);
}
else if(d_latitude) // upper 8th
{
ratio = (d_longitude*1000)/d_latitude;
return 360 - ratio2angle(ratio);
}
else
{
// we are at the destination:
return -1;
}
}
}
}
/* convert latitude coordinate string to degrees
*
* latitude -- string with format ddmm.mmmm
*
* return degrees * 1000 as integer
*/
static int32_t latitude2int(const char * latitude)
{
int32_t degrees, minutes;
degrees = (int32_t)(latitude[0] - '0') * 10000;
degrees += (latitude[1] - '0') * 1000;
minutes = (int32_t)(latitude[2] - '0') * 10000;
minutes += (latitude[3] - '0') * 1000;
minutes += (latitude[5] - '0') * 100;
minutes += (latitude[6] - '0') * 10;
minutes += (latitude[7] - '0');
degrees += minutes / 60;
return degrees;
}
/* convert longitude coordinate string to degrees
*
* latitude -- string with format dddmm.mmmm
*
* return degrees * 1000 as integer
*/
static int32_t longitude2int(const char * longitude)
{
int32_t degrees, minutes;
degrees = (int32_t)(longitude[0] - '0') * 100000;
degrees += (int32_t)(longitude[1] - '0') * 10000;
degrees += (longitude[2] - '0') * 1000;
minutes = (int32_t)(longitude[3] - '0') * 10000;
minutes += (int32_t)(longitude[4] - '0') * 1000;
minutes += (longitude[6] - '0') * 100;
minutes += (longitude[7] - '0') * 10;
minutes += (longitude[8] - '0');
degrees += minutes / 60;
return degrees;
}
/* convert string coordinates to geographic coordinates
*
* latitude -- string with format dddmm.mmmm
* longitude -- string with format ddmm.mmmm
* coordinate -- pointer to geographic type for output
*/
static void convert_coordinates(const char * latitude, const char * longitude, geographic_t * coordinate)
{
coordinate->latitude = latitude2int(latitude);
coordinate->longitude = longitude2int(longitude);
}
/**
* Write a floating point number to the LCD.
*
* Example:
*
* // Write a floating point number to the LCD (no exponent):
* writeDoubleLCD(1234567.890, 11, 3);
*
* The value of prec (precision) defines the number of decimal places.
* For 32 bit floating point variables (float, double ...) 6 is
* the max. value for prec (7 relevant digits).
* The value of width defines the overall number of characters in the
* floating point number including the decimal point. The number of
* pre-decimal positions is: (width - prec - 1).
*/
void writeDoubleLCD(double number, uint8_t width, uint8_t prec)
{char buffer[width + 1];
dtostrf(number, width, prec, &buffer[0]);
writeStringLCD(&buffer[0]);
}
/*****************************************************************************/
// Main function - The program starts here:
int main(void)
{
/* init controller and LC-Display: */
initRP6Control();
initLCD();
/* blink LEDs and show startup message: */
setLEDs(0b1111);
mSleep(500);
setLEDs(0b0000);
// Write a text message to the LCD:
showScreenLCD("######GPS#######", "################");
mSleep(1500);
clearLCD();
while(true) // main program loop
{
uint8_t ch;
/* clear buffers */
clearParseBuffer();
clearReceptionBuffer();
/* receive data from GPS module: */
do
{
ch = readChar();
} while(parseNMEAsentence("$GPRMC", ch));
if(checkNMEAsentence()) // data ok?
{
char lat[12];
char lon[12];
char item[12];
int16_t course_angle;
geographic_t current={0};
/* extract latitude and longitude: */
strcpy(lat, "0211.1111");
strcpy(lon, "11135.5000");
/* convert to geographic type and compute course angle: */
convert_coordinates(lat, lon, ¤t);
course_angle = get_direction(¤t);
/* correct 360° course */
if(course_angle == 360)
course_angle = 0;
/* prepare coordinates for display: */
if(lat[0] == '0') // leading zero?
{
lat[0] = lat[1];
lat[1] = '°';
lat[4] = '\'';
lat[5] = 0;
}
else
{
lat[4] = lat[3];
lat[3] = lat[2];
lat[2] = '°';
lat[5] = '\'';
lat[6] = 0;
}
if(lon[0] == '0') // leading zero?
{
if(lon[1] == '0') // two leading zeros?
{
lon[0] = lon[2];
lon[1] = '°';
lon[2] = lon[3];
lon[3] = lon[4];
lon[4] = '\'';
lon[5] = 0;
}
else
{
lon[0] = lon[1];
lon[1] = lon[2];
lon[2] = '°';
lon[5] = '\'';
lon[6] = 0;
}
}
else
{
lon[4] = lon[3];
lon[3] = lon[2];
lon[2] = '°';
lon[5] = '\'';
lon[6] = 0;
}
/* show coordinates: */
clearLCD();
setCursorPosLCD(0, 0); // line 1
parseNMEAitem(lat, 3);
writeStringLCD("Lat:"); writeStringLCD(lat);
setCursorPosLCD(1, 0); // line 2
parseNMEAitem(lon, 5);
writeStringLCD("Lon:"); writeStringLCD(lon);
mSleep(1000);
clearLCD();
setCursorPosLCD(0, 0); // line 1
parseNMEAitem(item, 7);
writeStringLCD("Speed:"); writeStringLCD(item); writeStringLCD(" KN");
/* avoid float calculations: */
item[2] = item[3];
item[3] = 0;
int32_t a = atoi(item);
a = a * 1852;
a = a / 10000;
setCursorPosLCD(1, 0); // line 1
writeStringLCD("Speed:"); writeIntegerLCD(a, DEC); writeStringLCD(" KM/H");
mSleep(1000);
clearLCD();
setCursorPosLCD(0, 0); // line 1
if(course_angle < 0)
{
writeStringLCD("Ziel erreicht!");
mSleep(2000);
clearLCD();
}
else
{
writeStringLCD("Ziel:"); writeIntegerLCD(course_angle, DEC);
mSleep(2000);
clearLCD();
}
}
}
return 0;
}
Zitieren
Lesezeichen