Skip to content

Instantly share code, notes, and snippets.

@LebedevRI

LebedevRI/gpsControl.c

Created Mar 15, 2016
Star
Embed
What would you like to do?
Learn more about clone URLs
Download ZIP
./gpsControl -s -d /dev/ttyAMA0 = Stationary Mode
Raw
gpsControl.c
#include <stdio.h>
#include <string.h>
#include <termios.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
void usage( void )
{
printf("Usage: gpsControl <mode> -d <device>\n");
printf("Modes are:\n");
printf(" -s = stationary mode\n");
printf(" -p = portable mode\n");
printf(" -d = device \n");
}
unsigned char ReadBByteFromSerial(int fd, int *k)
{
unsigned char tmp;
(*k)++;
if (read(fd, &tmp, 1) > 0)
{
// if (tmp & 0x80)
// {
// printf("<%2x> ", tmp);
// }
// else
// {
// printf("%c", tmp);
// }
}
return tmp;
}
int main(int argc, char *argv[])
{
const char cfg_cfg[] = {0xB5, 0x62, 0x06, 0x09, 0x0D, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x1D, 0xAB};
const char portable[] = {0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, \
0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x10};
const char sationary[] = {0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x02, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, \
0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x4E, 0x60};
const char walking[] = {0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x03, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, \
0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x13, 0x76};
const char automotive[] = {0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x04, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, \
0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x14, 0x98};
const char sea[] = {0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x05, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, \
0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x15, 0xBA};
const char airborne1[] = {0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x06, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, \
0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x16, 0xDC};
const char airborne2[] = {0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x07, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, \
0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xFE};
const unsigned char airborne4[] = {0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x08, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, \
0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x20};
const unsigned char ack_ack[] = {0xB5, 0x62, 0x05, 0x01, 0x02, 0x00, 0x06, 0x24, 0x32, 0x5B};
char buff[50000];
unsigned char *toSend;
int len, fd, Retries;
int option = 0;
int k = 0;
int OK;
char devName[50];
struct termios options;
memset( devName, 0, sizeof(devName));
while(( option = getopt( argc, argv, "spd:")) != -1)
{
switch( option)
{
case 'd':
printf("Configuring device %s\n", optarg );
strncpy( devName, optarg, sizeof(devName ));
break;
case 's':
printf("Set GPS for stationary mode\n");
toSend = (char*)sationary;
len = sizeof(sationary);
break;
/* case 'a':
printf("Set GPS for Automotive mode\n");
toSend = (char*)automotive;
len = sizeof(automotive);
break;
*/
case 'p':
printf("Set GPS for portable mode\n");
toSend = (char*)portable;
len = sizeof(portable);
break;
/*
case 'S':
printf("Set GPS for Sea mode\n");
toSend = (char*)sea;
len = sizeof(sea);
break;
case 'w':
printf("Set GPS for Walking mode\n");
toSend = (char*)walking;
len = sizeof(walking);
break;
case '1':
printf("Set GPS for Airborne <1G mode\n");
toSend = (char*)airborne1;
len = sizeof(airborne1);
break;
case '2':
printf("Set GPS for Airborne <2G mode\n");
toSend = (char*)airborne2;
len = sizeof(airborne2);
break;
case '4':
printf("Set GPS for Airborne <4G mode\n");
toSend = (char*)airborne4;
len = sizeof(airborne4);
break;
*/
default:
usage();
return(0);
}
}
if( devName[0] == 0)
{
usage();
return( 0 );
}
fd = open(devName, O_RDWR); // | O_NOCTTY);
if (fd < 0)
{
printf("Cannot open device '%s'\n", devName);
return 0;
}
// get the current options
tcgetattr(fd, &options);
options.c_lflag &= ~ECHO;
options.c_cc[VMIN] = 0;
options.c_cc[VTIME] = 10;
cfsetispeed(&options, B9600);
cfsetospeed(&options, B9600);
options.c_cflag &= ~CSTOPB;
options.c_cflag &= ~CSIZE;
options.c_cflag |= CS8;
options.c_oflag &= ~ONLCR;
options.c_oflag &= ~OPOST;
options.c_iflag &= ~IXON;
options.c_iflag &= ~IXOFF;
tcsetattr(fd, TCSANOW, &options);
Retries = 0;
OK = 0;
while ((Retries < 5) && !OK)
{
printf (">>>>>>>>>>>>>>>>>>>>> SENDING >>>>>>>>>>>>>>>\n");
write(fd, toSend, len);
k = 0;
Retries++;
while ((k < 1000) && !OK)
{
if (ReadBByteFromSerial(fd, &k) == 0xb5)
{
if (ReadBByteFromSerial(fd, &k) == 0x62)
{
if (ReadBByteFromSerial(fd, &k) == 0x05)
{
if (ReadBByteFromSerial(fd, &k) == 0x01)
{
ReadBByteFromSerial(fd, &k);
ReadBByteFromSerial(fd, &k);
if (ReadBByteFromSerial(fd, &k) == toSend[2])
{
if (ReadBByteFromSerial(fd, &k) == toSend[3])
{
OK = 1;
}
}
}
}
}
}
}
}
if (OK)
{
printf("GPS mode set OK\n");
}
else
{
printf("** FAILED to set GPS Mode**\n");
}
close(fd);
return 0;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment