/*
    LMS200Demo: This program demostrates the use of serial communication API
    with LMS-200 manufactured by SICK optics (http://www.sick.de).  
    Copyright (C) 2003.  David Yuen (d.yuen@auckland.ac.nz)

    This program 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 2 of the License, or
    (at your option) any later version.

    This program 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 this program; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

/*More information:
While SICK optics has released the serial communication protocol, it does not
work as intended.  (At least for our laser scanner LMS200-30106.  Some packets,
notably most of the ACK packets, have been changed.  Also, by default, they
only distribute a demo program in windows with no source code.  Therefore, I
decided to write another one in C.  The program is supposed to be ANSI C and
POSIX compliant.  Since it is a  demo program, some of the codes are a little
bit bloated.  

The communication protocol can be downloaded from SICK.  Do a search for
TLLMS2xxEN_8007954_04042003.pdf or "LMS telegram listing" on the web.

Although the program is written with LMS-200 in mind, it should work with
similar units like LMS-220, LMS-211, LM-221, possibly with some minor changes
for the declaration of the command and ACK packets.  The electronics of the
laser scanner can support fairly fast updating speed.  The serial link is the
bottle neck (if you don't buy their special 500kbps card).  Therefore, it makes
sense to use the fastest transfer mode (38400 bps) once after power up.  On
the other hand, it should be reset to the default of 9600bps after use.  The
SIGINT, SIGTERM etc are thus trapped to carry out the import scanner transfer
reset process.

In addition to the 500000bps card from SICK a USB to RS422
converter can also be used at 500000bps.  Both the USB-COMi and 
USB-COMi-si have been tried and used with an LMS291.

compile: gcc -Wall -oLMSdemo LMSdemo.c
syntax:  ./LMSdemo [Flags]
Flags:
  -n nrepeat       (number of repeat sets of measurements)
  -r 180|100       (scan range in degree)
  -s 0.25|0.5|1.0  (angular resolution in degree)
  -u cm|mm         (measurement unit)
  -p /dev/tty*    (the com port used)
  -b 9600|19200|38400|500000 (the baud rate to use)
  -h               (help)
The program has been built with both gcc-2.95.4 and gcc-3.2.3 in Debian unstable.

Last update: 2004/07/23
Version: 0.3.0   2004/07/23   
Added CRC checking/ 500k baud support.  Contributed by Kevin 
Knoedler <knoedler AT _no_spam_dslextreme.com>
Version: 0.2.1   2003/03/08
Initial public release from David Yuen <d.yuen AT _no_spam_auckland.ac.nz>
*/

#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <termios.h>
#include <stdio.h>
#include <sys/time.h>
#include <time.h>
#include <signal.h>
#include <getopt.h>
#include <unistd.h>
#include <string.h>
#include <stdlib.h>

#define _POSIX_SOURCE 1 /* POSIX compliant source */
#define FALSE 0
#define TRUE 1
#define MAXRETRY 25
#define MAXNDATA 802
#define STX 0x02   /*every PC->LMS packet is started by STX*/ 
#define ACKSTX 0x06 /*every PC->LMS packet is started by ACKSTX*/
#define BAUD_500000 512
#define BAUD_38400 256
#define BAUD_19200 128
#define BAUD_9600 64
#define RANGE_100 32
#define RANGE_180 16
#define RES_1_DEG 8
#define RES_0_5_DEG 4
#define RES_0_25_DEG 2 
#define MMMODE 1
#define CMMODE 0
typedef unsigned char uchar;


/*The only 2 global variable used by the signal handler so that
  the program can reset to 9600 bps mode before unexpected termination*/
static int LMS_FD;
static struct termios OLDTERMIOS;

