int i, length = 0, chk_sum = 0;  unsigned char ichar, ichar2;  if (!(serial_ready (fd, wait_time))) {    errorp = SERIAL_TIMEOUT_ERROR;    return(FALSE);  }  errorp = 0;    /* read the begin packet character, it should be 1 */  ichar = (unsigned char)GETC(fd, conn_type);  if (!errorp) {    if (ichar != 1) {      printf("start byte error: %u\n", ichar);      errorp = SERIAL_PKG_ERROR;    }    else {      chk_sum = 1;    }  }    if (!errorp) {    /* read length, it should be >0 */    ichar = GETC(fd, conn_type);     if (!errorp) {      chk_sum = chk_sum + (int)ichar;    }    ichar2 = GETC(fd, conn_type);    if (!errorp) {      length = two_bytes_to_unsigned_int (ichar, ichar2);      if (length < 1) {	printf("length byte error\n");	errorp = SERIAL_PKG_ERROR;      }      else {	chk_sum = chk_sum + (int)ichar2;      }    }  }    /* get the data portion of the message */  i = 0;  while ((!errorp) && (i<=length)) {    ichar = GETC(fd, conn_type);    if (!errorp) {      inbuf[i] = ichar;      /* printf("%u\n",  ichar); */      chk_sum = chk_sum + (int)(ichar);    }    i++;  }    if (!errorp) {    /* check chk_sum and end_pkg */    if (((chk_sum - inbuf[length-1] - 92) % 256) != inbuf[length-1]) {      printf("check sum error\n");      errorp = SERIAL_PKG_ERROR;    }        if (inbuf[length] != 92) {      printf("packet end error\n");      errorp = SERIAL_PKG_ERROR;    }  }    if ((errorp) && (errorp != SERIAL_TIMEOUT_ERROR)) {    printf("emptying buffer\n");    buf_fill(Fd, conn_type);  /* read everything else in the serial line into		    buffer */    bufp = bufe; /* and flush it */  }    if (errorp)    return(FALSE);  else    return (TRUE);}/********************************************************* * *   Laser Calibration Stuff  * *********************************************************//* Transformation function accordingly to the calibration *//* xi1 = pixel; yi1 = scanline */static void ProjectPhy(double xi1, double yi1, double *x, double *y){  double xi,yi;  double den;		    xi = xi1 - 254.5;  yi = yi1 - 240.5;     den = (LASER_CALIBRATION[0]*xi + LASER_CALIBRATION[1]*yi + 1);    *x = (LASER_CALIBRATION[2]*xi + LASER_CALIBRATION[3]*yi +	LASER_CALIBRATION[4])/den + LASER_OFFSET[0];  *y = (LASER_CALIBRATION[5]*xi + LASER_CALIBRATION[6]*yi +	LASER_CALIBRATION[7])/den + LASER_OFFSET[1];}  static void convert_laser(int *laser){  int i, num_points, offset, interval;  double line_num;  double laserx[483], lasery[483];    num_points = laser[0];  interval = NUM_LASER/(num_points-1);  offset = 3 + (NUM_LASER-(num_points * interval))/2;  for (i=1; i<=num_points; i++) {    line_num = (double)(offset+(i-1)*interval);    ProjectPhy((double)laser[i], line_num, &laserx[i], &lasery[i]);   }  for (i=1; i<=num_points; i++) {    laser[2*i-1] = (int)(laserx[i]*10.0);    laser[2*i] = (int)(lasery[i]*10.0);  }  return;}/********************************************************* * *   Processing different types of packages received from the robot  * *********************************************************//* process the response received from the robot which   encodes the state of the robot according to the mask */static void Process_State_Pkg(unsigned char inbuf[BUFSIZE]){  int i, byte_count = 1;  int low_half_used = FALSE;    /* infrared */  for (i = STATE_IR_0 ; i <= STATE_IR_15; i++)    if (usedSmask[i] > 0)       {	if (low_half_used == FALSE)	  {	    State[i] = low_half(inbuf[byte_count]);	    low_half_used = TRUE;	  }	else	  {	    State[i] = high_half(inbuf[byte_count]);	    byte_count++;	    low_half_used = FALSE;	  }      }  if (low_half_used == TRUE)    byte_count++;    /*   * if the pos attachment was required we read it   */    if (POS_INFRARED_PI(usedSmask[SMASK_POS_DATA]))    for (i = 0; i < INFRAREDS; i++)      if (usedSmask[SMASK_IR_1 + i] > 0) 	byte_count += posPackageProcess(&inbuf[byte_count], 					&(posDataAll.infrared[i]));    /* sonars */  for (i = STATE_SONAR_0; i <= STATE_SONAR_15; i++) {    if ( usedSmask[i] > 0 )       {	State[i] = inbuf[byte_count];	byte_count++;      }  }    /*   * if the pos attachment was required we read it   */    if (POS_SONAR_PI(usedSmask[SMASK_POS_DATA]))    for (i = 0; i < SONARS; i++)       if (usedSmask[SMASK_SONAR_1 + i] > 0)	byte_count += posPackageProcess(&inbuf[byte_count], 					&(posDataAll.sonar[i]));  if (usedSmask[ SMASK_BUMPER ] > 0)  {    if (model == MODEL_SCOUT)    {      State[ STATE_BUMPER ] = combine_bumper_vector(inbuf[byte_count + 0], 						    inbuf[byte_count + 1],						    inbuf[byte_count + 2]);    }    else    {      State[ STATE_BUMPER ] = combine_bumper_vector(inbuf[byte_count + 2], 						    inbuf[byte_count + 1],						    inbuf[byte_count + 0]);    }        byte_count = byte_count + 3;        /*     * if the position attachment was requested for the bumper     * we have to unpack the package.      */        if (POS_BUMPER_PI(usedSmask[SMASK_POS_DATA]))      byte_count += posPackageProcess(&inbuf[byte_count], 				      &(posDataAll.bumper));  }    /* the position data */    if (usedSmask[SMASK_CONF_X] > 0)  {    State[STATE_CONF_X] =  two_bytes_to_signed_int(inbuf[byte_count],						   inbuf[byte_count+1]);    byte_count = byte_count + 2;  }    if (usedSmask[SMASK_CONF_Y] > 0)  {    State[STATE_CONF_Y] = two_bytes_to_signed_int(inbuf[byte_count],						  inbuf[byte_count+1]);    byte_count = byte_count + 2;  }    if (usedSmask[SMASK_CONF_STEER] > 0)  {    State[STATE_CONF_STEER] = two_bytes_to_signed_int(inbuf[byte_count], 						      inbuf[byte_count+1]);    byte_count = byte_count + 2;  }    if (usedSmask[SMASK_CONF_TURRET] > 0)  {    State[STATE_CONF_TURRET] = two_bytes_to_signed_int(inbuf[byte_count], 						       inbuf[byte_count+1]);    byte_count = byte_count + 2;  }    /* the velocities */    if (usedSmask[SMASK_VEL_TRANS] > 0)  {    State[STATE_VEL_TRANS] = two_bytes_to_signed_int(inbuf[byte_count], 						     inbuf[byte_count+1]);    byte_count = byte_count + 2;  }    if (usedSmask[SMASK_VEL_STEER] > 0)  {    State[SMASK_VEL_STEER] = two_bytes_to_signed_int(inbuf[byte_count],						     inbuf[byte_count+1]);    byte_count = byte_count + 2;  }    if (usedSmask[SMASK_VEL_TURRET] > 0)  {    State[STATE_VEL_TURRET] = two_bytes_to_signed_int(inbuf[byte_count],						      inbuf[byte_count+1]);    byte_count = byte_count + 2;  }    /* the compass value */  if (usedSmask[SMASK_COMPASS] > 0)  {    State[STATE_COMPASS] = two_bytes_to_signed_int(inbuf[byte_count],						   inbuf[byte_count+1]);    byte_count = byte_count + 2;          /*     * if the position attachment was requested for the compass     * we have to unpack the package.      */        if (POS_COMPASS_PI(usedSmask[SMASK_POS_DATA]))      byte_count += posPackageProcess(&inbuf[byte_count], 				      &(posDataAll.compass));  }  /* laser */  if (usedSmask[SMASK_LASER] > 0)  {    /* the number of points */    Laser[0] = two_bytes_to_unsigned_int(inbuf[byte_count],					 inbuf[byte_count+1]);    byte_count = byte_count + 2;        /* check the laser mode */    if ((laser_mode&0x1e) == 0) /* Line mode */    {      if (Laser[0] > NUM_LASER/2)      {	printf("error in processing laser reply (1).\n");	errorp = SERIAL_READ_ERROR;	Laser[0] = 0;	return;      }      for (i=1; i<=4*Laser[0]; i++)       {	Laser[i] = two_bytes_to_signed_int(inbuf[byte_count],					   inbuf[byte_count+1]);	byte_count = byte_count+2;      }    }    else /* Points of some kind */    {      if (Laser[0] > NUM_LASER)       {	printf("error in processing laser reply (2).\n");	errorp = SERIAL_READ_ERROR;	Laser[0] = 0;	return;      }      for (i=1; i<=Laser[0]; i++)       {	Laser[i] = two_bytes_to_unsigned_int(inbuf[byte_count],					     inbuf[byte_count+1]);	byte_count = byte_count+2;      }    }    if ((laser_mode&0x1e) == 19)      convert_laser(Laser);        /*     * if the position attachment was requested for the laser     * we have to get it from somewhere else      */        if (POS_LASER_PI(usedSmask[SMASK_POS_DATA]))      byte_count += posPackageProcess(&inbuf[byte_count], 				      &(posDataAll.laser));  }  /* motor active */  State[STATE_MOTOR_STATUS] = (long)inbuf[byte_count++];    /* process the 6811 time */  byte_count += timePackageProcess(&inbuf[byte_count], &posDataTime);  /* process the voltages of motor/CPU */  byte_count += voltPackageProcess(&inbuf[byte_count], 				   &voltageCPU, &voltageMotor);}/* process the response from the robot which encodes the   active infrared reading */static void Process_Infrared_Pkg(unsigned char inbuf[BUFSIZE]){  int i, byte_count = 1;  int low_half_used = FALSE;  /*    * the ir datum from one sensor is only a nibble,    * two of them are merged into one byte    */    for (i = STATE_IR_0 ; i <=  STATE_IR_15; i++)    if (low_half_used == FALSE)       {	State[i] = low_half(inbuf[byte_count]);	low_half_used = TRUE;      }    else       {	State[i] = high_half(inbuf[byte_count]);	byte_count++;	low_half_used = FALSE;      }  /* align with next byte */  if ( low_half_used )    byte_count++;  /*   * if the pos attachment was required we read it   */  if ( POS_INFRARED_PI ( usedSmask[ SMASK_POS_DATA ] ) )  {    for (i=0; i<16; i++)      byte_count += posPackageProcess ( &inbuf[byte_count], 				        &( posDataAll.infrared[i] ) );  }  /* extract the time data for the 6811 */  byte_count += timePackageProcess ( &inbuf[byte_count], &posDataTime );}/* process the response from the robot which encodes the   active sonar reading */static void Process_Sonar_Pkg(unsigned char inbuf[BUFSIZE]){  int i, byte_count = 1;  /*   * read the sensory data from the buffer   */  for (i = STATE_SONAR_0; i <= STATE_SONAR_15; i++)     {      State[i] = inbuf[byte_count];      byte_count++;    }  /*   * if the pos attachment was required we read it   */  if ( POS_SONAR_PI ( usedSmask[ SMASK_POS_DATA ]) )    for (i=0; i<16; i++)       byte_count += posPackageProcess ( &inbuf[byte_count], 				        &( posDataAll.sonar[i] ) );      /* extract the time data for the 6811 */  byte_count += timePackageProcess ( &inbuf[byte_count], &posDataTime );}/* process the response from the robot which encodes the   configuration of the robot */static void Process_Configuration_Pkg(unsigned char inbuf[BUFSIZE]){  int byte_count = 1;    State[ STATE_CONF_X ] = two_bytes_to_signed_int(inbuf[byte_count],						  inbuf[byte_count+1]);  byte_count = byte_count + 2;    State[ STATE_CONF_Y ] = two_bytes_to_signed_int(inbuf[byte_count],						  inbuf[byte_count+1]);  byte_count = byte_count + 2;    State[ STATE_CONF_STEER ] = two_bytes_to_signed_int(inbuf[byte_count],						      inbuf[byte_count+1]);  byte_count = byte_count + 2;    State[ STATE_CONF_TURRET ] = two_bytes_to_signed_int(inbuf[byte_count],						       inbuf[byte_count+1]);}static void Process_Velocity_Pkg(unsigned char inbuf[BUFSIZE]){  int byte_count = 1;    State[ STATE_VEL_TRANS ] = two_bytes_to_signed_int(inbuf[byte_count],						     inbuf[byte_count+1]);  byte_count = byte_count + 2;    State[ STATE_VEL_STEER ] = two_bytes_to_signed_int(inbuf[byte_count],						     inbuf[byte_count+1]);  byte_count = byte_count + 2;    State[ STATE_VEL_TURRET ] = two_bytes_to_signed_int(inbuf[byte_count],						      inbuf[byte_count+1]);}static void Process_Acceleration_Pkg(unsigned char inbuf[BUFSIZE] __attribute__ ((unused))){}/* process the response from the robot which encodes the   compass reading of the robot */static void Process_Compass_Pkg(unsigned char inbuf[BUFSIZE]){  int byte_count = 1;    State[ STATE_COMPASS ] = two_bytes_to_unsigned_int(inbuf[byte_count],						     inbuf[byte_count+1]);  byte_count +=2;  /*   * if the position attachment was requested for the compass   * we have to unpack the package.    */  if ( POS_COMPASS_PI ( usedSmask[ SMASK_POS_DATA ] ) )    byte_count += posPackageProcess ( &inbuf[byte_count],   				      &( posDataAll.compass ) );