mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-02 05:17:03 +08:00
remove unused GPS files and add module conf file for arduimu basic
version
This commit is contained in:
Gautier Hattenberger
@@ -2,14 +2,13 @@
|
|||||||
|
|
||||||
<module name="ins_ArduIMU" dir="ins">
|
<module name="ins_ArduIMU" dir="ins">
|
||||||
<header>
|
<header>
|
||||||
<file name="ins_arduimu_basic.h"/>
|
<file name="ins_arduimu.h"/>
|
||||||
</header>
|
</header>
|
||||||
<init fun="ArduIMU_init()"/>
|
<init fun="ArduIMU_init()"/>
|
||||||
<periodic fun="ArduIMU_periodic()" freq="60"/> <!-- 15 ist soll -->
|
<periodic fun="ArduIMU_periodic()" freq="15" autorun="TRUE"/> <!-- 15 ist soll -->
|
||||||
<periodic fun="ArduIMU_periodicGPS()" freq="8"/> <!-- 8 ist soll -->
|
<periodic fun="ArduIMU_periodicGPS()" freq="8" autorun="TRUE"/> <!-- 8 ist soll -->
|
||||||
<event fun="ArduIMU_event()"/>
|
|
||||||
<makefile target="ap">
|
<makefile target="ap">
|
||||||
<file name="ins_arduimu_basic.c"/>
|
<file name="ins_arduimu.c"/>
|
||||||
</makefile>
|
</makefile>
|
||||||
<makefile target="sim">
|
<makefile target="sim">
|
||||||
<file_arch name="ins_arduimu.c"/>
|
<file_arch name="ins_arduimu.c"/>
|
||||||
|
|||||||
@@ -0,0 +1,18 @@
|
|||||||
|
<!DOCTYPE module SYSTEM "module.dtd">
|
||||||
|
|
||||||
|
<module name="ins_ArduIMU" dir="ins">
|
||||||
|
<header>
|
||||||
|
<file name="ins_arduimu_basic.h"/>
|
||||||
|
</header>
|
||||||
|
<init fun="ArduIMU_init()"/>
|
||||||
|
<periodic fun="ArduIMU_periodic()" freq="60"/>
|
||||||
|
<periodic fun="ArduIMU_periodicGPS()" freq="8"/>
|
||||||
|
<event fun="ArduIMU_event()"/>
|
||||||
|
<makefile target="ap">
|
||||||
|
<file name="ins_arduimu_basic.c"/>
|
||||||
|
</makefile>
|
||||||
|
<makefile target="sim">
|
||||||
|
<file_arch name="ins_arduimu.c"/>
|
||||||
|
</makefile>
|
||||||
|
</module>
|
||||||
|
|
||||||
@@ -1,204 +0,0 @@
|
|||||||
#if GPS_PROTOCOL == 2
|
|
||||||
|
|
||||||
#define BUF_LEN 100
|
|
||||||
|
|
||||||
// The input buffer
|
|
||||||
char gps_buffer[BUF_LEN]={
|
|
||||||
|
|
||||||
// Setup for SIRF binary at 38400 - $PSRF100,0,38400,8,1,0*3C<cr><lf>
|
|
||||||
0x24,0x50,0x53,0x52,0x46,0x31,0x30,0x30,0x2C,0x30,0x2C,0x33,0x38,0x34,0x30,0x30,0x2C,0x38,0x2C,0x31,0x2C,0x30,0x2A,0x33,0x43,0x0D,0x0A};
|
|
||||||
|
|
||||||
// Used to configure Sirf GPS
|
|
||||||
const byte gps_ender[]={0xB0,0xB3};
|
|
||||||
|
|
||||||
/****************************************************************
|
|
||||||
Parsing stuff for SIRF binary protocol.
|
|
||||||
****************************************************************/
|
|
||||||
void init_gps(void)
|
|
||||||
{
|
|
||||||
pinMode(2,OUTPUT); //Serial Mux
|
|
||||||
digitalWrite(2,HIGH); //Serial Mux
|
|
||||||
change_to_sirf_protocol();
|
|
||||||
delay(100);//Waits fot the GPS to start_UP
|
|
||||||
configure_gps();//Function to configure GPS, to output only the desired msg's
|
|
||||||
}
|
|
||||||
|
|
||||||
void decode_gps(void)
|
|
||||||
{
|
|
||||||
static unsigned long GPS_timer = 0;
|
|
||||||
static byte gps_counter = 0; //Another gps counter for the buffer
|
|
||||||
static byte GPS_step = 0;
|
|
||||||
boolean gps_failure = false;
|
|
||||||
static byte gps_ok = 0;//Counter to verify the reciving info
|
|
||||||
const byte read_gps_header[]={0xA0,0xA2,0x00,0x5B,0x29};//Used to verify the payload msg header
|
|
||||||
|
|
||||||
if(Serial.available() > 0)//Ok, let me see, the buffer is empty?
|
|
||||||
{
|
|
||||||
while(Serial.available() < 5){ }
|
|
||||||
switch(GPS_step) {
|
|
||||||
case 0: //This case will verify the header, to know when the payload will begin
|
|
||||||
while(Serial.available() > 0) //Loop if data available
|
|
||||||
{
|
|
||||||
if(Serial.read() == read_gps_header[gps_ok]){ //Checking if the head bytes are equal..
|
|
||||||
//if yes increment 1
|
|
||||||
gps_ok++;
|
|
||||||
}else{
|
|
||||||
//Otherwise restart.
|
|
||||||
gps_ok = 0;
|
|
||||||
}
|
|
||||||
if(gps_ok >= 5) {
|
|
||||||
//Ohh 5 bytes are correct, that means jump to the next step, and break the loop
|
|
||||||
gps_ok = 0;
|
|
||||||
GPS_step++;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
case 1: //Will receive all the payload and join the received bytes...
|
|
||||||
while(Serial.available() < 92){
|
|
||||||
}
|
|
||||||
gps_counter = 0;
|
|
||||||
memset(gps_buffer,0,sizeof(gps_buffer));
|
|
||||||
|
|
||||||
while(Serial.available() > 0){
|
|
||||||
//Read data and store it in the temp buffer
|
|
||||||
byte b1 = Serial.read();
|
|
||||||
byte b2 = gps_buffer[gps_counter-1];
|
|
||||||
// gps_ender[]={0xB0,0xB3};
|
|
||||||
|
|
||||||
if((b1 == gps_ender[1]) && (b2 == gps_ender[0])){
|
|
||||||
GPS_step = 0;
|
|
||||||
gps_counter = 0;
|
|
||||||
gpsFix = gps_buffer[1];
|
|
||||||
|
|
||||||
if(gpsFix == 0x00){
|
|
||||||
// GPS signal is error free
|
|
||||||
// ------------------------
|
|
||||||
digitalWrite(6,HIGH);
|
|
||||||
GPS_timer = millis();
|
|
||||||
gpsFixnew=1; //new information available flag for binary message
|
|
||||||
|
|
||||||
//Parse the data
|
|
||||||
GPS_join_data();
|
|
||||||
|
|
||||||
} else {
|
|
||||||
// GPS has returned an error code
|
|
||||||
// ------------------------------
|
|
||||||
gpsFix = 0x01; // In GPS language a good fix = 0, bad = 1
|
|
||||||
digitalWrite(6,LOW);
|
|
||||||
}
|
|
||||||
|
|
||||||
break;
|
|
||||||
}else{
|
|
||||||
gps_buffer[gps_counter] = b1;
|
|
||||||
gps_counter++;
|
|
||||||
|
|
||||||
if (gps_counter >= BUF_LEN){
|
|
||||||
Serial.flush();
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if(millis() - GPS_timer > 2000){
|
|
||||||
digitalWrite(6, LOW); //If we don't receive any byte in two seconds turn off gps fix LED...
|
|
||||||
gpsFix = 0x01;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void GPS_join_data(void)
|
|
||||||
{
|
|
||||||
// Read bytes and combine them with Unions
|
|
||||||
// ---------------------------------------
|
|
||||||
byte j = 22;
|
|
||||||
longUnion.byte[3] = gps_buffer[j++];
|
|
||||||
longUnion.byte[2] = gps_buffer[j++];
|
|
||||||
longUnion.byte[1] = gps_buffer[j++];
|
|
||||||
longUnion.byte[0] = gps_buffer[j++];
|
|
||||||
lat = longUnion.dword; // latitude * 10,000,000
|
|
||||||
|
|
||||||
|
|
||||||
longUnion.byte[3] = gps_buffer[j++];
|
|
||||||
longUnion.byte[2] = gps_buffer[j++];
|
|
||||||
longUnion.byte[1] = gps_buffer[j++];
|
|
||||||
longUnion.byte[0] = gps_buffer[j++];
|
|
||||||
lon = longUnion.dword; // longitude * 10,000,000
|
|
||||||
|
|
||||||
j = 34;
|
|
||||||
longUnion.byte[3] = gps_buffer[j++];
|
|
||||||
longUnion.byte[2] = gps_buffer[j++];
|
|
||||||
longUnion.byte[1] = gps_buffer[j++];
|
|
||||||
longUnion.byte[0] = gps_buffer[j++];
|
|
||||||
alt_MSL = longUnion.dword * 10; // alt in meters * 1000
|
|
||||||
|
|
||||||
j = 39;
|
|
||||||
intUnion.byte[1] = gps_buffer[j++];
|
|
||||||
intUnion.byte[0] = gps_buffer[j++];
|
|
||||||
speed_3d = (float)intUnion.word / 100.0; // meters/second We only get ground speed but store it as speed_3d for use in DCM
|
|
||||||
|
|
||||||
//iTOW
|
|
||||||
|
|
||||||
intUnion.byte[1] = gps_buffer[j++];
|
|
||||||
intUnion.byte[0] = gps_buffer[j++];
|
|
||||||
ground_course = (float)intUnion.word / 100.0; // degrees
|
|
||||||
ground_course = abs(ground_course); //The GPS has a BUG sometimes give you the correct value but negative, weird!!
|
|
||||||
|
|
||||||
// clear buffer
|
|
||||||
// -------------
|
|
||||||
memset(gps_buffer,0,sizeof(gps_buffer));
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void configure_gps(void)
|
|
||||||
{
|
|
||||||
const byte gps_header[]={
|
|
||||||
0xA0,0xA2,0x00,0x08,0xA6,0x00 };//Used to configure Sirf GPS
|
|
||||||
const byte gps_payload[]={
|
|
||||||
0x02,0x04,0x07,0x09,0x1B };//Used to configure Sirf GPS
|
|
||||||
const byte gps_checksum[]={
|
|
||||||
0xA8,0xAA,0xAD,0xAF,0xC1 };//Used to configure Sirf GPS
|
|
||||||
const byte cero = 0x00;//Used to configure Sirf GPS
|
|
||||||
|
|
||||||
for(int z=0; z<2; z++)
|
|
||||||
{
|
|
||||||
for(int x=0; x<5; x++)//Print all messages to setup GPS
|
|
||||||
{
|
|
||||||
for(int y=0; y<6; y++)
|
|
||||||
{
|
|
||||||
Serial.print(byte(gps_header[y]));//Prints the msg header, is the same header for all msg..
|
|
||||||
}
|
|
||||||
Serial.print(byte(gps_payload[x]));//Prints the payload, is not the same for every msg
|
|
||||||
for(int y=0; y<6; y++)
|
|
||||||
{
|
|
||||||
Serial.print(byte(cero)); //Prints 6 zeros
|
|
||||||
}
|
|
||||||
Serial.print(byte(gps_checksum[x])); //Print the Checksum
|
|
||||||
Serial.print(byte(gps_ender[0])); //Print the Ender of the string, is same on all msg's.
|
|
||||||
Serial.print(byte(gps_ender[1])); //ender
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void change_to_sirf_protocol(void)
|
|
||||||
{
|
|
||||||
Serial.begin(4800); //First try in 4800
|
|
||||||
delay(300);
|
|
||||||
for (byte x=0; x<=28; x++)
|
|
||||||
{
|
|
||||||
Serial.print(byte(gps_buffer[x]));//Sending special bytes declared at the beginning
|
|
||||||
}
|
|
||||||
delay(300);
|
|
||||||
Serial.begin(9600); //Then try in 9600
|
|
||||||
delay(300);
|
|
||||||
for (byte x=0; x<=28; x++)
|
|
||||||
{
|
|
||||||
Serial.print(byte(gps_buffer[x]));
|
|
||||||
}
|
|
||||||
Serial.begin(38400); //Universal Synchronous Asynchronous Recieving Transmiting
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif
|
|
||||||
|
|
||||||
@@ -1,292 +0,0 @@
|
|||||||
#if GPS_PROTOCOL == 1
|
|
||||||
|
|
||||||
//*********************************************************************************************
|
|
||||||
// You may need to insert parameters appropriate for your gps from this list into init_gps()
|
|
||||||
//GPS SIRF configuration strings...
|
|
||||||
#define SIRF_BAUD_RATE_4800 "$PSRF100,1,4800,8,1,0*0E\r\n"
|
|
||||||
#define SIRF_BAUD_RATE_9600 "$PSRF100,1,9600,8,1,0*0D\r\n"
|
|
||||||
#define SIRF_BAUD_RATE_19200 "$PSRF100,1,19200,8,1,0*38\r\n"
|
|
||||||
#define SIRF_BAUD_RATE_38400 "$PSRF100,1,38400,8,1,0*3D\r\n"
|
|
||||||
#define SIRF_BAUD_RATE_57600 "$PSRF100,1,57600,8,1,0*36\r\n"
|
|
||||||
#define GSA_ON "$PSRF103,2,0,1,1*27\r\n" // enable GSA
|
|
||||||
#define GSA_OFF "$PSRF103,2,0,0,1*26\r\n" // disable GSA
|
|
||||||
#define GSV_ON "$PSRF103,3,0,1,1*26\r\n" // enable GSV
|
|
||||||
#define GSV_OFF "$PSRF103,3,0,0,1*27\r\n" // disable GSV
|
|
||||||
#define USE_WAAS 1 //1 = Enable, 0 = Disable, good in USA, slower FIX...
|
|
||||||
#define WAAS_ON "$PSRF151,1*3F\r\n" // enable WAAS
|
|
||||||
#define WAAS_OFF "$PSRF151,0*3E\r\n" // disable WAAS
|
|
||||||
|
|
||||||
//GPS Locosys configuration strings...
|
|
||||||
#define USE_SBAS 0
|
|
||||||
#define SBAS_ON "$PMTK313,1*2E\r\n"
|
|
||||||
#define SBAS_OFF "$PMTK313,0*2F\r\n"
|
|
||||||
|
|
||||||
#define NMEA_OUTPUT_5HZ "$PMTK314,0,5,0,5,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" //Set GGA and RMC to 5HZ
|
|
||||||
#define NMEA_OUTPUT_4HZ "$PMTK314,0,4,0,4,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" //Set GGA and RMC to 4HZ
|
|
||||||
#define NMEA_OUTPUT_3HZ "$PMTK314,0,3,0,3,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" //Set GGA and RMC to 3HZ
|
|
||||||
#define NMEA_OUTPUT_2HZ "$PMTK314,0,2,0,2,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" //Set GGA and RMC to 2HZ
|
|
||||||
#define NMEA_OUTPUT_1HZ "$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" //Set GGA and RMC to 1HZ
|
|
||||||
|
|
||||||
#define LOCOSYS_REFRESH_RATE_200 "$PMTK220,200*2C" //200 milliseconds
|
|
||||||
#define LOCOSYS_REFRESH_RATE_250 "$PMTK220,250*29\r\n" //250 milliseconds
|
|
||||||
|
|
||||||
#define LOCOSYS_BAUD_RATE_4800 "$PMTK251,4800*14\r\n"
|
|
||||||
#define LOCOSYS_BAUD_RATE_9600 "$PMTK251,9600*17\r\n"
|
|
||||||
#define LOCOSYS_BAUD_RATE_19200 "$PMTK251,19200*22\r\n"
|
|
||||||
#define LOCOSYS_BAUD_RATE_38400 "$PMTK251,38400*27\r\n"
|
|
||||||
//**************************************************************************************************************
|
|
||||||
|
|
||||||
|
|
||||||
/****************************************************************
|
|
||||||
Parsing stuff for NMEA
|
|
||||||
****************************************************************/
|
|
||||||
#define BUF_LEN 200
|
|
||||||
|
|
||||||
// GPS Pointers
|
|
||||||
char *token;
|
|
||||||
char *search = ",";
|
|
||||||
char *brkb, *pEnd;
|
|
||||||
char gps_buffer[BUF_LEN]; //The traditional buffer.
|
|
||||||
|
|
||||||
void init_gps(void)
|
|
||||||
{
|
|
||||||
pinMode(2,OUTPUT); //Serial Mux
|
|
||||||
digitalWrite(2,HIGH); //Serial Mux
|
|
||||||
Serial.begin(9600);
|
|
||||||
delay(1000);
|
|
||||||
Serial.print(LOCOSYS_BAUD_RATE_38400);
|
|
||||||
Serial.begin(38400);
|
|
||||||
delay(500);
|
|
||||||
Serial.print(LOCOSYS_REFRESH_RATE_250);
|
|
||||||
delay(500);
|
|
||||||
Serial.print(NMEA_OUTPUT_4HZ);
|
|
||||||
delay(500);
|
|
||||||
Serial.print(SBAS_OFF);
|
|
||||||
|
|
||||||
|
|
||||||
/* EM406 example init
|
|
||||||
Serial.begin(4800); //Universal Sincronus Asyncronus Receiveing Transmiting
|
|
||||||
delay(1000);
|
|
||||||
Serial.print(SIRF_BAUD_RATE_9600);
|
|
||||||
|
|
||||||
Serial.begin(9600);
|
|
||||||
delay(1000);
|
|
||||||
|
|
||||||
Serial.print(GSV_OFF);
|
|
||||||
Serial.print(GSA_OFF);
|
|
||||||
|
|
||||||
#if USE_WAAS == 1
|
|
||||||
Serial.print(WAAS_ON);
|
|
||||||
#else
|
|
||||||
Serial.print(WAAS_OFF);
|
|
||||||
#endif
|
|
||||||
*/
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
void decode_gps(void)
|
|
||||||
{
|
|
||||||
const char head_rmc[]="GPRMC"; //GPS NMEA header to look for
|
|
||||||
const char head_gga[]="GPGGA"; //GPS NMEA header to look for
|
|
||||||
|
|
||||||
static unsigned long GPS_timer = 0; //used to turn off the LED if no data is received.
|
|
||||||
|
|
||||||
static byte unlock = 1; //some kind of event flag
|
|
||||||
static byte checksum = 0; //the checksum generated
|
|
||||||
static byte checksum_received = 0; //Checksum received
|
|
||||||
static byte counter = 0; //general counter
|
|
||||||
|
|
||||||
//Temporary variables for some tasks, specially used in the GPS parsing part (Look at the NMEA_Parser tab)
|
|
||||||
unsigned long temp = 0;
|
|
||||||
unsigned long temp2 = 0;
|
|
||||||
unsigned long temp3 = 0;
|
|
||||||
|
|
||||||
|
|
||||||
while(Serial.available() > 0)
|
|
||||||
{
|
|
||||||
if(unlock == 0)
|
|
||||||
{
|
|
||||||
gps_buffer[0] = Serial.read();//puts a byte in the buffer
|
|
||||||
|
|
||||||
if(gps_buffer[0]=='$')//Verify if is the preamble $
|
|
||||||
{
|
|
||||||
counter = 0;
|
|
||||||
checksum = 0;
|
|
||||||
unlock = 1;
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
gps_buffer[counter] = Serial.read();
|
|
||||||
|
|
||||||
if(gps_buffer[counter] == 0x0A)//Looks for \F
|
|
||||||
{
|
|
||||||
unlock = 0;
|
|
||||||
|
|
||||||
if (strncmp (gps_buffer, head_rmc, 5) == 0)//looking for rmc head....
|
|
||||||
{
|
|
||||||
|
|
||||||
/*Generating and parsing received checksum, */
|
|
||||||
for(int x=0; x<100; x++)
|
|
||||||
{
|
|
||||||
if(gps_buffer[x]=='*')
|
|
||||||
{
|
|
||||||
checksum_received = strtol(&gps_buffer[x + 1], NULL, 16);//Parsing received checksum...
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
checksum ^= gps_buffer[x]; //XOR the received data...
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if(checksum_received == checksum)//Checking checksum
|
|
||||||
{
|
|
||||||
/* Token will point to the data between comma "'", returns the data in the order received */
|
|
||||||
/*THE GPRMC order is: UTC, UTC status , Lat, N/S indicator, Lon, E/W indicator, speed, course, date, mode, checksum*/
|
|
||||||
token = strtok_r(gps_buffer, search, &brkb); //Contains the header GPRMC, not used
|
|
||||||
|
|
||||||
token = strtok_r(NULL, search, &brkb); //UTC Time, not used
|
|
||||||
//time= atol (token);
|
|
||||||
token = strtok_r(NULL, search, &brkb); //Valid UTC data? maybe not used...
|
|
||||||
|
|
||||||
|
|
||||||
//Longitude in degrees, decimal minutes. (ej. 4750.1234 degrees decimal minutes = 47.835390 decimal degrees)
|
|
||||||
//Where 47 are degrees and 50 the minutes and .1234 the decimals of the minutes.
|
|
||||||
//To convert to decimal degrees, devide the minutes by 60 (including decimals),
|
|
||||||
//Example: "50.1234/60=.835390", then add the degrees, ex: "47+.835390 = 47.835390" decimal degrees
|
|
||||||
token = strtok_r(NULL, search, &brkb); //Contains Latitude in degrees decimal minutes...
|
|
||||||
|
|
||||||
//taking only degrees, and minutes without decimals,
|
|
||||||
//strtol stop parsing till reach the decimal point "." result example 4750, eliminates .1234
|
|
||||||
temp = strtol (token, &pEnd, 10);
|
|
||||||
|
|
||||||
//takes only the decimals of the minutes
|
|
||||||
//result example 1234.
|
|
||||||
temp2 = strtol (pEnd + 1, NULL, 10);
|
|
||||||
|
|
||||||
//joining degrees, minutes, and the decimals of minute, now without the point...
|
|
||||||
//Before was 4750.1234, now the result example is 47501234...
|
|
||||||
temp3 = (temp * 10000) + (temp2);
|
|
||||||
|
|
||||||
|
|
||||||
//modulo to leave only the decimal minutes, eliminating only the degrees..
|
|
||||||
//Before was 47501234, the result example is 501234.
|
|
||||||
temp3 = temp3 % 1000000;
|
|
||||||
|
|
||||||
|
|
||||||
//Dividing to obtain only the de degrees, before was 4750
|
|
||||||
//The result example is 47 (4750/100 = 47)
|
|
||||||
temp /= 100;
|
|
||||||
|
|
||||||
//Joining everything and converting to * 10,000,000 ...
|
|
||||||
//First i convert the decimal minutes to degrees decimals stored in "temp3", example: 501234/600000 =.835390
|
|
||||||
//Then i add the degrees stored in "temp" and add the result from the first step, example 47+.835390 = 47.835390
|
|
||||||
//The result is stored in "lat" variable...
|
|
||||||
//**This is all changed in this case to be a long integer which is decimal degrees * 10**7
|
|
||||||
|
|
||||||
lat = (temp * 10000000) + ((temp3 *100) / 6);
|
|
||||||
|
|
||||||
token = strtok_r(NULL, search, &brkb); //lat, north or south?
|
|
||||||
//If the char is equal to S (south), multiply the result by -1..
|
|
||||||
if(*token == 'S'){
|
|
||||||
lat *= -1;
|
|
||||||
}
|
|
||||||
|
|
||||||
//This the same procedure use in lat, but now for Lon....
|
|
||||||
token = strtok_r(NULL, search, &brkb);
|
|
||||||
temp = strtol (token,&pEnd, 10);
|
|
||||||
temp2 = strtol (pEnd + 1, NULL, 10);
|
|
||||||
temp3 = (temp * 10000) + (temp2);
|
|
||||||
temp3 = temp3%1000000;
|
|
||||||
temp/= 100;
|
|
||||||
lon = (temp * 10000000) + ((temp3 * 100) / 6);
|
|
||||||
|
|
||||||
token = strtok_r(NULL, search, &brkb); //lon, east or west?
|
|
||||||
if(*token == 'W'){
|
|
||||||
lon *= -1;
|
|
||||||
}
|
|
||||||
|
|
||||||
token = strtok_r(NULL, search, &brkb); //Speed over ground
|
|
||||||
speed_3d = (float)atoi(token); // We only get ground speed but store it as speed_3d for use in DCM
|
|
||||||
|
|
||||||
token = strtok_r(NULL, search, &brkb); //Course
|
|
||||||
ground_course = (float)atoi(token);
|
|
||||||
|
|
||||||
gpsFixnew=1; //new information available flag for binary message
|
|
||||||
|
|
||||||
}
|
|
||||||
checksum = 0;
|
|
||||||
}//End of the GPRMC parsing
|
|
||||||
|
|
||||||
if (strncmp (gps_buffer, head_gga, 5) == 0)//now looking for GPGGA head....
|
|
||||||
{
|
|
||||||
/*Generating and parsing received checksum, */
|
|
||||||
for(int x = 0; x<100; x++)
|
|
||||||
{
|
|
||||||
if(gps_buffer[x]=='*')
|
|
||||||
{
|
|
||||||
checksum_received = strtol(&gps_buffer[x + 1], NULL, 16);//Parsing received checksum...
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
checksum^= gps_buffer[x]; //XOR the received data...
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if(checksum_received== checksum)//Checking checksum
|
|
||||||
{
|
|
||||||
//strcpy(gps_GGA,gps_buffer);
|
|
||||||
|
|
||||||
token = strtok_r(gps_buffer, search, &brkb);//GPGGA header, not used anymore
|
|
||||||
token = strtok_r(NULL, search, &brkb);//UTC, not used!!
|
|
||||||
token = strtok_r(NULL, search, &brkb);//lat, not used!!
|
|
||||||
token = strtok_r(NULL, search, &brkb);//north/south, nope...
|
|
||||||
token = strtok_r(NULL, search, &brkb);//lon, not used!!
|
|
||||||
token = strtok_r(NULL, search, &brkb);//wets/east, nope
|
|
||||||
token = strtok_r(NULL, search, &brkb);//Position fix, used!!
|
|
||||||
|
|
||||||
if(atoi(token) >= 1){
|
|
||||||
gpsFix = 0x00; // gpsFix = 0 means valid fix
|
|
||||||
}else{
|
|
||||||
gpsFix = 0x01;
|
|
||||||
}
|
|
||||||
token = strtok_r(NULL, search, &brkb); //sats in use!! Nein...
|
|
||||||
token = strtok_r(NULL, search, &brkb);//HDOP, not needed
|
|
||||||
token = strtok_r(NULL, search, &brkb);//ALTITUDE, is the only meaning of this string.. in meters of course.
|
|
||||||
alt_MSL = abs(atoi(token)) * 1000;
|
|
||||||
|
|
||||||
if(gpsFix == 0x00) digitalWrite(6, HIGH); //Status LED...
|
|
||||||
else digitalWrite(6, LOW);
|
|
||||||
}
|
|
||||||
checksum = 0; //Restarting the checksum
|
|
||||||
}
|
|
||||||
|
|
||||||
for(int a = 0; a<= counter; a++)//restarting the buffer
|
|
||||||
{
|
|
||||||
gps_buffer[a]= 0;
|
|
||||||
}
|
|
||||||
counter = 0; //Restarting the counter
|
|
||||||
GPS_timer = millis(); //Restarting timer...
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
counter++; //Incrementing counter
|
|
||||||
if (counter >= 200)
|
|
||||||
{
|
|
||||||
//Serial.flush();
|
|
||||||
counter = 0;
|
|
||||||
checksum = 0;
|
|
||||||
unlock = 0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if(millis() - GPS_timer > 2000){
|
|
||||||
digitalWrite(6, LOW); //If we don't receive any byte in two seconds turn off gps fix LED...
|
|
||||||
gpsFix = 0x01;
|
|
||||||
gpsFixnew = 0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
Reference in New Issue
Block a user