/*the cmd and ack packets for the 5 different range/resolution modes*/
const uchar PCLMS_RES1[]={0x02,0x00,0x05,0x00,0x3b,0x64,0x00,0x64,0x00};
const uchar PCLMS_RES2[]={0x02,0x00,0x05,0x00,0x3b,0x64,0x00,0x32,0x00};
const uchar PCLMS_RES3[]={0x02,0x00,0x05,0x00,0x3b,0x64,0x00,0x19,0x00};
const uchar PCLMS_RES4[]={0x02,0x00,0x05,0x00,0x3b,0xb4,0x00,0x64,0x00};
const uchar PCLMS_RES5[]={0x02,0x00,0x05,0x00,0x3b,0xb4,0x00,0x32,0x00};
const uchar LMSPC_RES1_ACK[]={0x06,0x02,0x80,0x07,0x00,0xbb,0x01,0x64,0x00,0x64,0x00,0x10};
const uchar LMSPC_RES2_ACK[]={0x06,0x02,0x80,0x07,0x00,0xbb,0x01,0x64,0x00,0x32,0x00,0x10};
const uchar LMSPC_RES3_ACK[]={0x06,0x02,0x80,0x07,0x00,0xbb,0x01,0x64,0x00,0x19,0x00,0x10};
const uchar LMSPC_RES4_ACK[]={0x06,0x02,0x80,0x07,0x00,0xbb,0x01,0xb4,0x00,0x64,0x00,0x10};
const uchar LMSPC_RES5_ACK[]={0x06,0x02,0x80,0x07,0x00,0xbb,0x01,0xb4,0x00,0x32,0x00,0x10};

/*the cmd and ack packets different measurement unit modes*/
const uchar PCLMS_SETMODE[]={0x02,0x00,0x0a,0x00,0x20,0x00,0x53,0x49,0x43,0x4b,0x5f,0x4c,0x4d,0x53};
const uchar PCLMS_MM[]={0x02,0x00,0x23,0x00,0x77,
			 0x00,0x00, // Block A
			 0x46,0x00, // Block B
			 0x01,      // Block C [9]
			 0x00,      // Block D (was 0xd)
			 0x01, // Block E - 0x00 = cm, 0x01 = mm
			 0x00, // Block F
			 0x00, // Block G
			 0x02, // Block H
			 0x02, // Block I
			 0x02, // Block J
			 0x00, // Block K
			 0x00, // Block L
			 0x0a, // Block M
			 0x0a, // Block N
			 0x50, // Block O
			 0x64, // Block P
			 0x00, // Block Q
			 0x0a, // Block R
			 0x0a, // Block S
			 0x50, // Block T
			 0x80, // Block U
			 0x00, // Block V
			 0x0a, // Block W
			 0x0a, // Block X
			 0x50, // Block Y
			 0x64, // Block Z
			 0x00, // Block A1
			 0x00, // Block A2
			 0x00,0x00, // Block A3
			 0x02,0x00};// Block A4
const uchar LMSPC_MM_ACK[]={0x06,0x02,0x80,0x25,0x00,0xf7,0x01,0x00,0x00,0x46,0x00,0x01,0x0d,0x01,\
			    0x00,0x00,0x02,0x02,0x02,0x00,0x00,0x0a,0x0a,0x50,0x64,0x00,0x0a,0x0a,\
			    0x50,0x80,0x00,0x0a,0x0a,0x50,0x64,0x00,0x00,0x00,0x00,0x02,0x00,0x10};

/*the cmd packets for setting transfer speed and  controlling the start and stop of measurement*/
const uchar PCLMS_STATUS[]= {0x02,0x00,0x01,0x00,0x31};
const uchar PCLMS_START[]=  {0x02,0x00,0x02,0x00,0x20,0x24};
const uchar PCLMS_STOP[]=   {0x02,0x00,0x02,0x00,0x20,0x25};
const uchar PCLMS_B9600[] = {0x02,0x00,0x02,0x00,0x20,0x42};
const uchar PCLMS_B19200[]= {0x02,0x00,0x02,0x00,0x20,0x41};
const uchar PCLMS_B38400[]= {0x02,0x00,0x02,0x00,0x20,0x40};
const uchar PCLMS_B500000[]={0x02,0x00,0x02,0x00,0x20,0x48};
/*the ack packet for the above*/
const uchar LMSPC_CMD_ACK[]={0x06,0x02,0x80,0x03,0x00,0xa0,0x00,0x10};


// Calculates the CRC for packets sent to/from the LMS
unsigned short LMSCRC(unsigned char* theBytes, int lenBytes)
{
  unsigned char xyz1 = 0;
  unsigned char xyz2 = 0;
  unsigned short uCrc16 = 0;
  int i;
  for (i = 0; i < lenBytes; i++) {
    xyz2 = xyz1;
    xyz1 = theBytes[i];

    if (uCrc16 & 0x8000) {
      uCrc16 = (uCrc16 & 0x7fff) << 1;
      uCrc16 = uCrc16 ^ 0x8005;
    }
    else {
      uCrc16 = uCrc16 << 1;
    }
    uCrc16 = uCrc16 ^ (xyz1 | (xyz2 << 8));
  }
  return uCrc16;
}

