|
Putting the whole IO subsystem in a class is a good idea, if you later decide to add a simulator you can simply fake this class.
To compile: g++ io.cpp -c
io.h
#define IO_H
#include <termios.h> // serial
typedef struct s_gps {
double lon; // - = south, in metric
double lat; // - = west, in metric
float speed; // in m/s
float bearing; // compass heading
double time; // time of fix, seconds since 1970
};
class IO {
public:
IO(); // Constructor
~IO(); // Destructor
void SendSerial(unsigned char *val, int length); // send a char
s_gps DecodeGPS(char *val); // decode the gps packet off the serial line
s_gps GetGPS() { return cur_pos; } // return the current GPS location
void RecvSerial(char val); // recieve a single serial char ** don't forget \n\r when calling manually
char* GetLastSerial() { return last_buffer; } // returns the rolling serial buffer (for debugging mostly)
void RecvSerialStr(char* buf); // fakes serial input to io, for debugging
private:
int fd_serial;
struct termios oldtty; // will be used to save old port settings
int GPSGoodData(char *val, char *checksum); // check the checksum
struct s_gps cur_pos; // current position
char serial_buffer[256];
char last_buffer[256];
int serial_index;
};
#endif
io.cppusing namespace std;
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <unistd.h> // serial
#include <termios.h> // serial
#include <time.h> // mktime, struct tm
#include <string.h> // strncmp
#include <math.h> // modf
#include "io.h"
#define SERIALPORT "/dev/ttyS0"
#define BAUDRATE B9600
IO::IO() {
int res;
struct termios tty; // will be used for new port settings
char buf[255]; // buffer used to store received characters
serial_index = 0;
// set up serial port
fd_serial = open(SERIALPORT, O_RDWR | O_NOCTTY );
if (fd_serial < 0) {
cout << "IO::InitServo: Unable to write to serial port (" << SERIALPORT << "), are you root?" << endl;
exit(-1);
}
tcgetattr(fd_serial, &oldtty); // save current port settings
bzero(&tty, sizeof(tty)); // InitServoialize the port settings structure to all zeros
// then set the baud rate, handshaking and a few other settings
tty.c_iflag = 0;
tty.c_lflag = 0; // set input mode (non-canonical, no echo,.)
tty.c_oflag = 0;
tty.c_cflag = BAUDRATE | CS8 | CLOCAL;
tty.c_cc[VTIME] = 0; // inter-character timer unused
tty.c_cc[VMIN] = 1; // blocking read until first char received
tcflush(fd_serial, TCIFLUSH);
tcsetattr(fd_serial, TCSANOW, &tty);
}
IO::~IO() {
tcsetattr(fd_serial, TCSANOW, &oldtty); // restore the old port settings before quitting
}
void IO::SendSerial(unsigned char *val, int length) {
// :: Send a serial byte down the line ::
write(fd_serial, val, length);
}
s_gps IO::DecodeGPS(char *val) { // decode the GPS packet and return the struct
s_gps data; // gps packet
if (GPSGoodData(val, "")) { // error messages are handled inside gpsgooddata
if (strncmp(val, "", 6) == 0) {
// ,3354.4970,N,11759.5354,W,025604,V,S*52 lat/lon; V(a=valid, v=invalid)
// 0 1 2 3 4 5 6 7
double lat_deg_nmea, lon_deg_nmea;
char lat_dir, lon_dir;
int hms;
char valid;
struct tm tm;
sscanf(val, ",%lf,%c,%lf,%c,%d,%c,", &lat_deg_nmea, &lat_dir, &lon_deg_nmea, &lon_dir, &hms, &valid);
tm.tm_sec = hms % 100;
hms = hms / 100;
tm.tm_min = hms % 100;
hms = hms / 100;
tm.tm_hour = hms % 100;
data.time = mktime(&tm) + time(0);
// lat_deg is in the format ddmm.mmmm
double grades, frac;
frac = modf(lat_deg_nmea / 100.0, &grades);
data.lat = (double)(grades + frac * 100.0 / 60.0) * (lat_dir == 'S' ? -1.0 : 1.0);
frac = modf(lon_deg_nmea/ 100.0, &grades);
data.lon = (double)(grades + frac * 100.0 / 60.0) * (lon_dir == 'W' ? -1.0 : 1.0);
} else if (strncmp(val, "", 6) == 0) {
// ,045.3,T,044.2,M,,*47 bearing, origin to destination
// 0 1 2 3 4 56
float bearing;
sscanf(val, ",%f,", &bearing);
data.bearing = bearing;
}
}
return data;
}
int IO::GPSGoodData(char *val, char *checksum) {
// NMEA 0183 sentences begin with $ and and with CR LF
if (val[0] != '$') {
return false;
}
// Next to last character must be a CR
if (val[strlen(val)-2] != '\n') {
return false;
}
if (val[strlen(val)-1] != '\r') {
return false;
}
// todo: check checksum if present
return true;
}
void IO::RecvSerial(char val) { // ** don't forget
when calling manually
s_gps data; // gps packet
// recieve a single serial char
if (serial_index > sizeof(serial_buffer)) {
serial_index = 0; // reset buffer on overflow, dump the packet
}
// add to buffer
if ((serial_index > 0 || val == '$') && val != '\n' && val != '\r') { // wait for $ to start line, if at start
serial_buffer[serial_index] = val;
serial_index++;
}
// if newline, decode and update
if (val == '\r' || val == 0) {
strncpy(last_buffer, serial_buffer, strlen(serial_buffer));
serial_buffer[serial_index] = '\n';
serial_buffer[serial_index+1] = '\r';
data = DecodeGPS(serial_buffer);
if (data.bearing != 0) { cur_pos.bearing = data.bearing; }
// todo: make work for different packet types
serial_index = 0;
}
}
void IO::RecvSerialStr(char* buf) {
// used to emulate serial input, only used in debugging
int i;
for (i = 0; i < strlen(buf); i++) {
RecvSerial(buf[i]);
}
}
|
