/** * \file logig20 * \author SBG Systems (Raphael SIRYANI) * \author USGS csherwood@usgs.gov * \date 16/12/08 * \date 9-Aug-2011 * * Log the IG-20 accelerometer to an ASCII file * Based on ig500Continuous.c */ #include #include #include #include #include #include #include #include #include #include #define TARGET_MOXA #if defined TARGET_MOXA // this should point to the xscale version when Moxa is target #include "/usr/local/xscale_sbgCom/src/sbgCom.h" #else #include "/usr/local/include/sbgCom/sbgCom.h" #endif #define MAX_TBUF_SIZE 21 #define MAX_DBUF_SIZE 220 #define MAX_OBUF_SIZE 255 #define MAX_FNAME_SIZE 255 #define MAX_MSG_LEN 255 #define BUF_SIZE 127 #define FREQ_DIVIDER 3 // Values for FREQ_DIVIDER // 25 = 2 Hz // 6 = 8.3 Hz // 5 = 10 Hz // 3 = 16.7 Hz // 2 = 25 Hz // 1 = 50 Hz // 0 = 100 Hz // Global variables FILE *fdp, *fdp_log, *fdp_cfg, *fdp_data; /* file stream pointer for stdio library calls */ int fds, fds_log, fds_cfg, fds_data, fds_port; /* file descriptor for system calls */ char datafilename[127]="/var/sda/data/accel/instrument_00000_2011MMddhhmmss.dat\0"; // Function declarations // Function called each time we have an error on a continuous frame. void continuousErrorCallback(SbgProtocolHandleInt *pHandler, SbgErrorCode errorCode, void *pUsrArg); // Function called each time we have received a new data on a continuous frame. void continuousCallback(SbgProtocolHandleInt *handler, SbgOutput *pOutput, void *pUsrArg); static void my_exit(int sig); int tsout( FILE *fd, char *buf ); //----------------------------------------------------------------------// // Main program // //----------------------------------------------------------------------// int main(int argc, char** argv) { SbgProtocolHandle protocolHandle; int ier; char buf[BUF_SIZE]; /* may be too small */ char logfname[MAX_FNAME_SIZE]; time_t t; struct tm *gmp; struct tm gm; pid_t my_pid, parent_pid; struct timeval tv; tv.tv_sec = 1; tv.tv_usec = 0; signal(SIGINT,my_exit); /* terminal interuppt (e.g., ctrl-C) */ signal(SIGTERM,my_exit); /* terminate process */ t=time(NULL); gmp = gmtime(&t); gm = *gmp; my_pid = getpid(); parent_pid = getppid(); // open a log file with a name that includes the PID //sprintf(logfname,"%s_%d.log",argv[0],my_pid); snprintf(logfname,MAX_FNAME_SIZE, "/var/sda/logs/ig20_%04d%02d%02d%02d%02d%02d_%06d.log", 1900+gm.tm_year, gm.tm_mon+1,gm.tm_mday,gm.tm_hour,gm.tm_min,gm.tm_sec,(int)my_pid); fdp_log = fopen(logfname,"wt"); if(fdp_log == NULL) { // TODO Send this message to a better place? fprintf(stderr,"Problem opening log file\n"); my_exit(SIGUSR1); } // Get system fd of log file. See TLPI p. 248 fds_log = fileno(fdp_log); // Log program name and PID to log file snprintf(buf,BUF_SIZE,"%s PID : %d",argv[0],my_pid ); ier = tsout( fdp_log, buf ); ier = tsout( stdout, buf ); // Init our communications with the device (Please change here the com port and baud rate // On MacOS X, use for example "/dev/cu.SLAB_USBtoUART" // On Windows, use for example "COM3" // On crs ThinkPad R40 Ubuntu: "/dev/ttyUSB0" // On Moxa: "/dev/ttyM1" #if defined TARGET_MOXA if (sbgComInit("/dev/ttyM1", 115200, &protocolHandle) == SBG_NO_ERROR) #else if (sbgComInit("/dev/ttyUSB0", 115200, &protocolHandle) == SBG_NO_ERROR) #endif { // Wait until the device has been initialised sbgSleep(50); // Define the default output mask for continuous mode // This setting is not saved in flash non volatile memory if (sbgSetDefaultOutputMask(protocolHandle, FALSE, SBG_OUTPUT_QUATERNION| SBG_OUTPUT_EULER | SBG_OUTPUT_ACCELEROMETERS | SBG_OUTPUT_TEMPERATURES | SBG_OUTPUT_TIME_SINCE_RESET ) != SBG_NO_ERROR) { fprintf(stderr,"Unable to set default output mask\n"); fflush(stderr); } // Enable continuous mode, setting is not saved in flash memory if (sbgSetContinuousMode(protocolHandle, FALSE, TRUE, FREQ_DIVIDER) != SBG_NO_ERROR) { fprintf(stderr,"Unable to enable continuous mode\n"); fflush(stderr); } // Define our continuous handlers sbgSetContinuousErrorCallback(protocolHandle, continuousErrorCallback, NULL); sbgSetContinuousModeCallback(protocolHandle, continuousCallback, NULL); // Document progress so far snprintf(buf,BUF_SIZE,"Initialized IG-20" ); ier = tsout( fdp_log, buf ); ier = tsout( stdout, buf ); // Make a data file name snprintf(datafilename,MAX_FNAME_SIZE, "/var/sda/data/accel/ig20_%04d%02d%02d%02d%02d%02d_%06d.dat", 1900+gm.tm_year, gm.tm_mon+1,gm.tm_mday,gm.tm_hour,gm.tm_min, gm.tm_sec, (int)my_pid); // open data file fdp_data = fopen(datafilename,"w"); if(fdp_data == NULL) { sprintf(buf,"Problem opening data file"); ier = tsout( fdp_log, buf ); ier = tsout( stdout, buf ); my_exit( SIGUSR1 ); } else { sprintf(buf,"Opened data file: %s",datafilename); ier = tsout( fdp_log, buf ); ier = tsout( stdout, buf ); } // Get system fd of log file. See TLPI p. 248 fds_data = fileno(fdp_data); // Main loop while (1) { // Handle our continuous mode system sbgProtocolContinuousModeHandle(protocolHandle); // Small pause to unload CPU sbgSleep(10); } // Close our protocol system sbgProtocolClose(protocolHandle); return 0; } else { fprintf(stderr, "Unable to open IG-20 device\n"); fflush(stderr); return -1; } } /*! * Function called each time we have an error on a continuous frame. * \param[in] pHandler Our sbgCom protocol handler. * \param[in] errorCode Error code returned from continuous operation. * \param[in] pUsrArg User argument pointer as defined in sbgSetContinuousErrorCallback function. */ void continuousErrorCallback(SbgProtocolHandleInt *pHandler, SbgErrorCode errorCode, void *pUsrArg) { char errorMsg[256]; // Convert our error code to a human readable error message sbgComErrorToString(errorCode, errorMsg); // Display an error message fprintf(stderr,"continuousErrorCallback: We have received the following error %s\n", errorMsg); fflush(stderr); } /*! * Function called each time we have received a new data on a continuous frame. * \param[in] pHandler Our sbgCom protocol handler. * \param[in] pOutput Pointer on our received data struct. * (You don't have the ownership so don't delete it!) * \param[in] pUsrArg User argument pointer as defined in sbgSetContinuousModeCallback function. */ void continuousCallback(SbgProtocolHandleInt *pHandler, SbgOutput *pOutput, void *pUsrArg) { int ier=-1; time_t t; struct tm *gmp; struct tm gm; char dbuf[MAX_DBUF_SIZE]; char tbuf[MAX_TBUF_SIZE]; char obuf[MAX_OBUF_SIZE]; if (pOutput){ // Get current time t = time(NULL); gmp = gmtime(&t); gm = *gmp; // save local copy...see TLPI p. 192 // Write string to file // TODO - this prevents obuf from getting too big, but is a kludge //obuf[0]='\0'; snprintf(dbuf,MAX_DBUF_SIZE,"%08d % 7.2f % 7.2f % 11.6f % 11.6f % 11.6f % 6.2f % 6.2f", (pOutput->timeSinceReset), SBG_RAD_TO_DEG(pOutput->stateEuler[0]), SBG_RAD_TO_DEG(pOutput->stateEuler[1]), (pOutput->accelerometers[0]), (pOutput->accelerometers[1]), (pOutput->accelerometers[2]), (pOutput->temperatures[0]), (pOutput->temperatures[1])); ier = tsout( fdp_data, dbuf ); } } // MY_EXIT - Clean up, close files, write to log, exit static void my_exit(int sig) { // sig is assigned by signal or passed by user // UNSAFE use of printf() flush(), close(), and exit() // TODO - Do this with async-safe calls? int ier; char buf[255]; // Identify cause of exit switch (sig) { case SIGINT : { sprintf(buf,"Caught SIGINT\n"); break; } case SIGTERM : { sprintf(buf,"Caught SIGTERM\n"); break; } case SIGUSR1 : { sprintf(buf,"Caught SIGUSR1\n"); break; } default : { sprintf(buf,"Unknown signal\n"); } } // Close files in order of importance: // (1) close accelerometer? /* if( (ier=SerialClose(portnum)) != SERIAL_OK ) sprintf(buf,"Problem closing serial port %d; returned %d",portnum,ier); sprintf(buf,"Closed serial port %d",portnum); ier = tsout(fdp_log,buf); */ // (2) close data file if( fdp_data != NULL ){ fflush(fdp_data); fclose(fdp_data); } // (3) identify cause of exit and close log file if( fdp_log != NULL ){ ier = tsout(fdp_log,buf); fflush(fdp_log); fclose(fdp_log); } exit(0); } // TSOUT - prepend time stamp, print contents of buf, add CR int tsout( FILE *fd, char *buf ) { //time_t t; struct tm gm, *gmp; struct timeval tv; int iw=1, tstat, nlen=0; char tbuf[255]; tstat=gettimeofday(&tv,NULL); //gmp = gmtime(&t); // pointing at the tv seconds gets the time from gettimeofday, so usecs will be synched gmp = gmtime(&tv.tv_sec); gm = *gmp; // save local copy...see TLPI p. 192 snprintf(tbuf,29,"%4d %02d %02d %02d %02d %10.7f ", 1900+gm.tm_year,gm.tm_mon+1,gm.tm_mday,gm.tm_hour,gm.tm_min,(gm.tm_sec+((double)tv.tv_usec/1000000))); //printf("lengths of tbuf and buf are: %d, %d.\n", strlen(tbuf), strlen(buf)); nlen=strlen(buf); if ((strlen(tbuf) + strlen(buf)) < sizeof(tbuf)-1) { strncat(tbuf,buf,nlen); strncat(tbuf,"\n",1); iw = fprintf(fd,"%s",tbuf); fflush(fd); } else { printf("string too long- look for error in data!\n"); //email manager that something's wrong iw=-1; } usleep(2000); }