void printError(const char * msg)
{
  printf("%s", msg);
  // fprintf(stderr, "%s", msg);
}

void printError1(const char * msg, int val1)
{
  printf("%s 0x%x\n", msg, val1);
  // fprintf(stderr, "%s 0x%x\n", msg, val1);
}

void printError2(const char * msg, int val1, int val2)
{
  printf("%s 0x%x 0x%x\n", msg, val1, val2);
  // fprintf(stderr, "%s 0x%x 0x%x\n", msg, val1, val2);
}


// Compares two messages
uchar msgcmp(int len1, const uchar *s1, int len2, const uchar *s2)
{
  int i;
  if (len1 != len2) {
    printError2("msgcmp - message lengths didn't match", len1, len2);
    return FALSE;
  }
  for(i=0; i<len1; i++) {
    if (s1[i] != s2[i]) {
      printError2("msgcmp - character didn't match", s1[i], s2[i]);
      return FALSE;
    }
  }
  unsigned short crcval = LMSCRC((unsigned char*)s1, len1);
  if ((crcval & 0xff) != s2[len2]) {
    printError2("msgcmp - CRC didn't match1", crcval & 0xff, s2[len2-2]);
    return FALSE;
  }
  if (((crcval >> 8) & 0xff) != s2[len2+1]) {
    printError2("msgcmp - CRC didn't match2",(crcval >> 8) & 0xff, s2[len2-1]);
    return FALSE;
  }

  return TRUE;
}

// Sleeps 55uS (minimum LMS interval between bytes)
void sleep55us()
{
  // This only works if the program priority is raised to real time.
  // Otherwise the usleep of 55uS actually takes 1 or more scheduling
  // intervals which can be more than the 14mS maximum interval between
  // characters for the LMS.
  // usleep(55);
}


// Writes a message to the LMS
void wtLMSmsg(int fd, int len, const uchar *msg)
{
  int i;
  unsigned short crcval = LMSCRC((unsigned char*)msg, len);
  unsigned char sendchar;

  for (i = 0; i < len; i++) {
    sleep55us();
    write(fd,(const void*) (msg+i),1);
  }

  sendchar = crcval & 0xff;
  sleep55us();
  write(fd, (const void *) &sendchar, 1);
  sendchar = (crcval >> 8) & 0xff;
  sleep55us();
  write(fd, (const void *) &sendchar, 1);


#ifdef DEBUG
  printf("write msg: ");
  for(i=0;i<len; i++) printf("%02x ",msg[i]);
  printf("%02x %02x\n", crcval & 0xff, (crcval >> 8) & 0xff);
#endif
}


int rdLMSmsg(int fd, int len, const uchar *buf)
{
  int sumRead=0,nRead=0,toRead=len,n;
  static struct timeval tv0;
  static fd_set rfds;
  tv0.tv_sec = 0;
  tv0.tv_usec = 500000;
  FD_ZERO (&rfds);
  FD_SET(fd, &rfds);

#ifdef DEBUG
  int i;
  printf("read msg: ");
#endif
  while(toRead>0){
    n=toRead>255 ? 255:toRead;
    toRead-=n;
    if (select(fd+1, &rfds, NULL, NULL, &tv0)) {
      nRead=read(fd,(void *)(buf+sumRead),n);
    }
    else {
      nRead = 20; // just to keep it going
      printError("rdLMSmsg - timeout error in multi-byte read\n");
    }
#ifdef DEBUG
    for(i=0;i<nRead; i++) printf("%02x ",buf[i]);
#endif
    sumRead+=nRead;
    if(nRead!=n) break;
  }
#ifdef DEBUG
  printf("\n");  
#endif
  return nRead;
}

