Ubloxconf Source Code
From FroboMind [www.frobomind.org]
/*
* Copyright 2012 Hjalte Nygaard
*
* This program is part of FroboMind.
*
* gpsconfig is free software: you can redistribute it and/or modify it under the terms
* of the GNU General Public License as published by the Free Software Foundation, either
* version 3 of the License, or (at your option) any later version.
*
* gpsconfig is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
* without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along with gpsconfig.
* If not, see http://www.gnu.org/licenses/.
*
*
* gpsconfig is a linux parser for ublox configuration files generated by ucenter.
* These files normally require ucenter to be uploaded to the chipset, however,
* if the gps has no volatile memory, a startup script is needed. To avoid having
* to use Wine or similar, this parser reads and transmits the configuration files directly
* to a gps attached to the serial port.
*
* Please note that this program does not verify successful transmission, it is recommended to
* either send the file multiple times or implement a read function for acknowledge messages.
*
*/
#include <stdio.h> // standard input / output functions
#include <fcntl.h> // File control definitions
#include <termios.h> // POSIX terminal control definitionss
#include <iostream>
#include <fstream>
#include <string.h>
#include <unistd.h>
#include <stdlib.h>
using namespace std;
int send_gps_command(unsigned char data[], int ncommands, const char* device,
unsigned int baudrate);
speed_t get_baud(int baudrate);
int configure_gps(unsigned char data[], int fd, int ncommands);
int main(int argc, char *argv[]) {
int failed_commands, succesful_commands, total_commands = 0;
/*
* Reading input parameters and checking if there are enough
*/
if (argc != 4) { //need 3 args -> dev, baud, conffile
cout
<< "Please provide device, baudrate and conffile_location by passing three arguments "
<< endl
<< "eg. ./gpsconfig /dev/ttyS0 9600 /home/user/documents/my_conffile.txt"
<< endl << "Terminating program..." << endl;
return -1;
}
/*
* Assigning input args to values
*/
const char *device = argv[1]; //device, eg. /dev/ttyUSB0
speed_t baudrate = get_baud(atoi(argv[2])); // Baudrate
printf("device: %s\n", device);
printf("baudrate: %ui", baudrate);
/*
* Open the ublox configuration file
*/
string confstring;
ifstream conffile;
string temp;
int strindex = 0;
conffile.open(argv[3]);
//conffile.open("/home/hjnyg07/Workspace/ros/stacks/gpsconfig/src/flystixconf.txt");
if (!conffile) {
cout << "error reading conf file";
return 0;
}
/*
* Set up serial port to behave as we like, using termios
*/
int fd, res;
struct termios oldtio, newtio;
char buf[255];
fd = open(device, O_RDWR | O_NOCTTY); //read write, not controlling because we don't want to get killed if noise sends CTRL-C.
if (fd < 0) { // Errorcheck
perror(device);
return (-1);
}
tcgetattr(fd, &oldtio); /* save current port settings */
bzero(&newtio, sizeof(newtio));/* clear struct for new port settings */
/*
BAUDRATE: Set bps rate
CRTSCTS : output hardware flow control (only used if the cable has
all necessary lines. See sect. 7 of Serial-HOWTO)
CS8 : 8n1 (8bit,no parity,1 stopbit)
CLOCAL : local connection, no modem contol
CREAD : enable receiving characters
*/
newtio.c_cflag = baudrate | CRTSCTS | CS8 | CLOCAL | CREAD;
newtio.c_iflag = IGNPAR;/*IGNPAR : ignore bytes with parity errors*/
newtio.c_oflag = 0;/* Raw output. */
newtio.c_lflag = 0;/* set input mode (non-canonical, no echo,...) */
newtio.c_cc[VTIME] = 0; /* inter-character timer unused */
newtio.c_cc[VMIN] = 5; /* blocking read until 5 chars received */
tcflush(fd, TCIFLUSH); /* Clean the tty line*/
tcsetattr(fd, TCSANOW, &newtio);/* Activate the settings for the port */
/*
* Everything ready, now start parsing the ublox conf file line by line
*/
while (getline(conffile, temp)) { //So long as lines remain in input file
total_commands++;
int charindex = 0;
unsigned char send_bytes[1000] = { 0 }; //init send buffer, 1000 should do... //TODO dynamic size
send_bytes[charindex++] = 0xB5; //std ublox cmd (my)
send_bytes[charindex++] = 0x62; //std ublox cmd (b)
strindex = temp.find(' ') + 3; // Find first space - to get past the text and ' - ' add 3
unsigned int CK_A = 0, CK_B = 0; // checksum containers
while (strindex < (int) temp.size()) { //Process one command line and extract values
unsigned int hex = 0;
sscanf(temp.substr(strindex, 2).c_str(), "%X", &hex); // read string as individual hex and convert to int
//printf("%s = %d\n", temp.substr(strindex, 2).c_str(), hex);
send_bytes[charindex++] = hex;
CK_A = CK_A + send_bytes[charindex - 1]; //update checksum a
CK_B = CK_B + CK_A; // Update checksum b
strindex += 3; //Move to next hex code in file-string
}
send_bytes[charindex++] = CK_A & 0xff; // mask checksum a with 8 bit
send_bytes[charindex++] = CK_B & 0xff; // mask checksum b with 8 bit
/*
* Parsing of one line done, new send command and check for ack
* If no ack is recieved after 10 tries, give up and move to next command.
*/
int count = 0;
while (configure_gps(send_bytes, fd, charindex) < 0 && count++ < 10)
;
if (count > 9) {
cout << "ERROR sending conf " << total_commands
<< " , proceeding to next cmd" << endl;
failed_commands++;
} else {
cout << "Command " << total_commands << " configured corectly"
<< endl;
succesful_commands++;
}
// while (send_gps_command(send_bytes, charindex, device, baudrate) < 0
// && count++ < 10)
// ;
// if (count > 9) {
// cout << "ERROR sending conf" << endl;
// }
//send_file_command(send_bytes, charindex);
}
cout << "Done configuring..." << endl;
cout << "Total commands: " << total_commands << endl;
cout << "Succesfull commands: " << succesful_commands << endl;
cout << "Failed commands: " << failed_commands << endl;
cout << "Now closing serial and conf files" << endl;
conffile.close(); /* Close ublox conf file */
tcsetattr(fd, TCSANOW, &oldtio);/* Set tty conf to original settings */
int myclose = 10;
myclose = close(fd); /* Close serial port */
cout << "Close status: " << myclose << endl;
usleep(1000000);
return 0;
}
int configure_gps(unsigned char data[], int fd, int ncommands) {
int res; /* number of read data */
cout << "sending: ";
for (int x = 0; x < ncommands; x++) {
printf("%02X ", data[x]);
};
cout << endl;
tcflush(fd, TCIFLUSH); /* Clean fd */
write(fd, data, ncommands); /* write the command */
fsync(fd); /* make sure everything is sent */
char buf[255]; // read buffer
/*
* Now read replys until we get an ack, or give up after 15 tries (bloody gps data spamming the line..)
*/
if (data[2] == 0x06) { // Only check ack if we have a cfg message
for (int i = 0; i < 15; i++) {
res = read(fd, buf, 255); /* Read reply. Returns after 'newtio.c_cc[VMIN]' chars have been read */
buf[res] = 0; /* so we can printf... Not necessary when not printing */
cout << "I recieved: ";
for (int x = 0; x < res; x++) {
printf("%02X ", buf[x]);
};
cout << endl;
for (int x = 0; x < (res - 4); x++) { //Search for the ack message {0xB5 0x62 0x05 0x01}
if (/* buf[x] == 0xB5 && */ buf[x + 1] == 0x62 && buf[x + 2] == 0x05
&& buf[x + 3] == 0x01) {
cout << "Bingo" << endl;
return 1;/* Succes, ack was recieved */
}
}
}
return -1; /* failed to recieve ack after 15 read tries */
}
return 1; // Non cft message sent
}
int hard_send_gps_command(unsigned char data[], int ncommands) {
int file = open("/dev/ttyUSB0", O_RDWR | O_NOCTTY); //open tty for write only
if (file < 0) {
cout << "error opening serial port, command not sent";
return -1;
}
/*
* 1hz
* 0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xE8, 0x03, 0x01, 0x00, 0x01, 0x00, 0x01, 0x39
*/
/*
* 5hz
* 0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xC8, 0x00, 0x01, 0x00, 0x01, 0x00, 0xDE, 0x6A
*/
struct termios port_settings; // structure to store the port settings in
cfsetispeed(&port_settings, B9600); // set baud rates
cfsetospeed(&port_settings, B9600);
port_settings.c_cflag &= ~PARENB; // set no parity, stop bits, data bits
port_settings.c_cflag &= ~CSTOPB;
port_settings.c_cflag &= ~CSIZE;
port_settings.c_cflag |= CS8;
port_settings.c_lflag &= (~ICANON); //
port_settings.c_cc[VMIN] = 128;
cfmakeraw(&port_settings);
tcsetattr(file, TCSANOW, &port_settings); // apply the settings to the port
// unsigned char send_bytes[] = { 0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xC8,
// 0x00, 0x01, 0x00, 0x01, 0x00, 0xDE, 0x6A };
cout << "sending: " << endl;
for (int x = 0; x < ncommands; x++) {
printf("%02X ", data[x]);
}
cout << endl;
// if (write(file, data, ncommands) != ncommands) { //Send data
// cout << "Warning, error writing configuration to device";
// };
tcflush(file, TCIFLUSH);
write(file, data, ncommands);
fsync(file);
ssize_t read_result;
//unsigned int read_result;
ssize_t nbytes = 0;
unsigned char buf[12];
buf[0] = 0;
if (data[2] == 0x06) { //Only expect ack/nck if we have a cfg message (=0x06)
int count = 10;
while (buf[0] != 0xB5 && --count > 0) { //find start of ack message
read_result = read(file, &(buf[0]), 1);
// printf("%02X : %i \n", buf[0], read_result);
}
if (count <= 0) {
printf("I got kicked..!\n");
return -1;
} else {
printf("I kicked it!\n");
}
nbytes = 1; //move to next buffer index
while (nbytes < 3) { //read rest
read_result = read(file, &(buf[nbytes]), 4 - nbytes);
if (read_result < 0) {
cout << "error reading tty : " << read_result << endl;
return -1;
}
nbytes += read_result;
}
close(file);
if (buf[1] == 0x62 && buf[2] == 0x05 && buf[3] == 0x01) {
return 0;
}
return -2;
// while (nbytes < 5) { //read rest
// cout << "inhere 1111" << endl;
// read_result = read(file, &(buf[nbytes]), 6 - nbytes);
// if (read_result < 0) {
// cout << "error reading tty : " << read_result << endl;
// return -1;
// }
// nbytes += read_result;
// }
// int length = (buf[4] & 0xFF) + ((buf[5] & 0xFF) << 8) + 2 + 6;
// cout << endl << "length " << length << endl;
// ;
// for (int i = 0; i < 12; i++) {
// printf("%02X ", buf[i]);
// }
// while (nbytes < length) { //read rest
// cout << "inhere 2222" << endl;
//
// read_result = read(file, &(buf[nbytes]), length - nbytes);
// if (read_result < 0) {
// cout << "error reading tty : " << read_result << endl;
// return -1;
// }
// nbytes += read_result;
// }
//
// unsigned int CK_A = 0, CK_B = 0;
// for (int i = 2; i < length - 2; i++) { //Check checksum
// CK_A = CK_A + buf[i]; //update checksum a
// CK_B = CK_B + CK_A; // Update checksum b
// }
// CK_A = CK_A & 0xff; // mask checksum a with 8 bit
// CK_B = CK_B & 0xff; // mask checksum b with 8 bit
// printf("cacl checksum %02X : %02X \n", CK_A, CK_B);
// printf("expect %02X : %02X \n", buf[length - 2], buf[length - 1]);
// cout << "received: ";
// for (int i = 0; i < length; i++) {
// printf("%02X ", buf[i]);
// }
// cout << endl;
//
// if (CK_A == buf[length - 2] && CK_B == buf[length - 1]
// && buf[3] == 0x01) {
// cout << "succes" << endl;
// return 0;
// } else {
// cout << "erreor" << endl;
// return -2;
// }
// cout << "Read: " << endl;
// for (unsigned int x = 0; x < sizeof(buffer); x++) {
// cout << "tests" << endl;
// printf("%02X ", *buffer[x]);
// }
// cout << endl;
}
}
int send_gps_command(unsigned char data[], int ncommands, const char* device,
unsigned int baudrate) {
int file = open(device, O_RDWR | O_NOCTTY); //open tty for write only
if (file < 0) {
cout << "error opening serial port, command not sent";
return -1;
}
/*
* 1hz
* 0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xE8, 0x03, 0x01, 0x00, 0x01, 0x00, 0x01, 0x39
*/
/*
* 5hz
* 0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xC8, 0x00, 0x01, 0x00, 0x01, 0x00, 0xDE, 0x6A
*/
struct termios port_settings; // structure to store the port settings in
cfsetispeed(&port_settings, baudrate); // set baud rates
cfsetospeed(&port_settings, baudrate);
port_settings.c_cflag &= ~PARENB; // set no parity, stop bits, data bits
port_settings.c_cflag &= ~CSTOPB;
port_settings.c_cflag &= ~CSIZE;
port_settings.c_cflag |= CS8;
port_settings.c_lflag &= (~ICANON); //
// port_settings.c_cc[VMIN] = 128;
port_settings.c_cc[VMIN] = 1;
/*Experiment*/
// port_settings.c_cflag = baudrate | CRTSCTS | CS8 | CLOCAL | CREAD;
// port_settings.c_iflag = IGNPAR | ICRNL;
// port_settings.c_oflag = 0;
// port_settings.c_lflag = ICANON;
// port_settings.c_cc[VMIN] = 1;
// port_settings.c_cc[VTIME] = 0;
// tcflush(file, TCIFLUSH);
// tcsetattr(file, TCSANOW, &port_settings);
/*END experiment*/
cfmakeraw(&port_settings);
tcsetattr(file, TCSANOW, &port_settings); // apply the settings to the port
// unsigned char send_bytes[] = { 0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xC8,
// 0x00, 0x01, 0x00, 0x01, 0x00, 0xDE, 0x6A };
cout << "sending: " << endl;
for (int x = 0; x < ncommands; x++) {
printf("%02X ", data[x]);
}
cout << endl;
// if (write(file, data, ncommands) != ncommands) { //Send data
// cout << "Warning, error writing configuration to device";
// };
cout << "before flush" << endl;
tcflush(file, TCIFLUSH);
cout << "after flush, before write" << endl;
write(file, data, ncommands);
cout << "after write, before sync" << endl;
fsync(file);
cout << "after sync" << endl;
ssize_t read_result;
//unsigned int read_result;
ssize_t nbytes = 0;
unsigned char buf[12];
buf[0] = 0;
cout << "Before data[2] == 0x06 check" << endl;
if (data[2] == 0x06) { //Only expect ack/nck if we have a cfg message (=0x06)
int count = 10;
cout << "before while buf[0] != 0xB5 && --count > 0" << endl;
while (buf[0] != 0xB5 && --count > 0) { //find start of ack message
cout << "count: " << count << endl;
read_result = read(file, &(buf[0]), 1);
// printf("%02X : %i \n", buf[0], read_result);
}
cout << "after data[2] == 0x06" << endl;
if (count <= 0) {
printf("I got kicked..!\n");
return -1;
} else {
printf("I kicked it!\n");
}
nbytes = 1; //move to next buffer index
while (nbytes < 3) { //read rest
read_result = read(file, &(buf[nbytes]), 4 - nbytes);
if (read_result < 0) {
cout << "error reading tty : " << read_result << endl;
return -1;
}
nbytes += read_result;
}
close(file);
if (buf[1] == 0x62 && buf[2] == 0x05 && buf[3] == 0x01) {
return 0;
}
return -2;
// while (nbytes < 5) { //read rest
// cout << "inhere 1111" << endl;
// read_result = read(file, &(buf[nbytes]), 6 - nbytes);
// if (read_result < 0) {
// cout << "error reading tty : " << read_result << endl;
// return -1;
// }
// nbytes += read_result;
// }
// int length = (buf[4] & 0xFF) + ((buf[5] & 0xFF) << 8) + 2 + 6;
// cout << endl << "length " << length << endl;
// ;
// for (int i = 0; i < 12; i++) {
// printf("%02X ", buf[i]);
// }
// while (nbytes < length) { //read rest
// cout << "inhere 2222" << endl;
//
// read_result = read(file, &(buf[nbytes]), length - nbytes);
// if (read_result < 0) {
// cout << "error reading tty : " << read_result << endl;
// return -1;
// }
// nbytes += read_result;
// }
//
// unsigned int CK_A = 0, CK_B = 0;
// for (int i = 2; i < length - 2; i++) { //Check checksum
// CK_A = CK_A + buf[i]; //update checksum a
// CK_B = CK_B + CK_A; // Update checksum b
// }
// CK_A = CK_A & 0xff; // mask checksum a with 8 bit
// CK_B = CK_B & 0xff; // mask checksum b with 8 bit
// printf("cacl checksum %02X : %02X \n", CK_A, CK_B);
// printf("expect %02X : %02X \n", buf[length - 2], buf[length - 1]);
// cout << "received: ";
// for (int i = 0; i < length; i++) {
// printf("%02X ", buf[i]);
// }
// cout << endl;
//
// if (CK_A == buf[length - 2] && CK_B == buf[length - 1]
// && buf[3] == 0x01) {
// cout << "succes" << endl;
// return 0;
// } else {
// cout << "erreor" << endl;
// return -2;
// }
// cout << "Read: " << endl;
// for (unsigned int x = 0; x < sizeof(buffer); x++) {
// cout << "tests" << endl;
// printf("%02X ", *buffer[x]);
// }
// cout << endl;
}
}
void send_file_command(unsigned char data[], int ncommands) { //Print output to a file
ofstream myfile;
char buf[5];
myfile.open("/home/hjnyg07/Workspace/ros/stacks/gpsconfig/src/test",
ios::app);
for (int i = 0; i < ncommands; i++) {
sprintf(buf, "%02X ", data[i]);
myfile << buf;
}
myfile << endl;
myfile.close();
}
speed_t get_baud(int baudrate) {
switch (baudrate) {
case 0:
return B0;
case 50:
return B50;
case 75:
return B75;
case 110:
return B110;
case 134:
return B134;
case 150:
return B150;
case 200:
return B200;
case 300:
return B300;
case 600:
return B600;
case 1200:
return B1200;
case 1800:
return B1800;
case 2400:
return B2400;
case 4800:
return B4800;
case 9600:
cout << "baud set correctly to 9600";
return B9600;
case 19200:
return B19200;
case 38400:
return B38400;
case 57600:
return B57600;
case 115200:
return B115200;
default:
return B9600;
}
}
