int main(int argc, char **argv)
{
if (argc > 1) {
openserial(argv[1]);
} else {
openserial(SERIAL_PORT);
}
setDTR(1);
return 0;
}
Xbee::Xbee (char *devPath) {
serial_d = openserial(devPath);
// ----- Begin of setup serial ports
tcgetattr(serial_d, &soptions_org);
tcgetattr(serial_d, &soptions);
speed = B9600; // Speed options: B19200, B38400, B57600, B115200
cfsetispeed(&soptions, speed);
cfsetospeed(&soptions, speed);
// Enable the reciver and set local mode...
soptions.c_cflag |= ( CLOCAL | CREAD );
// Setting Parity Checking (8N1)
soptions.c_cflag &= ~PARENB;
soptions.c_cflag &= ~CSTOPB;
soptions.c_cflag &= ~CSIZE;
soptions.c_cflag |= CS8;
// Local setting
//soptions.c_lflag = (ICANON | ECHO | ECHOE); //canonical
soptions.c_lflag = ~(ICANON | ECHO | ECHOE | ISIG); //noncanonical
// Input setting
//soptions.c_iflag |= (IXON | IXOFF | IXANY); //software flow control
soptions.c_iflag |= (INPCK | ISTRIP);
soptions.c_iflag = IGNPAR;
// Output setting
soptions.c_oflag = 0;
soptions.c_oflag &= ~OPOST;
// Read options
soptions.c_cc[VTIME] = 0;
soptions.c_cc[VMIN] = 1; //transfer at least 1 character or block
// Apply setting
tcsetattr(serial_d, TCSANOW, &soptions);
// ----- End of setup serial ports
// restore setting and close
//tcsetattr(serial_d, TCSANOW, &soptions_org);
//close(serial_d);
//return 0;
}
int main (int argc, char **argv) {
int i;
int serial_d;
speed_t speed;
struct termios soptions, soptions_org;;
unsigned char sensor_index = 0+'0'; // indexes are ASCII-based
char wCommand;
unsigned char send_buf[BUFLEN];
unsigned char recv_buf[BUFLEN];
int acc_x, acc_y, acc_z;
int data_x, data_y, data_z;
float data_TSI, theta;
unsigned int num_recv_c, num_send_c;
unsigned int timecycle = 0;
FILE *xdata_fp;
int file_index = 0; // file extensions: .1, .2,...
const int sample = 30; // output file will contain 1*sample points
const char *filebase = "xdata.";
const int num_file = 2;
char filename[BUFLEN];
// no port specified
if (argc == 1) {
printf("Usage: zstar3-test sensor_index port: \"zstar3-test 0 /dev/ttyACM0\"\n");
printf("\"sensor_index\" is used to select sensors if multiple sensors are detected by USB transceiver.\n");
printf("\"/dev/ttyACM0\" is a USB serial device (cdc_acm kernel module) to support serial control to modems.\n");
return 1;
}
sensor_index = argv[1][0]; // 0~15(F)
if ((serial_d = openserial(argv[2])) == -1) return 1;
// ----- Begin of setup serial ports
tcgetattr(serial_d, &soptions_org);
tcgetattr(serial_d, &soptions);
speed = B115200; // Speed options: B19200, B38400, B57600, B115200
cfsetispeed(&soptions, speed);
cfsetospeed(&soptions, speed);
// Enable the reciver and set local mode...
soptions.c_cflag |= ( CLOCAL | CREAD );
// Setting Parity Checking (8N1)
soptions.c_cflag &= ~PARENB;
soptions.c_cflag &= ~CSTOPB;
soptions.c_cflag &= ~CSIZE;
soptions.c_cflag |= CS8;
// Local setting
// soptions.c_lflag = (ICANON | ECHO | ECHOE); // canonical
soptions.c_lflag = ~(ICANON | ECHO | ECHOE | ISIG); // noncanonical
// Input setting
// soptions.c_iflag |= (IXON | IXOFF | IXANY); // software flow control
soptions.c_iflag |= (INPCK | ISTRIP);
soptions.c_iflag = IGNPAR;
// Output setting
soptions.c_oflag = 0;
soptions.c_oflag &= ~OPOST;
// Read options
soptions.c_cc[VTIME] = 0;
soptions.c_cc[VMIN] = 1; // transfer at least 1 character or block
// Apply setting
tcsetattr(serial_d, TCSANOW, &soptions);
// ----- End of setup serial ports
memset(recv_buf, '\0', BUFLEN);
tcflush(serial_d, TCIOFLUSH);
usleep(10000);
printf("Start Measuring...\n");
while (1) {
sprintf(filename, "%s%d\0", filebase, file_index);
if (xdata_fp = fopen(filename, "w")) {
fprintf(xdata_fp, "{\"time\":%d,\n\"xarray\":[\n", timecycle);
for (i=0; i<sample; i++) {
wCommand = 'V';
num_send_c = write(serial_d, &wCommand, 1);
tcdrain(serial_d);
memset(recv_buf, '\0', BUFLEN); // receive data: Wait for 3-axis ready
num_recv_c = read(serial_d, &recv_buf[0], BUFLEN);
tcdrain(serial_d);
sscanf(recv_buf, " %d %d %d %f,", &data_x, &data_y, &data_z, &data_TSI);
acc_x = data_x;
acc_y = data_y;
acc_z = data_z;
theta = acos(trunc_norm(acc_z, acc_z_abs_max)) * 180 / PI;
#ifdef DEBUG
printf("X command receive: (%d %d %d %f %f) t=%d\n", data_x, data_y, data_z, theta, data_TSI, timecycle);
#endif
if (i != sample-1) {
fprintf(xdata_fp, "[%d, %d, %d, %f],\n", acc_x, acc_y, acc_z, data_TSI);
} else { // Last sample does not need a trailing comma for JSON format
fprintf(xdata_fp, "[%d, %d, %d, %f] \n", acc_x, acc_y, acc_z, data_TSI);
}
usleep(8000);
}
// Write postscript to file
fprintf(xdata_fp, "]\n}\n");
fclose(xdata_fp);
} else {
printf("Can not open \"%s\"", filename);
}
file_index = (file_index+1) % num_file; // cycle to next file index
timecycle++; // time counter
}
wCommand = 'x'; //Send 'x' to stop burst mode
num_send_c = write(serial_d, &wCommand, 1);
tcdrain(serial_d);
memset(recv_buf, '\0', BUFLEN);
num_recv_c = read(serial_d, recv_buf, 1);
tcdrain(serial_d);
// restore setting and close
tcsetattr(serial_d, TCSANOW, &soptions_org);
close(serial_d);
return 0;
}
int main(int argc, char *argv[])
{
int speed = B115200;
const char *device = "/dev/ttyUSB0";
unsigned char x;
int fd, n;
unsigned char buf[32768];
unsigned do_erase = 0;
unsigned do_write = 0;
unsigned do_exec = 0;
unsigned addr = 0;
if (argc < 2)
return usage();
if (!strcmp(argv[1],"erase")) {
do_erase = 1;
} else if (!strcmp(argv[1],"flash")) {
do_erase = 1;
do_write = 1;
addr = 0x08000000;
} else if (!strcmp(argv[1],"exec")) {
do_write = 1;
do_exec = 1;
addr = 0x20001000;
} else {
return usage();
}
if (do_write && argc != 3)
return usage();
fd = openserial(device, speed);
if (fd < 0) {
fprintf(stderr, "stderr open '%s'\n", device);
return -1;
}
n = TIOCM_DTR;
ioctl(fd, TIOCMBIS, &n);
usleep(2500);
ioctl(fd, TIOCMBIC, &n);
usleep(2500);
/* If the board just powered up, we need to send an ACK
* to auto-baud and will get an ACK back. If the board
* is already up, two ACKs will get a NAK (invalid cmd).
* Either way, we're talking!
*/
for (n = 0; n < 5; n++) {
unsigned char SYNC = 0x7F;
if (write(fd, &SYNC, 1)) { /* do nothing */ }
if (read(fd, &x, 1) != 1)
continue;
if ((x == 0x79) || (x == 0x1f))
break;
}
if (n == 5) {
fprintf(stderr,"sync failure\n");
return -1;
}
#if 0
readMemory(fd, 0x1FFFF000, buf, 4096);
for (n = 0; n < 1024; n++)
fprintf(stderr,"%02x ", buf[n]);
return 0;
#endif
if (do_write) {
int fd2 = open(argv[2], O_RDONLY);
n = read(fd2, buf, sizeof(buf));
close(fd2);
if ((fd2 < 0) || (n <= 0)) {
fprintf(stderr,"cannot read '%s'\n", argv[2]);
return -1;
}
n += (n % 4);
if (do_erase) {
fprintf(stderr,"erasing flash...\n");
if (eraseFlash(fd)) {
fprintf(stderr,"erase failed\n");
return -1;
}
}
fprintf(stderr,"sending %d bytes...\n", n);
if (writeMemory(fd, addr, buf, n)) {
fprintf(stderr,"write failed\n");
return -1;
}
fprintf(stderr,"done\n");
if (do_exec) {
jumpToAddress(fd, addr);
} else {
return 0;
}
} else if (do_erase) {
if (eraseFlash(fd)) {
fprintf(stderr,"erase failed\n");
return -1;
}
fprintf(stderr,"flash erased\n");
return 0;
}
for (;;) {
if (read(fd, &x, 1) == 1) {
if (x == 27) break;
if ((x < 0x20) || (x > 0x7f))
if ((x != 10) && (x != 13))
x = '.';
fprintf(stderr,"%c", x);
}
}
return 0;
}
int main(int argc, char** argv) {
/// Serial port full path to open
char *serialport = NULL;
/// Database file to open
char *databasename = NULL;
/// List of columsn to log
char *log_columns = NULL;
/// Number of samples to take
int samplecount = -1;
/// Number of samples per second
int samplespersecond = 1;
/// Ask to show the capabilities of the OBD device then exit
int showcapabilities = 0;
/// Set if the user wishes to upgrade the baudrate
long baudrate_upgrade = -1;
/// Time between samples, measured in microseconds
long frametime = 0;
/// Spam all readings to stdout
int spam_stdout = 0;
/// Enable elm optimisations
int enable_optimisations = 0;
/// Enable serial logging
int enable_seriallog = 0;
/// Serial log filename
char *seriallogname = NULL;
#ifdef OBDPLATFORM_POSIX
/// Daemonise
int daemonise = 0;
#endif //OBDPLATFORM_POSIX
/// Requested baudrate
long requested_baud = -1;
// Config File
struct OBDGPSConfig *obd_config = obd_loadConfig(0);
if(NULL != obd_config) {
samplespersecond = obd_config->samplerate;
enable_optimisations = obd_config->optimisations;
requested_baud = obd_config->baudrate;
baudrate_upgrade = obd_config->baudrate_upgrade;
}
// Do not attempt to buffer stdout at all
setvbuf(stdout, (char *)NULL, _IONBF, 0);
int optc;
int mustexit = 0;
while ((optc = getopt_long (argc, argv, shortopts, longopts, NULL)) != -1) {
switch (optc) {
case 'h':
printhelp(argv[0]);
mustexit = 1;
break;
case 'v':
printversion();
mustexit = 1;
break;
case 's':
if(NULL != serialport) {
free(serialport);
}
serialport = strdup(optarg);
break;
case 'o':
enable_optimisations = 1;
break;
case 't':
spam_stdout = 1;
break;
case 'u':
{
int newout = open(optarg, O_CREAT|O_RDWR|O_APPEND,
S_IRUSR|S_IWUSR|S_IRGRP|S_IROTH);
if(-1 == newout) {
perror(optarg);
} else {
printf("Redirecting output to %s\n", optarg);
close(STDOUT_FILENO);
close(STDERR_FILENO);
dup2(newout, STDOUT_FILENO);
dup2(newout, STDERR_FILENO);
}
}
break;
#ifdef OBDPLATFORM_POSIX
case 'm':
daemonise = 1;
break;
#endif //OBDPLATFORM_POSIX
case 'c':
samplecount = atoi(optarg);
break;
case 'b':
requested_baud = strtol(optarg, (char **)NULL, 10);
break;
case 'B':
baudrate_upgrade = strtol(optarg, (char **)NULL, 10);
break;
case 'd':
if(NULL != databasename) {
free(databasename);
}
databasename = strdup(optarg);
break;
case 'i':
if(NULL != log_columns) {
free(log_columns);
}
log_columns = strdup(optarg);
break;
case 'a':
samplespersecond = atoi(optarg);
break;
case 'l':
enable_seriallog = 1;
if(NULL != seriallogname) {
free(seriallogname);
}
seriallogname = strdup(optarg);
break;
case 'p':
showcapabilities = 1;
break;
default:
mustexit = 1;
break;
}
}
if(mustexit) exit(0);
if(0 >= samplespersecond) {
frametime = 0;
} else {
frametime = 1000000 / samplespersecond;
}
if(NULL == serialport) {
if(NULL != obd_config && NULL != obd_config->obd_device) {
serialport = strdup(obd_config->obd_device);
} else {
serialport = strdup(OBD_DEFAULT_SERIALPORT);
}
}
if(NULL == databasename) {
if(NULL != obd_config && NULL != obd_config->log_file) {
databasename = strdup(obd_config->log_file);
} else {
databasename = strdup(OBD_DEFAULT_DATABASE);
}
}
if(NULL == log_columns) {
if(NULL != obd_config && NULL != obd_config->log_columns) {
log_columns = strdup(obd_config->log_columns);
} else {
log_columns = strdup(OBD_DEFAULT_COLUMNS);
}
}
if(enable_seriallog && NULL != seriallogname) {
startseriallog(seriallogname);
}
// Open the serial port.
int obd_serial_port = openserial(serialport, requested_baud, baudrate_upgrade);
if(-1 == obd_serial_port) {
fprintf(stderr, "Couldn't open obd serial port. Attempting to continue.\n");
} else {
fprintf(stderr, "Successfully connected to serial port. Will log obd data\n");
}
// Just figure out our car's OBD port capabilities and print them
if(showcapabilities) {
printobdcapabilities(obd_serial_port);
printf("\n");
/*
unsigned int retvals[50];
int vals_returned;
getobderrorcodes(obd_serial_port,
retvals, sizeof(retvals)/sizeof(retvals[0]), &vals_returned);
int q = 0;
int c = retvals[0];
for(q=1;q<1+2*c && q+1<vals_returned;q+=2) {
printf("Error: %s\n", obderrconvert(retvals[q], retvals[q+1]));
}
*/
closeserial(obd_serial_port);
exit(0);
}
#ifdef HAVE_GPSD
// Open the gps device
struct gps_data_t *gpsdata;
gpsdata = opengps(GPSD_ADDR, GPSD_PORT);
if(NULL == gpsdata) {
fprintf(stderr, "Couldn't open gps port on startup.\n");
} else {
fprintf(stderr, "Successfully connected to gpsd. Will log gps data\n");
}
#endif //HAVE_GPSD
if(-1 == obd_serial_port
#ifdef HAVE_GPSD
&& NULL == gpsdata
#endif //HAVE_GPSD
) {
fprintf(stderr, "Couldn't find either gps or obd to log. Exiting.\n");
exit(1);
}
#ifdef HAVE_DBUS
obdinitialisedbus();
#endif //HAVE_DBUS
// sqlite database
sqlite3 *db;
// sqlite statement
sqlite3_stmt *obdinsert;
// number of columns in the insert
int obdnumcols;
// sqlite return status
int rc;
// Open the database and create the obd table
if(NULL == (db = opendb(databasename))) {
closeserial(obd_serial_port);
exit(1);
}
// Disable sqlite's synchronous pragma.
/* char *zErrMsg;
rc = sqlite3_exec(db, "PRAGMA synchronous=OFF",
NULL, NULL, &zErrMsg);
if(rc != SQLITE_OK) {
printf("SQLite error %i: %s\n", rc, zErrMsg);
sqlite3_free(zErrMsg);
} */
// Wishlist of commands from config file
struct obdservicecmd **wishlist_cmds = NULL;
obd_configCmds(log_columns, &wishlist_cmds);
void *obdcaps = getobdcapabilities(obd_serial_port,wishlist_cmds);
obd_freeConfigCmds(wishlist_cmds);
wishlist_cmds=NULL;
createobdtable(db,obdcaps);
// Create the insert statement. On success, we'll have the number of columns
if(0 == (obdnumcols = createobdinsertstmt(db,&obdinsert, obdcaps)) || NULL == obdinsert) {
closedb(db);
closeserial(obd_serial_port);
exit(1);
}
createtriptable(db);
createecutable(db);
// All of these have obdnumcols-1 since the last column is time
int cmdlist[obdnumcols-1]; // Commands to send [index into obdcmds_mode1]
int i,j;
for(i=0,j=0; i<sizeof(obdcmds_mode1)/sizeof(obdcmds_mode1[0]); i++) {
if(NULL != obdcmds_mode1[i].db_column) {
if(isobdcapabilitysupported(obdcaps,i)) {
cmdlist[j] = i;
j++;
}
}
}
freeobdcapabilities(obdcaps);
// We create the gps table even if gps is disabled, so that other
// SQL commands expecting the table to at least exist will work.
// sqlite statement
sqlite3_stmt *gpsinsert;
// number of columns in the insert
int gpsnumcols;
creategpstable(db);
if(0 == (gpsnumcols = creategpsinsertstmt(db, &gpsinsert) || NULL == gpsinsert)) {
closedb(db);
closeserial(obd_serial_port);
exit(1);
}
#ifdef OBDPLATFORM_POSIX
if(daemonise) {
if(0 != obddaemonise()) {
fprintf(stderr,"Couldn't daemonise, exiting\n");
closeserial(obd_serial_port);
exit(1);
}
}
#endif //OBDPLATFORM_POSIX
#ifdef HAVE_GPSD
// Ping a message to stdout the first time we get
// enough of a satellite lock to begin logging
int have_gps_lock = 0;
#endif //HAVE_GPSD
install_signalhandlers();
// The current thing returned by starttrip
sqlite3_int64 currenttrip = 0;
// Set when we're actually inside a trip
int ontrip = 0;
// The current time we're inserting
double time_insert;
// The last time we tried to check the gps daemon
double time_lastgpscheck = 0;
// Number of samples per transaction
const int basetransactioncount = TRANSACTIONTIME * (0==samplespersecond?10:samplespersecond);
// Store a few seconds worth of samples per transaction
int transactioncount = 0;
obdbegintransaction(db);
while(samplecount == -1 || samplecount-- > 0) {
struct timeval starttime; // start time through loop
struct timeval endtime; // end time through loop
struct timeval selecttime; // =endtime-starttime [for select()]
if(0 != gettimeofday(&starttime,NULL)) {
perror("Couldn't gettimeofday");
break;
}
#ifdef HAVE_DBUS
enum obd_dbus_message msg_ret;
while(OBD_DBUS_NOMESSAGE != (msg_ret = obdhandledbusmessages())) {
switch(msg_ret) {
case OBD_DBUS_STARTTRIP:
if(!ontrip) {
currenttrip = starttrip(db, time_insert);
fprintf(stderr,"Created a new trip (%i)\n", (int)currenttrip);
ontrip = 1;
}
break;
case OBD_DBUS_NOMESSAGE:
default:
break;
}
}
#endif //HAVE_DBUS
time_insert = (double)starttime.tv_sec+(double)starttime.tv_usec/1000000.0f;
if(sig_starttrip) {
if(ontrip) {
fprintf(stderr,"Ending current trip\n");
updatetrip(db, currenttrip, time_insert);
ontrip = 0;
}
currenttrip = starttrip(db, time_insert);
fprintf(stderr,"Created a new trip (%i)\n", (int)currenttrip);
ontrip = 1;
sig_starttrip = 0;
}
enum obd_serial_status obdstatus;
if(-1 < obd_serial_port) {
// Get all the OBD data
for(i=0; i<obdnumcols-1; i++) {
float val;
unsigned int cmdid = obdcmds_mode1[cmdlist[i]].cmdid;
int numbytes = enable_optimisations?obdcmds_mode1[cmdlist[i]].bytes_returned:0;
OBDConvFunc conv = obdcmds_mode1[cmdlist[i]].conv;
obdstatus = getobdvalue(obd_serial_port, cmdid, &val, numbytes, conv);
if(OBD_SUCCESS == obdstatus) {
#ifdef HAVE_DBUS
obddbussignalpid(&obdcmds_mode1[cmdlist[i]], val);
#endif //HAVE_DBUS
if(spam_stdout) {
printf("%s=%f\n", obdcmds_mode1[cmdlist[i]].db_column, val);
}
sqlite3_bind_double(obdinsert, i+1, (double)val);
// printf("cmd: %02X, val: %f\n",obdcmds_mode1[cmdlist[i]].cmdid,val);
} else {
break;
}
}
if(obdstatus == OBD_SUCCESS) {
// If they're not on a trip but the engine is going, start a trip
if(0 == ontrip) {
printf("Creating a new trip\n");
currenttrip = starttrip(db, time_insert);
ontrip = 1;
}
sqlite3_bind_double(obdinsert, i+1, time_insert);
sqlite3_bind_int64(obdinsert, i+2, currenttrip);
// Do the OBD insert
rc = sqlite3_step(obdinsert);
if(SQLITE_DONE != rc) {
printf("sqlite3 obd insert failed(%i): %s\n", rc, sqlite3_errmsg(db));
}
} else if(OBD_ERROR == obdstatus) {
fprintf(stderr, "Received OBD_ERROR from serial read. Exiting\n");
receive_exitsignal = 1;
} else {
// If they're on a trip, and the engine has desisted, stop the trip
if(ontrip) {
printf("Ending current trip\n");
updatetrip(db, currenttrip, time_insert);
ontrip = 0;
}
}
sqlite3_reset(obdinsert);
}
// Constantly update the trip
updatetrip(db, currenttrip, time_insert);
#ifdef HAVE_GPSD
// Get the GPS data
double lat,lon,alt,speed,course,gpstime;
int gpsstatus = -1;
if(NULL != gpsdata) {
gpsstatus = getgpsposition(gpsdata, &lat, &lon, &alt, &speed, &course, &gpstime);
} else {
if(time_insert - time_lastgpscheck > 10) { // Try again once in a while
gpsdata = opengps(GPSD_ADDR, GPSD_PORT);
if(NULL != gpsdata) {
printf("Delayed connection to gps achieved\n");
} else {
// fprintf(stderr, "Delayed connection to gps failed\n");
}
time_lastgpscheck = time_insert;
}
}
if(gpsstatus < 0 || NULL == gpsdata) {
// Nothing yet
} else if(gpsstatus >= 0) {
if(0 == have_gps_lock) {
fprintf(stderr,"GPS acquisition complete\n");
have_gps_lock = 1;
}
sqlite3_bind_double(gpsinsert, 1, lat);
sqlite3_bind_double(gpsinsert, 2, lon);
if(gpsstatus >= 1) {
sqlite3_bind_double(gpsinsert, 3, alt);
} else {
sqlite3_bind_null(gpsinsert, 3);
}
sqlite3_bind_double(gpsinsert, 4, speed);
sqlite3_bind_double(gpsinsert, 5, course);
sqlite3_bind_double(gpsinsert, 6, gpstime);
if(spam_stdout) {
printf("gpspos=%f,%f,%f,%f,%f\n",
lat, lon, (gpsstatus>=1?alt:-1000.0), speed, course);
}
// Use time worked out before.
// This makes table joins reliable, but the time itself may be wrong depending on gpsd lagginess
sqlite3_bind_double(gpsinsert, 7, time_insert);
sqlite3_bind_int64(gpsinsert, 8, currenttrip);
// Do the GPS insert
rc = sqlite3_step(gpsinsert);
if(SQLITE_DONE != rc) {
printf("sqlite3 gps insert failed(%i): %s\n", rc, sqlite3_errmsg(db));
}
sqlite3_reset(gpsinsert);
}
#endif //HAVE_GPSD
if(0 != gettimeofday(&endtime,NULL)) {
perror("Couldn't gettimeofday");
break;
}
// Set via the signal handler
if(receive_exitsignal) {
break;
}
// Commit this if we've done more than a certain number
transactioncount++;
transactioncount%=basetransactioncount;
if(0 == transactioncount) {
obdcommittransaction(db);
obdbegintransaction(db);
}
// usleep() not as portable as select()
if(0 < frametime) {
selecttime.tv_sec = endtime.tv_sec - starttime.tv_sec;
if (selecttime.tv_sec != 0) {
endtime.tv_usec += 1000000*selecttime.tv_sec;
selecttime.tv_sec = 0;
}
selecttime.tv_usec = (frametime) -
(endtime.tv_usec - starttime.tv_usec);
if(selecttime.tv_usec < 0) {
selecttime.tv_usec = 1;
}
select(0,NULL,NULL,NULL,&selecttime);
}
}
obdcommittransaction(db);
if(0 != ontrip) {
updatetrip(db, currenttrip, time_insert);
ontrip = 0;
}
sqlite3_finalize(obdinsert);
sqlite3_finalize(gpsinsert);
closeserial(obd_serial_port);
#ifdef HAVE_GPSD
if(NULL != gpsdata) {
gps_close(gpsdata);
}
#endif //HAVE_GPSD
closedb(db);
if(enable_seriallog) {
closeseriallog();
}
if(NULL != log_columns) free(log_columns);
if(NULL != databasename) free(databasename);
if(NULL != serialport) free(serialport);
obd_freeConfig(obd_config);
return 0;
}
int main (int argc, char *argv[]) {
int serial_d;
speed_t speed;
struct termios soptions, soptions_org;
char command;
unsigned char send_buf[BUFLEN];
unsigned char recv_buf[BUFLEN];
int sent_c, recv_c;
char move;
//no port specified
if (argc == 1) {
printf("Parameter: /dev/ttyUSB0 [command]\n");
printf("\"/dev/ttyUSB0\" is a USB serial device to support serial control.\n");
return 1;
}
serial_d = openserial(argv[1]);
if (serial_d == -1) return 1;
// ----- Begin of setup serial ports
tcgetattr(serial_d, &soptions_org);
tcgetattr(serial_d, &soptions);
speed = B9600; // Speed options: B19200, B38400, B57600, B115200
cfsetispeed(&soptions, speed);
cfsetospeed(&soptions, speed);
// Enable the reciver and set local mode...
soptions.c_cflag |= ( CLOCAL | CREAD );
// Setting Parity Checking (8N1)
soptions.c_cflag &= ~PARENB;
soptions.c_cflag &= ~CSTOPB;
soptions.c_cflag &= ~CSIZE;
soptions.c_cflag |= CS8;
// Local setting
//soptions.c_lflag = (ICANON | ECHO | ECHOE); //canonical
soptions.c_lflag = ~(ICANON | ECHO | ECHOE | ISIG); //noncanonical
// Input setting
//soptions.c_iflag |= (IXON | IXOFF | IXANY); //software flow control
soptions.c_iflag |= (INPCK | ISTRIP);
soptions.c_iflag = IGNPAR;
// Output setting
soptions.c_oflag = 0;
soptions.c_oflag &= ~OPOST;
// Read options
soptions.c_cc[VTIME] = 0;
soptions.c_cc[VMIN] = 1; //transfer at least 1 character or block
// Apply setting
tcsetattr(serial_d, TCSANOW, &soptions);
// ----- End of setup serial ports
map[mapsize-1][0]=wall; // left down corner =wall
map[mapsize-2][0]=wall;
map[mapsize-1][1]=wall;
map[mapsize-2][1]=visit;
state =0;
x=1;
y=8;
int i,j,k;
int* package;
char p[10];
int start =0;
//system("./plotmap.pyc &");
while (1) {
printf("Receiving Data\n");
/*
printf("Enter command ('q' to exit): ");
command = getchar();
clean_buffer();
if (command == 'q') {
printf ("Bye!\n");
break;
}
printf("Sending command '%c'...\n", command);
sent_c = write(serial_d, &command, 1); // Send command
tcdrain(serial_d);
*/
usleep(1000000); // Wait for response
if(start ==0)
{
memset(recv_buf, '\0', BUFLEN);
recv_c = read(serial_d, recv_buf, BUFLEN); // Get response message
tcdrain(serial_d);
if(recv_buf[0] == 'g')
start =1;
else
start=0;
}
else{
for(k=0; k<5;k++){
memset(recv_buf, '\0', BUFLEN);
recv_c = read(serial_d, recv_buf, BUFLEN); // Get response message
tcdrain(serial_d);
printf("%c\n", recv_buf[0]);
p[k] = recv_buf[0];
}
}
package =Decode(p);
move = p[1];
state = package[0];
lb = package[1];
fb = package[2];
rb = package[3];
printf("parameter %d %c %d %d %d\n",p[0],p[1],p[2],p[3],p[4]);
printf("parameter %d %c %d %d %d\n",state,move,lb,fb,rb);
//Draw
Record(state);
state = walk(state,move);
map[y][x]=visit;
//printf("%c %d %d %d\n",move,x,y,state);
FILE *f = fopen("map.txt","w");
for(i=0;i<mapsize;i++){
for(j=0;j<mapsize;j++){
printf("%d ",map[i][j]);
fprintf(f,"%d ",map[i][j]);
}
printf("\n");
fprintf(f,"\n");
}
fclose(f);
//system("./plotmap.pyc &");
}
// restore setting and close
tcsetattr(serial_d, TCSANOW, &soptions_org);
close(serial_d);
return 0;
}