uchar rdLMSbyte(int fd)
{
  uchar buf;
  static struct timeval tv0;
  static fd_set rfds;
  tv0.tv_sec = 0;
  tv0.tv_usec = 100000;

  FD_ZERO (&rfds);
  FD_SET(fd, &rfds);
  if (select(fd+1, &rfds, NULL, NULL, &tv0)) {
    read(fd,&buf,1);
#ifdef DEBUG
    printf("read byte: %02x\n", buf);
#endif
  }
  else {
    printError("rdLMSbyte - timeout Error\n");
    buf = 0;
  }
  return buf;
}


/*procedure that shows the laser measurements. Format: 
Timestamp
v0  v1  v2  ... v19
v10 v21 v22 ... v39
...
The total number of measurements is dependent on the resolution
and angular range selected*/
void showdata(int len, uchar *buf)
{
  struct timeval now;
  struct timezone tz;
  int i,hr,min,sec;
  double val;

  gettimeofday(&now,&tz);
  hr=(now.tv_sec/3600 % 24 - tz.tz_minuteswest/60) % 24;
  min=(now.tv_sec % 3600)/60;
  sec=now.tv_sec % 60;
  printf("\n%02d:%02d:%02d.%04ld  ",hr,min,sec,now.tv_usec/1000);
  
  /*each value are represented by 16 bits*/
  /*only the lower 12 bits are significant*/
  
  for(i=0; i<len; i=i+2) {
    if((i % 40) ==0) printf("\n%d:",i/2);
    val=(double)( (buf[i+1] & 0x1f) <<8  |buf[i]);
    printf("%5.0f ", val);
  }
  printf("\n");  
}


/*return true if the ACK packet is as expected*/
uchar chkAck(int fd, int ackmsglen, const uchar *ackmsg)
{
  int i,buflen;
  uchar buf[MAXNDATA];

  /*the following is to deal with a possibly timing issue*/
  /*the communication occasionally breaks without this*/
  usleep(100000); 
  for(i=0;i<MAXRETRY;i++) {
    if(rdLMSbyte(fd)==ackmsg[0]) break;
  }
  buflen=rdLMSmsg(fd,ackmsglen+1,buf);
  return msgcmp(ackmsglen-1,ackmsg+1,buflen-2,buf);
}

/*set the communication speed and terminal properties*/
int initLMS(const char *serialdev, int baud_sel)
{
  int fd;
  const uchar *msg;
  tcflag_t cflagval;
  struct termios newtio_struct, *newtio=&newtio_struct;
  fd = open(serialdev, O_RDWR | O_NOCTTY ); 
  if (fd <0) {
    printError1("initLMS - Error opening 1", fd);
    perror(serialdev);
    exit(-1);
  }
        
  tcgetattr(fd,&OLDTERMIOS); /* save current port settings */

  /*after power up, the laser scanner will reset to 9600bps*/
  memset(newtio, 0, sizeof(struct termios));
  newtio->c_cflag = B9600 | CS8 | CLOCAL | CREAD;
  newtio->c_iflag = IGNPAR;
  newtio->c_oflag = 0;
        
  /* set input mode (non-canonical, no echo,...) */
  newtio->c_lflag = 0;
  newtio->c_cc[VTIME]    = 10;   /* inter-character timer unused */
  newtio->c_cc[VMIN]     = 255;   /* blocking read until 1 chars received */
  tcflush(fd, TCIFLUSH);
  tcsetattr(fd,TCSANOW,newtio);

  if (baud_sel == BAUD_500000) { // step to the 500000bps mode
    msg = PCLMS_B500000;
    cflagval = B500000 | CS8 | CLOCAL | CREAD;
  }
  else if (baud_sel == BAUD_38400) { // step to the 38400bps mode
    msg = PCLMS_B38400;
    cflagval = B38400 | CS8 | CLOCAL | CREAD;
  }
  else if (baud_sel == BAUD_19200) { // step to the 19200bps mode
    msg = PCLMS_B19200;
    cflagval = B19200 | CS8 | CLOCAL | CREAD;
  }
  else {
    // Do nothing - goal is 9600 and already at 9600
    return fd;
  }

  wtLMSmsg(fd,sizeof(PCLMS_B500000)/sizeof(uchar),msg);
  if(!chkAck(fd,sizeof(LMSPC_CMD_ACK)/sizeof(uchar),LMSPC_CMD_ACK))
    printError("initLMS - Baud rate changes failure\n");

  // set the PC side as well    
  tcflush(fd, TCIFLUSH);
  close(fd);
  usleep(400000); //  This is needed on the faster kernels otherwise the
  // open statement on the next line fails
  fd = open(serialdev, O_RDWR | O_NOCTTY );
  if (fd <0) {
    printError1("initLMS - Error opening 2", fd);
    perror(serialdev);
    exit(-1);
  }
  newtio->c_cflag = cflagval;

  tcflush(fd, TCIFLUSH);
  tcsetattr(fd,TCSANOW,newtio);
  tcflush(fd, TCIFLUSH);
  return fd;
}

