/*************************************************************************** * * * Laser Range finder Functions: lrf_functions.c * * * * Various functions to allow communications with the Laser Range Finder * * and to process any data obtained from the range finder. * * * * WRITTEN BY: Bill Kapralos * * DATE: June 9, 1997 * * * * York University * * Department of Computer Science * * * * The serial port initialization code (serialPortInit), was obtained * * from the Greg Reid's Lego project (York University), and from tty-io.c * * written by James R. Service. The function fdHasCharacters() was also * * obtained from code written by James R. Service (lrfd.c). * * * ****************************************************************************/ #include "lrf_functions.h" #include "lrf_audio.h" /*************************************************************** * * Initialize the serial port. * Laser range finder communicates with 8 data bytes, 1 stop bit, no * parity bit and 9600 baud. Returns 1 if initialization is successful, * 0 otherwise. * ****************************************************************/ int serialPortInit(char *SerialPortName){ /* Holds all the parameters/settings of serial port. */ struct termios newterm; if ((serialFd = open(SerialPortName, O_RDWR | O_NOCTTY)) < 0 ){ fprintf(stderr, "Could not open serial port."); return ERROR; } /*Set I/O flags*/ /*Note IXOFF was tried and caused more errors*/ newterm.c_iflag = IGNBRK; /*Set local flags*/ newterm.c_lflag = 0; newterm.c_oflag = 0 ; /*Set Control flags: Allow 8 data bits, no parity and one stop bit. */ newterm.c_cflag = (CS8 | CREAD | CLOCAL ); /* Will wait until the specified amount of bytes are read from the serial port. If set to 0, once the specified time (see below) has expired, the read call can return with no read data. */ newterm.c_cc[VMIN] = 0; newterm.c_cc[VTIME] = 0; /* Set port speed. */ cfsetospeed(&newterm, B9600); cfsetispeed(&newterm, B9600); /* Get rid of any data on the port by flushing. */ if (tcflush(serialFd, TCIOFLUSH) == -1) fprintf(stderr, "Falied Flush\n"); /* Set the serial port to the above settings. */ if (tcsetattr(serialFd, TCSANOW, &newterm) == -1) fprintf(stderr, "Failed setattr\n"); lrfInitialized = 1; return (1); } /****************************************************************** * * Send commands to the LRF via the (serial port). A delay is required * to allow the commands to be processed by the LRF. Returns the number of * characters sent or -1 if an error occurs. * *******************************************************************/ int sendCommand(char *cmnd, int cmdLength){ int count; if (!lrfInitialized){ fprintf(stderr, "ERROR: LRF not initialized!\n"); return 0; } if((count = write(serialFd, cmnd, cmdLength)) != cmdLength){ fprintf(stderr, "sendCommand: ERROR - Incomplete Wrte!\n"); count = -1; } /* Minimum of 200 usec required however I find it requires more. */ usleep(DELAY_TIME); /* Allow LRF to process the command. */ return count; } /************************************************************** * * Returns the number of characters available on the specified file desc. * **************************************************************/ fdHasCharacters (int fileDes) { long int bytesToRead = 0 ; /*while(!bytesToRead){*/ if(ioctl (fileDes, FIONREAD, (caddr_t) &bytesToRead) == -1) { printf("fdHasCharacters(): ioctl (...FIONREAD...) failed -- ") ; return (-1) ; } /*}*/ return bytesToRead ; /* Number of bytes available at serial port. */ } /******************************************************************** * * Obtain reading from the LRF. Return 1 if the number read is equal to the * amount available. Otherwise return 0. * ********************************************************************/ int getReading(unsigned char *buffer){ unsigned char *ptr; int numOfBytes, numReadBytes, success = 0; /* Read any bytes available at the serial port. Can only obtain one reading at a time. */ if(numOfBytes = fdHasCharacters(serialFd)){ numReadBytes = (numOfBytes >= LR_READINGLEN) ? LR_READINGLEN - 1 : numOfBytes; if(read(serialFd, ptr = buffer, numReadBytes ) == numReadBytes){ success = 1; *ptr = '\0'; } } return success; } /************************************************************** * * Convert string of ASCII digits into a double. The reading obtained from * the LRF begins with NULL (can be ignored ), followed by 3 ASCII SP * (when reading is not saturated), followed by value and ending with ASCII * CR and NL. An out of range reading contains 4 ASCII "(" followed by 3 * ASCII SP, CR and LF. * ***************************************************************/ float processReading(unsigned char *reading){ int index = 1; /* First character obtained from LRF can be ignored. */ double value = 0.0, power = 0.0, result; /* Check for out-of-range reading. indicated by 4 '('. */ if(reading[index] == SATURATEDREADING ){ index++; /* More than one "(" indicates out of range reading. Invalid reading. */ if(reading[index] == SATURATEDREADING) return 0.0; } /* Process the whole number portion of the reading - the portion before the decimal point, (a '*' is used instead of a decimal point when the battery is low). */ for(value = 0.0; (reading[index] != '.' && reading[index] != BATTERYLOW) && index < LR_READINGLEN; index++){ if(isdigit(reading[index])) value = 10.0 * value + (reading[index] - ASCII_ZERO); } if(reading[index] == DECIMAL_POINT || reading[index] == BATTERYLOW) index++; /* Move to decimal portion of reading. */ else /* Non-numeric data obtained from LRF. Do not process further. */ return 0.0; /* Process the decimal portion of the reading. */ for(power = 1.0; reading[index] != CR && index < LR_READINGLEN; index++){ if(isdigit(reading[index])){ value = 10.0 * value + (reading[index] - ASCII_ZERO); power *= 10; } } /* Returned reading must be within min and max allowable range. Reading must contain both whole and decimal parts otherwise it is not a valid numeric reading. */ if(value && power) result = value / power; else return 0.0; if(result <= MAX_DISTANCE) return (result >= MIN_DISTANCE) ? result : MIN_DISTANCE; else return MAX_DISTANCE; } /********************************************************* * * Send all the desired operation commands to the lrf prior to * obtaining any readings. * ********************************************************/ void lrfSetUp(){ char *cleanUpBuffer[100]; if(sendCommand(CONT_UPDATE, strlen(CONT_UPDATE)) != strlen(CONT_UPDATE)) error("Sending Command to port!"); if(sendCommand(BEEPOFF, strlen(BEEPOFF)) != strlen(BEEPOFF)) error("Sending Command to port!"); if(sendCommand(LR_TIMEOUT, strlen(LR_TIMEOUT)) != strlen(LR_TIMEOUT)) error("Sending Command to port!"); /* The commands sent to lrf remain on the read queue. Remove these bytes. */ if(fdHasCharacters(serialFd)) read(serialFd, cleanUpBuffer, fdHasCharacters(serialFd)); } /**************************************************************** * * Calculates and returns the average of all values contained in the * array 'readings'. 0.0 is returned if the array is empty. * *****************************************************************/ double calcAvgDistance(double *readings, int numOfReadings){ double tempDist = 0.0; int index; for(index = 0; index < numOfReadings; index++) tempDist += readings[index]; return ((numOfReadings > 0) ? (tempDist / numOfReadings) : 0.0); } /**************************************************************** * * Display the message 'errorMsg' to stderr, close the audio and * serial ports and exit the program. * *****************************************************************/ void error(char *errorMsg){ fprintf(stderr, "ERROR: %s\n", errorMsg); if(serialFd) close(serialFd); if(audioFd > 0) close(audioFd); exit(0); } /**************************************************************************/ int lrfChangeShotsPerReading(int shots){ char line[BUFSIZ]; /* * Note that the laser range-finder needs four digits *including* the * leading zeros. * */ sprintf(line, "%04d" "\r", shots); return sendCommand(line, strlen(line)); }