static int gprs_test( lua_State* L )
{
const char *NMEA = luaL_checkstring( L, 1 );
double LAT;
double LONG;
double ALT;
double DOP;
char saida[15];
int ret = 0;
ret = parse_gps((char*)NMEA, &LAT, &LONG, &ALT, &DOP);
if( ret == 4 )
{
c_memset(saida,0,15);
c_sprintf(saida,"%.06f",LAT);
lua_pushstring(L, saida);
c_memset(saida,0,15);
c_sprintf(saida,"%.06f",LONG);
lua_pushstring(L, saida);
c_memset(saida,0,15);
c_sprintf(saida,"%.01f",ALT);
lua_pushstring(L, saida);
c_memset(saida,0,15);
c_sprintf(saida,"%.01f",DOP);
lua_pushstring(L, saida);
return 4; // 4 é o número de valores a serem retornados
}
return 0;
}
static int gprs_getlocation( lua_State* L )
{
size_t len;
//uint32_t timeout = luaL_checkinteger(L, 1)*1000;
uint32_t period = luaL_checkinteger(L, 3);
char *position;
double LAT = 0.0f;
double LONG = 0.0f;
double ALT = 0.0f;
double DOP = 0.0f;
char saida[15];
char cmd[15];
int ret = 0;
/*
* $GPGGA,180857.000,2524.83983,S,04917.42591,W,1,03,3.1,0.0,M,,M,,0000*47
$GPGSA,A,2,09,23,22,,,,,,,,,,3.3,3.1,1.0*38
$GPGSV,3,1,09,09,37,236,26,23,53,188,34,22,49,061,28,30,02,319,*77
$GPGSV,3,2,09,26,20,117,,07,36,317,,06,18,230,,03,73,108,*77
$GPGSV,3,3,09,01,29,352,*4E
$GPRMC,180857.000,A,2524.83983,S,04917.42591,W,0.70,315.11,120617,,,A*69
$GPVTG,315.11,T,,M,0.70,N,1.30,K,A*3F
* */
if (ReadEnable == 0)
{
memset(cmd,0,15);
c_sprintf(cmd,"%s=%d",GPSRD,period);
if( sendCommonCommand(L,cmd) == 1)
ReadEnable = 1;
}
//if( sendCommonCommand(L,cmd) == 1)
if( (position = readCommand(NULL, "$GNACC", "$GNACC", GPSRDLOCAL, EOL, L)) != NULL )
{
ret = parse_gps(position, &LAT, &LONG, &ALT, &DOP);
if( ret == 4 )
{
c_memset(saida,0,15);
c_sprintf(saida,"%.6f",LAT);
lua_pushstring(L, saida);
c_memset(saida,0,15);
c_sprintf(saida,"%.6f",LONG);
lua_pushstring(L, saida);
c_memset(saida,0,15);
c_sprintf(saida,"%.1f",ALT);
lua_pushstring(L, saida);
c_memset(saida,0,15);
c_sprintf(saida,"%.1f",DOP);
lua_pushstring(L, saida);
return 4; // 4 é o número de valores a serem retornados
}
}
return 0;
}
//Print the state of the world array
//This array is a mix of visible ASCII and binary values
void print_sotw(void)
{
printf("\n\n");
//Get GPS
#ifdef GPS_ON
if(parse_gps() != 0)
{
printf("LA:%04d.", get_lat_high()); //GPS
printf("%04d ", get_lat_low());
printf("LO:%05d.", get_long_high());
printf("%04d ", get_long_low());
printf("FIX:%01d ", get_fix());
printf("SIV:%01d ", get_siv());
}
else
printf("!NoGPS!");
#endif
printf("\n");
}
int read_gps(struct gps *gpsData_ptr)
{
// serial port buffer handle
cyg_io_handle_t port_handle;
cyg_serial_buf_info_t buff_info;
unsigned int len = sizeof (buff_info);
// get serial port handle
cyg_io_lookup( gpsData_ptr->portName, &port_handle );
cyg_io_get_config (port_handle, CYG_IO_GET_CONFIG_SERIAL_BUFFER_INFO,\
&buff_info, &len);
unsigned int bytesInBuffer = buff_info.rx_count;
unsigned int bytesReadThisCall =0;;
unsigned short msgPayloadSize = 0, bytesToRead = 0, bytesRead = 0;
int status = 0;
// Initialization of persistent local buffer
if (gpsData_ptr->localBuffer == NULL)
{
gpsData_ptr->localBuffer = (unsigned char*) malloc (1024 * sizeof (unsigned char));
}
// First check if there are any bytes in the serial buffer, return if none
if( bytesInBuffer == 0 )
return -1;
// Get localBuffer stored in gps packet. This is to keep the following code readable
localBuffer = gpsData_ptr->localBuffer;
bytesInLocalBuffer= gpsData_ptr->bytesInLocalBuffer;
readState = gpsData_ptr->readState;
// Keep reading until we've processed all of the bytes in the buffer
while (bytesReadThisCall < bytesInBuffer){
switch (readState){
case 0: //Look for packet header bytes
// Read in up to 2 bytes to the first open location in the local buffer
bytesRead = read(gpsData_ptr->port,&localBuffer[bytesInLocalBuffer],2-bytesInLocalBuffer);
bytesReadThisCall += bytesRead; // keep track of bytes read during this call
if (localBuffer[0] == 0xA0){ // Check for first header byte
bytesInLocalBuffer = 1;
if (localBuffer[1] == 0xA2){ // Check for second header byte
bytesInLocalBuffer = 2;
readState++; // header complete, move to next stage
}
}
else {
gpsData_ptr->err_type = noPacketHeader;
}
break; // end case 0
case 1: //Look for message size bytes
// Find how many bytes need to be read for data length (2) - bytes already read (includes 2 byte header)
bytesToRead = 4 - bytesInLocalBuffer;
// Read in up to 2 bytes to the first open location in the local buffer
bytesRead = read(gpsData_ptr->port,&localBuffer[bytesInLocalBuffer],bytesToRead);
bytesReadThisCall += bytesRead; // keep track of bytes read during this call
bytesInLocalBuffer += bytesRead; // keep track of bytes in local buffer
if (bytesRead == bytesToRead){
readState++; // size bytes complete, move to next stage
}
else{
//printf ("\n<GPS>: Failed to get size bytes");
gpsData_ptr->err_type = incompletePacket;
}
break; // end case 1
case 2: //Read payload, checksum, and stop bytes
// Find message payload size
msgPayloadSize = getbeuw(localBuffer,2)&0x7FFF; // packet size is 15 bits. bitwise AND ensures only 15 bits are used.
//printf("<GPS>: msgPayloadSize: %d\n",msgPayloadSize);
// Error checking on payload size. If size is bigger than expected, dump packet
if(msgPayloadSize > GPS_MAX_MSG_SIZE){
gpsData_ptr->err_type = incompletePacket;
reset_localBuffer();
}
// Find how many bytes need to be read for the total message (Header (2) + Size (2) + Payload + checksum (2) + stop (2) - bytes already read )
bytesToRead = msgPayloadSize + 8 - bytesInLocalBuffer;
// Read in the remainder of the message to the local buffer, starting at the first empty location
bytesRead = read (gpsData_ptr->port, &localBuffer[bytesInLocalBuffer], bytesToRead);
bytesInLocalBuffer += bytesRead; // keep track of bytes in local buffer
bytesReadThisCall += bytesRead; // keep track of bytes read during this call
if (bytesRead == bytesToRead){
//printf ("\n<GPS>: Got complete message! Tried for %d, got %d",bytesToRead,bytesRead);
//printf("<GPS>: Packet ID %d \n", localBuffer[4]);
//printf("<GPS>: msgPayloadSize: %d\n",msgPayloadSize);
//printf("<GPS>: My checksum: %d Recv: %d\n",do_chksum(localBuffer, 4, msgPayloadSize+4),getbeuw(localBuffer,4+msgPayloadSize));
// Checksum verification (15-bit, big endian)
if ( (do_chksum(localBuffer, 4, msgPayloadSize+4)&0x7FFF) == (getbeuw(localBuffer,4+msgPayloadSize)&0x7FFF) ){
// If it's OK, extract data
parse_gps( gpsData_ptr );
gpsData_ptr->err_type = data_valid;
//printf ("\n<GPS>: Success!");
status = 1;
}
else{
//printf ("\n<GPS>: Checksum mismatch!");
gpsData_ptr->err_type = checksum_err;
}
reset_localBuffer(); // Clear the local buffer regardless
}
else{
//printf ("<GPS>: Didn't get complete message. Tried for %d, got %d. %d\n",bytesToRead,bytesRead,msgPayloadSize);
gpsData_ptr->err_type= incompletePacket;
status = 0;
}
break; // end case 2
default:
reset_localBuffer();
printf ("\n<GPS>: Why are you here?");
status = 0;
break; // end default
} // end switch (readState)
} // end while (bytesReadThisCall < bytesInBuffer)
// Store local buffer in gps packet
gpsData_ptr->localBuffer = localBuffer;
gpsData_ptr->bytesInLocalBuffer = bytesInLocalBuffer;
gpsData_ptr->readState = readState;
return status;
}
// How to handle static variables with multiple sensors? objects? add to gpspacket?
int read_gps(struct gps *gpsData_ptr)
{
cyg_io_handle_t port_handle;
cyg_serial_buf_info_t buff_info;
unsigned int len = sizeof (buff_info);
// get serial port handle
cyg_io_lookup( gpsData_ptr->portName, &port_handle );
cyg_io_get_config (port_handle, CYG_IO_GET_CONFIG_SERIAL_BUFFER_INFO,\
&buff_info, &len);
unsigned int bytesInBuffer = buff_info.rx_count;
unsigned int bytesReadThisCall =0;;
unsigned short msgPayloadSize = 0, bytesToRead = 0, bytesRead = 0;
int status =0;
// Initialization of persistent local buffer
if (gpsData_ptr->localBuffer == NULL)
{
gpsData_ptr->localBuffer = (unsigned char*) malloc (1024 * sizeof (unsigned char));
}
// First check if there are any bytes in the serial buffer, return if none
if( bytesInBuffer == 0 )
return -1;
// Get localBuffer stored in gps packet. This is to keep the following code readable
localBuffer = gpsData_ptr->localBuffer;
bytesInLocalBuffer= gpsData_ptr->bytesInLocalBuffer;
readState = gpsData_ptr->readState;
//fprintf(stderr, "read state is %d (before while)\n", readState);
// Keep reading until we've processed all of the bytes in the buffer
while (bytesReadThisCall < bytesInBuffer){
//fprintf(stderr, "read state is %d (after while)\n", readState);
switch (readState){
case 0: //Look for packet header bytes
// Read in up to 4 bytes to the first open location in the local buffer
bytesRead = read(gpsData_ptr->port,&localBuffer[bytesInLocalBuffer],4-bytesInLocalBuffer);
bytesReadThisCall += bytesRead; // keep track of bytes read during this call
if (localBuffer[0] == '$'){ // Check for first header byte
bytesInLocalBuffer = 1;
//fprintf(stderr, "case 0, $ header type \n");
if (localBuffer[1] == 'B'){ // Check for second header byte
bytesInLocalBuffer = 2;
if (localBuffer[2] == 'I'){ // Check for third header byte
bytesInLocalBuffer = 3;
if (localBuffer[3] == 'N'){ // Check for fourth header byte
bytesInLocalBuffer = 4;
readState++; // header complete, move to next stage
}
}
}
}
else {
gpsData_ptr->err_type = noPacketHeader;
}
break; // end case 0
case 1: // Look for block ID and data length
// Find how many bytes need to be read for block ID (2) + data length (2) - bytes already read (includes 4 byte header)
bytesToRead = 8 - bytesInLocalBuffer;
// Read in bytes to the last location in the local buffer
bytesRead = read(gpsData_ptr->port,&localBuffer[bytesInLocalBuffer],bytesToRead);
bytesInLocalBuffer += bytesRead; // keep track of bytes in local buffer
bytesReadThisCall += bytesRead; // keep track of bytes read during this call
if (bytesRead == bytesToRead){
readState++;
//printf ("\n<GPS>: Got msgID: %d and Data Length: %d", localBuffer[5]*256 + localBuffer[4], localBuffer[7]*256 + localBuffer[6]);
}
else{
gpsData_ptr->err_type = incompletePacket;
}
break; // end case 1
case 2: //Read payload, checksum, and stop bytes
// Find message payload size
msgPayloadSize = localBuffer[7]*256 + localBuffer[6]; // data is in little endian format
// Error checking on payload size. If size is bigger than expected, dump packet
if(msgPayloadSize > GPS_MAX_MSG_SIZE){
gpsData_ptr->err_type = incompletePacket;
reset_localBuffer();
}
// Find how many bytes need to be read for the total message (Header (4) + ID (2) + Size (2) + Payload + checksum (2) + stop (2) - bytes already read )
bytesToRead = msgPayloadSize + 12 - bytesInLocalBuffer;
// Read in the remainder of the message to the local buffer, starting at the first empty location
bytesRead = read (gpsData_ptr->port, &localBuffer[bytesInLocalBuffer], bytesToRead);
bytesInLocalBuffer += bytesRead; // keep track of bytes in local buffer
bytesReadThisCall += bytesRead; // keep track of bytes read during this call
if (bytesRead == bytesToRead){
//printf ("\n<GPS>: Got complete message! Tried for %d, got %d",bytesToRead,bytesRead);
//printf ("\n<GPS>: My checksum: %d Recv: %d",do_chksum(localBuffer, 8, msgPayloadSize+8),(localBuffer[8+msgPayloadSize+1]*256 + localBuffer[8+msgPayloadSize]));
// Checksum verification
if ( do_chksum(localBuffer, 8, msgPayloadSize+8) == (localBuffer[8+msgPayloadSize+1]*256 + localBuffer[8+msgPayloadSize]) ){
// If it's OK, extract data
parse_gps( gpsData_ptr );
gpsData_ptr->err_type = data_valid;
//printf ("\n<GPS>: Success!");
status = 1;
}
else{
//printf ("\n<GPS>: Checksum mismatch!");
gpsData_ptr->err_type = checksum_err;
}
reset_localBuffer();
}
else{
//printf ("\n<GPS>: Didn't get complete message. Tried for %d, got %d",bytesToRead,bytesRead);
gpsData_ptr->err_type= incompletePacket;
status = 0;
}
break; // end case 2
default:
reset_localBuffer();
printf ("\n<GPS>: Why are you here?");
status = 0;
break; // end default
} // end switch (readState)
} // end while (bytesReadThisCall < bytesInBuffer)
// Store local buffer in gps packet
gpsData_ptr->localBuffer = localBuffer;
gpsData_ptr->bytesInLocalBuffer = bytesInLocalBuffer;
gpsData_ptr->readState = readState;
return status;
}
int parse_var(char *oneline,char *file)
{
static int limit = 0;
if(limit++ < 50) {
return -1;
} else {
limit = 0;
}
enum {
NAME = 0,
UTC,
LAT_FLAG,
LATITUDE,
SN,
LONGITUDE,
WE,
SPEED,
GDIR,
HIG_DATE,
CIPJ,
CIPJE,
LOCT_MODE,
CRC
};
char gpsinfo[16][32] = {"","","","","","","","","","","","","","","",""};
if(strstr(oneline,"RMC")) {
parse_gps(oneline,LAT_FLAG,gpsinfo[LAT_FLAG]);
parse_gps(oneline,SN,gpsinfo[SN]);
parse_gps(oneline,SPEED,gpsinfo[SPEED]);
parse_gps(oneline,GDIR,gpsinfo[GDIR]);
parse_gps(oneline,LATITUDE,gpsinfo[LATITUDE]);
parse_gps(oneline,LONGITUDE,gpsinfo[LONGITUDE]);
} else if (strstr(oneline,"GGA")) {
//get height
parse_gps(oneline,HIG_DATE,gpsinfo[HIG_DATE]);
} else {
//do not need other gps info
return -1;
}
FILE *fp = fopen(file,"w+");
if(!fp)
return -1;
char strbuf[128] = "Default\n";
fwrite(strbuf,1,strlen(strbuf),fp);
// A is Valid. V is invalid
if(strcmp(gpsinfo[LAT_FLAG],"A") == 0) {
sprintf(strbuf,"ns=%s\n",gpsinfo[SN]);
fwrite(strbuf,1,strlen(strbuf),fp);
sprintf(strbuf,"speed=%s\n",gpsinfo[SPEED]);
fwrite(strbuf,1,strlen(strbuf),fp);
sprintf(strbuf,"height=%s\n",gpsinfo[HIG_DATE]);
fwrite(strbuf,1,strlen(strbuf),fp);
sprintf(strbuf,"gpsdir=%s\n",gpsinfo[GDIR]);
fwrite(strbuf,1,strlen(strbuf),fp);
sprintf(strbuf,"latitude=%s\n",gpsinfo[LATITUDE]);
fwrite(strbuf,1,strlen(strbuf),fp);
sprintf(strbuf,"longitude=%s\n",gpsinfo[LONGITUDE]);
fwrite(strbuf,1,strlen(strbuf),fp);
} else {
fwrite("speed=\n",1,strlen("speed=\n"),fp);
fwrite("gpsdir=\n",1,strlen("gpsdir=\n"),fp);
fwrite("latitude=\n",1,strlen("latitude=\n"),fp);
fwrite("longitude=\n",1,strlen("longitude=\n"),fp);
fwrite("ns=\n",1,strlen("ns=\n"),fp);
fwrite("height=\n",1,strlen("height=\n"),fp);
}
fclose(fp);
#if debug
int i = 0;
for(i;i<CRC;i++)
printf("[%s] ",gpsinfo[i]);
printf("\n");
#endif
return 0;
}
// How to handle static variables with multiple sensors? objects? add to gpspacket?
int read_gps(struct gps *gpsData_ptr)
{
cyg_io_handle_t port_handle;
cyg_serial_buf_info_t buff_info;
unsigned int len = sizeof (buff_info);
// get serial port handle
cyg_io_lookup( gpsData_ptr->portName, &port_handle );
cyg_io_get_config (port_handle, CYG_IO_GET_CONFIG_SERIAL_BUFFER_INFO,\
&buff_info, &len);
unsigned int bytesInBuffer = buff_info.rx_count;
unsigned int bytesReadThisCall = 0;
unsigned short msgPayloadSize = 0, bytesToRead = 0, bytesRead = 0;
unsigned long CRC_computed, CRC_read;
int j, status =0;
// Initialization of persistent local buffer
if (gpsData_ptr->localBuffer == NULL)
{
gpsData_ptr->localBuffer = (unsigned char*) malloc (1024 * sizeof (unsigned char));
}
// First check if there are any bytes in the serial buffer, return if none
if( bytesInBuffer == 0 )
return -1;
// Get localBuffer stored in gps packet. This is to keep the following code readable
localBuffer = gpsData_ptr->localBuffer;
bytesInLocalBuffer= gpsData_ptr->bytesInLocalBuffer;
readState = gpsData_ptr->readState;
while (bytesReadThisCall < bytesInBuffer){
switch (readState){
case 0: //Look for packet header bytes
// Read in up to 3 bytes to the first open location in the local buffer
//fprintf(stderr,"bytesInLocalBuffer is %d\n",bytesInLocalBuffer);
bytesRead = read(gpsData_ptr->port,&localBuffer[bytesInLocalBuffer],3-bytesInLocalBuffer);
//fprintf(stderr,"The first three bytes are %0X %0X %0X\n",localBuffer[0],localBuffer[1],localBuffer[2]);
//fprintf(stderr,"bytesRead is %d\n",bytesRead);
//fprintf(stderr,"Read %d bytes, The first three bytes are %0X %0X %0X\n", bytesRead,localBuffer[0],localBuffer[1],localBuffer[2]);
bytesReadThisCall += bytesRead; // keep track of bytes read during this call
if (localBuffer[0] == 0xAA){ // Check for first header byte
bytesInLocalBuffer = 1;
//fprintf(stderr, "case 0, 0xAA header type \n");
if (localBuffer[1] == 0x44){ // Check for second header byte
bytesInLocalBuffer = 2;
if (localBuffer[2] == 0x12){ // Check for third header byte
bytesInLocalBuffer = 3;
readState++;
}
}
}
else {
gpsData_ptr->err_type = noPacketHeader;
}
break; // end case 0
case 1: // Look for block ID and data length
// Read 28 Header Bytes
bytesToRead = 28 - bytesInLocalBuffer;
// Read in bytes to the last location in the local buffer
bytesRead = read(gpsData_ptr->port,&localBuffer[bytesInLocalBuffer],bytesToRead);
bytesInLocalBuffer += bytesRead; // keep track of bytes in local buffer
bytesReadThisCall += bytesRead; // keep track of bytes read during this call
if (bytesRead == bytesToRead){
readState++;
//fprintf (stderr,"<GPS>: Got msgID: %d and Data Length: %d\n", localBuffer[5]*256 + localBuffer[4], localBuffer[9]*256 + localBuffer[8]);
//printf ("<GPS>: localBuffer[0] = %02X localBuffer[1] = %02X localBuffer[2] = %02X localBuffer[3] = %02X localBuffer[4] = %02X localBuffer[5] = %02X \n", localBuffer[0], localBuffer[1], localBuffer[2], localBuffer[3], localBuffer[4], localBuffer[5]);
}
else{
gpsData_ptr->err_type = incompletePacket;
}
break; // end case 1
case 2: //Read payload
// Find message payload size
msgPayloadSize = localBuffer[9]*256 + localBuffer[8]; // data is in little endian format
// Error checking on payload size. If size is bigger than expected, dump packet
if(msgPayloadSize > GPS_MAX_MSG_SIZE){
gpsData_ptr->err_type = incompletePacket;
reset_localBuffer();
}
// Find how many bytes need to be read for the total message (Sync (3) + Remaining Header (25) + Payload - bytes already read )
bytesToRead = msgPayloadSize + 28 - bytesInLocalBuffer;
//printf("bytesInLocalBuffer is %d bytesToRead is %d \n",bytesInLocalBuffer,bytesToRead);
// Read in the remainder of the message to the local buffer, starting at the first empty location
bytesRead = read (gpsData_ptr->port, &localBuffer[bytesInLocalBuffer], bytesToRead);
bytesInLocalBuffer += bytesRead; // keep track of bytes in local buffer
bytesReadThisCall += bytesRead; // keep track of bytes read during this call
if (bytesRead == bytesToRead){
//printf ("<GPS>: Got complete message! Tried for %d, got %d\n",bytesToRead,bytesRead);
readState++;
}
else {
gpsData_ptr->err_type = incompletePacket;
}
break; // end case 2
case 3: // read CRC bytes (4 bytes)
bytesToRead = 4;
bytesRead = read (gpsData_ptr->port, &localBuffer[bytesInLocalBuffer], bytesToRead);
bytesInLocalBuffer += bytesRead;
bytesReadThisCall += bytesRead;
if(bytesRead == bytesToRead) {
// CRC verification
CRC_computed = CalculateBlockCRC32(bytesInLocalBuffer-4,localBuffer);
endian_swap(localBuffer,140,4);
CRC_read = *(uint32_t *) (localBuffer+140);
if (CRC_computed == CRC_read) {
gpsData_ptr->err_type = data_valid;
parse_gps(gpsData_ptr);
//fprintf (stderr,"<GPS t = %9.3lf>: Success!\n",gpsData_ptr->GPS_TOW);
}
else{
send_status("GPS CRC ERR");
//fprintf (stderr,"<GPS>: Checksum mismatch!\n");
/* ============= DEBUG CHECKSUM ERROR ================
fprintf (stderr,"<GPS %d>: Checksum mismatch! Buffer: %02X%02X%02X%02X Read: %08lX Computed: %08lX\n",localBuffer[5]*256 + localBuffer[4],localBuffer[140],localBuffer[141],localBuffer[142],localBuffer[143],CRC_read,CRC_computed);
fprintf (stderr,"Hex: \n");
for (j = 0; j < bytesInLocalBuffer; j++) {
fprintf(stderr,"%02X ",localBuffer[j]);
if(j%8==7)
fprintf(stderr,"\n");
}
*/
gpsData_ptr->err_type = checksum_err;
}
reset_localBuffer();
}
else{
//printf ("\n<GPS>: Didn't get complete message. Tried for %d, got %d",bytesToRead,bytesRead);
gpsData_ptr->err_type= incompletePacket;
status = 0;
}
break; // end case 3
default:
reset_localBuffer();
printf ("\n<GPS>: Why are you here?");
status = 0;
break; // end default
} // end switch (readState)
} // end while (bytesReadThisCall < bytesInBuffer)
// Store local buffer in gps packet
gpsData_ptr->localBuffer = localBuffer;
gpsData_ptr->bytesInLocalBuffer = bytesInLocalBuffer;
gpsData_ptr->readState = readState;
return status;
}