// Sets the sweep width and resolution
int setRangeRes(int fd, int res)
{
  const uchar *msg, *ackmsg;
  int retval = 0;

  /*change the resolution*/
  switch(res){
  case (RES_1_DEG | RANGE_100): 
    msg=PCLMS_RES1;
    ackmsg=LMSPC_RES1_ACK;
    break;
  case (RES_0_5_DEG | RANGE_100):
    msg=PCLMS_RES2;
    ackmsg=LMSPC_RES2_ACK;
    break;
  case (RES_0_25_DEG | RANGE_100):
    msg=PCLMS_RES3;
    ackmsg=LMSPC_RES3_ACK;
    break;
  case (RES_1_DEG | RANGE_180):
    msg=PCLMS_RES4;
    ackmsg=LMSPC_RES4_ACK;      
    break;
  case (RES_0_5_DEG | RANGE_180):
    msg=PCLMS_RES5;
    ackmsg=LMSPC_RES5_ACK;
    break;
  default:
    printError("Invalid resolution selected. Drop back to default\n");
    msg=PCLMS_RES1;
    ackmsg=LMSPC_RES1_ACK;
    break;
  }

  // the following two line works only because msg & ackmsg are const uchar str
  wtLMSmsg(fd,sizeof(PCLMS_RES1)/sizeof(uchar),msg);
  if(!chkAck(fd,sizeof(LMSPC_RES1_ACK)/sizeof(uchar),ackmsg)) {
    printError("setRangeRes - Resolution mode setting failure\n");
    retval = 1;
  }
  return retval;
}

// Sends the password to enable changing the mode
int sendPassword(int fd)
{
  /*invoking setting mode*/
  int retval = 0;
  wtLMSmsg(fd,sizeof(PCLMS_SETMODE)/sizeof(uchar),PCLMS_SETMODE);
  if(!chkAck(fd,sizeof(LMSPC_CMD_ACK)/sizeof(uchar),LMSPC_CMD_ACK)) {
    printError("sendPassword - Measurement mode setting failure\n");
    retval = 1;
  }
  return retval;
}

// Selects mm/cm
// This message causes the red signal to light on an LMS291 - reason unknown
int setUnits(int fd, int unit)
{
  int retval = 0;
  uchar msg[100], ackmsg[100];
  memcpy(msg, PCLMS_MM, sizeof(PCLMS_MM));
  memcpy(ackmsg, LMSPC_MM_ACK, sizeof(LMSPC_MM_ACK));
  /*change the measurement unit*/
  /*may need to increase the timeout to 7sec here*/
  if (unit == MMMODE) {
    // No change is required since MM is the default
  }
  else if (unit == CMMODE) {
    msg[11] = 0x00;
  }
  else {
    printError("setUnits - Invalid units specified\n");
  }

  // The ACK contains the original message data
  memcpy(ackmsg+7, msg+5, sizeof(PCLMS_MM) - 5);

  // the following two line works only because msg & ackmsg are const uchar str
  wtLMSmsg(fd,sizeof(PCLMS_MM)/sizeof(uchar),msg);
  if(!chkAck(fd,sizeof(LMSPC_MM_ACK)/sizeof(uchar),ackmsg)) {
    printError("setUnits - Measurement mode setting failure2\n");
    retval = 1;
  }
  return retval;
}


/*tell the scanner to enter the continuous measurement mode*/
int startLMS(int fd)
{
  int retval = 0;
  wtLMSmsg(fd,sizeof(PCLMS_START)/sizeof(uchar),PCLMS_START);
  if(!chkAck(fd,sizeof(LMSPC_CMD_ACK)/sizeof(uchar),LMSPC_CMD_ACK)) {
    printError("startLMS - LMS fails to start\n");
    retval = 1;
  }
  return retval;
}

/*stop the continuous measurement mode*/
void stopLMS(int fd)
{
  wtLMSmsg(fd,sizeof(PCLMS_STOP)/sizeof(uchar),PCLMS_STOP);
  if(!chkAck(fd,sizeof(LMSPC_CMD_ACK)/sizeof(uchar),LMSPC_CMD_ACK))
    printError("stopLMS - LMS fails to stop\n");
}


/*check the status bit of the measurement*/
void chkstatus(uchar c)
{
  switch(c & 0x07){
  case 0: printError("chkstatus - no error\n"); break;
  case 1: printError("chkstatus - info\n"); break;
  case 2: printError("chkstatus - warning\n"); break;
  case 3: printError("chkstatus - error\n"); break;
  case 4: printError("chkstatus - fatal error\n"); break;
  default: printError1("chkstatus - unknown error", c); break;
  }
  if(c & 0x40) 
    printError("chkstatus - implausible measured value\n");
  if(c & 0x80)
    printError("chkstatus - pollution\n");
}

/*reset terminal and transfer speed of laser scanner before quitting*/
void resetLMS(int fd)
{
  wtLMSmsg(fd,sizeof(PCLMS_B9600)/sizeof(uchar),PCLMS_B9600);
  if(!chkAck(fd,sizeof(LMSPC_CMD_ACK)/sizeof(uchar),LMSPC_CMD_ACK))
    printError("resetLMS - Baud rate changes failure\n");

  tcflush(fd, TCIFLUSH);
  tcsetattr(fd,TCSANOW,&OLDTERMIOS);
  close(fd);
}

/*trap a number of signals like SIGINT, ensure the invoke
  of resetLMS() before quitting*/
static void sig_trap(int sig)
{
  printError("sig_trap - Premature termination\n");
  stopLMS(LMS_FD);
  resetLMS(LMS_FD);
  exit(sig);
}

static int connectToLMS(int range_mode, int res_mode, int unit_mode,
			char * port, int baud_sel)
{
  static int fd = -1;
  int retval = 0;
  int numTrys = 1;


  /*initialisation*/
  for (numTrys = 1; numTrys < 26; numTrys++) {
    retval = 0;

    #ifdef DEBUG
    printf("trying to connect %d\n", numTrys);
    #endif

    // Set the baud rate of the PC and the LMS
    fd=initLMS(port, baud_sel);
    LMS_FD=fd;

    if  ((signal(SIGTERM, sig_trap) == SIG_ERR) ||
	 (signal(SIGHUP, sig_trap)  == SIG_ERR) ||
         (signal(SIGINT, sig_trap)  == SIG_ERR) ||
	 (signal(SIGQUIT, sig_trap) == SIG_ERR) ||
	 (signal(SIGABRT, sig_trap) == SIG_ERR)) {
      printError("connectToLMS - Cannot catch the signals\n");
    }

    /*mode setting*/
    retval |= setRangeRes(fd, range_mode | res_mode);
    retval |= sendPassword(fd); // this enables the following command
    retval |= setUnits(fd, unit_mode);
    retval |= startLMS(fd);

    if (retval == 0) {
      // LMS is working
      printf("connected to LMS\n");
      return fd;
    }
    else {
      // Tell the LMS to stop sending and set baud to 9600

      // Reset the LADAR to put it into a known state
      stopLMS(fd);
      resetLMS(fd);
      printError2("connectToLMS - Failed to connect", retval, numTrys);
    }
  }
  printError1("Failed to connect to ladar after many tries!", numTrys);
  exit(0);
}


int readLMSdata(int fd, uchar* buf)
{
  unsigned short realCRC = 0;
  int datalen;
  buf[0] = 0;

  while (buf[0] != 0x02) {
    buf[0] = rdLMSbyte(fd);
  }
  
  buf[1] = rdLMSbyte(fd); // should be the ADR byte, ADR=0X80 here
  // LEN refers to the packet length in bytes, datalen 
  // refers to the number of measurements, each of them are 16bits long
  buf[2] = rdLMSbyte(fd); // should be the LEN low byte
  buf[3] = rdLMSbyte(fd); // should be the LEN high byte
  buf[4] = rdLMSbyte(fd); // should be the CMD byte, CMD=0xb0 in this mode
  buf[5] = rdLMSbyte(fd); // samples low byte
  buf[6] = rdLMSbyte(fd); // samples high byte
  
  // only lower 12 bits of high byte are significant 
  datalen = buf[5] | ((buf[6] & 0x1f) << 8);
  datalen = datalen * 2; /*each reading is 2 bytes*/

  // Check that we have a valid ADR byte, valid data length and valid CMD byte
  if ((datalen > MAXNDATA) ||
      (0x80 != buf[1]) ||
      (0xb0 != buf[4])) {
    return 0;
  }

  rdLMSmsg(fd,datalen,buf+7);

  buf[datalen + 7] = rdLMSbyte(fd); // Status byte
  buf[datalen + 8] = rdLMSbyte(fd); // should be CRC low byte*/
  buf[datalen + 9] = rdLMSbyte(fd); // should be CRC high byte*/
  realCRC = buf[datalen+8] | (buf[datalen+9] << 8); 
  
  
  int lenBytes = datalen+8;
  unsigned short CRCcalculated;
  CRCcalculated = LMSCRC(buf, lenBytes);
  
  
  if (CRCcalculated != realCRC) {
    printError2("readLMSdata - CRC Error ", CRCcalculated, realCRC);
    return 0;
  }
  return datalen; // return 0 on error (0 valid samples)
}




int main(int argc, char **argv)
{
  uchar buf[MAXNDATA];
  char c, defaultdev[]="/dev/ttyS0", *port=defaultdev;
  int fd,datalen,count=0,count_max=10;
  int range_mode=RANGE_100,res_mode=RES_1_DEG,unit_mode=MMMODE;
  int baud_sel=BAUD_9600;

  /*parse the command line input*/
  while ((c = getopt(argc, argv, "n:r:s:u:p:b:h")) != -1) {
    switch (c) {
    case 'n':
      count_max=atoi(optarg);
      break;
    case 'r':
      if(strncmp(optarg,"180",3)==0) range_mode=RANGE_180;
      else range_mode=RANGE_100;
      break;
    case 's':
      if(strncmp(optarg,"0.25",4)==0) res_mode=RES_0_25_DEG;
      else if(strncmp(optarg,"0.5",3)==0) res_mode=RES_0_5_DEG;
      else res_mode=RES_1_DEG;
      break;
    case 'u':
      if(strncmp(optarg,"cm",2)==0) unit_mode=CMMODE;
      else unit_mode=MMMODE;
      break;
    case 'p':
      port=optarg;
      break;
    case 'b':
      if (strncmp(optarg, "500000",6) == 0) baud_sel = BAUD_500000;
      else if (strncmp(optarg, "38400",5) == 0) baud_sel = BAUD_38400;
      else if (strncmp(optarg, "19200",5) == 0) baud_sel = BAUD_19200;
      else baud_sel = BAUD_9600;
      break;
    case 'h':
      printf("Syntax: %s [flags]\n",argv[0]);
      printf("Flags:\n");
      printf("  -n nrepeat       (number of repeat sets of measurements)\n");
      printf("  -r 180|100       (scan range in degree)\n");
      printf("  -s 0.25|0.5|1.0  (angular resolution in degree)\n");
      printf("  -u cm|mm         (measurement unit)\n");
      printf("  -p /dev/ttyS*    (the com port used)\n");
      printf("  -h               (help)\n");
      printf("  -b 9600|19200|38400|500000 (the baud rate to use)\n");
      printf("Version: 0.3.0 Fri Jul 23 10:50:04 NZST 2004\n");
      printf("This is a GPL program by David Yuen (d.yuen@auckland.ac.nz)\n");
      printf("It intends to demostrate the serial communication of LMS-200 laser scanner\n\n");
      return 0;
    }
  }
  
  // initialisation - put the LMS in a mode where it constantly sends data
  fd = connectToLMS(range_mode, res_mode, unit_mode, port, baud_sel);

  // Read back and print out that data
  while (count < count_max) {
    datalen = readLMSdata(fd, buf);
    if (datalen != 0) {
      showdata(datalen, buf+7);
      chkstatus(buf[datalen+7]);
      count++;
    }
  }

  stopLMS(fd);
  resetLMS(fd);
  return 0;
}