mip_sdk_user_functions.c
Go to the documentation of this file.
00001 
00015 /* -- Includes -- */
00016 #include "mip_sdk_user_functions.h"
00017 
00018 
00038 u16 purge(ComPortHandle comPortHandle){
00039 
00040  if (tcflush(comPortHandle,TCIOFLUSH)==-1){
00041 
00042   printf("flush failed\n");
00043   return MIP_USER_FUNCTION_ERROR;
00044 
00045  }
00046 
00047  return MIP_USER_FUNCTION_OK;
00048 
00049 }
00050 
00068 u16 mip_sdk_port_open(void **port_handle, const char *portstr, int baudrate)
00069 {
00070  char port_name[100]  = {0};
00071  static ComPortHandle local_port_handle;
00072  int hardware_bit_baud, status;
00073  struct termios options;
00074 
00075  // Copy portstr argument to port_name
00076  strcat(port_name,portstr);
00077  //printf("Attempting to open port: %s\n",port_name);
00078  //Attempt to open the specified port
00079  local_port_handle = open(port_name, O_RDWR | O_NOCTTY);
00080 
00081  //Check for an invalid handle
00082  if(local_port_handle == -1)
00083  {
00084    printf("Unable to open com Port %s\n Errno = %i\n", port_name, errno);
00085 
00086   return MIP_USER_FUNCTION_ERROR;
00087  }
00088  //printf("Port: %s opened successfully.\n",port_name);
00089  
00090  //Convert specified baud to hardware specific value
00091  switch (baudrate)
00092  {
00093  case 0:
00094   hardware_bit_baud = B0;
00095   break;
00096 
00097  case 50:
00098   hardware_bit_baud = B50;
00099   break;
00100 
00101  case 75:
00102   hardware_bit_baud = B75;
00103   break;
00104 
00105  case 110:
00106   hardware_bit_baud = B110;
00107   break;
00108 
00109  case 134:
00110   hardware_bit_baud = B134;
00111   break;
00112 
00113  case 150:
00114   hardware_bit_baud = B150;
00115   break;
00116 
00117  case 200:
00118   hardware_bit_baud = B200;
00119   break;
00120 
00121  case 300:
00122   hardware_bit_baud = B300;
00123   break;
00124 
00125  case 600:
00126   hardware_bit_baud = B600;
00127   break;
00128 
00129  case 1200:
00130   hardware_bit_baud = B1200;
00131   break;
00132 
00133  case 1800:
00134   hardware_bit_baud = B1800;
00135   break;
00136 
00137  case 2400:
00138   hardware_bit_baud = B2400;
00139   break;
00140 
00141  case 4800:
00142   hardware_bit_baud = B4800;
00143   break;
00144 
00145  case 9600:
00146   hardware_bit_baud = B9600;
00147   break;
00148 
00149  case 19200:
00150   hardware_bit_baud = B19200;
00151   break;
00152 
00153  case 38400:
00154   hardware_bit_baud = B38400;
00155   break;
00156 
00157 # ifdef B7200
00158  case 7200:
00159   hardware_bit_baud = B7200;
00160   break;
00161 
00162 # endif
00163 
00164 # ifdef B14400
00165  case 14400:
00166   hardware_bit_baud = B14400;
00167   break;
00168 
00169 # endif
00170 
00171 # ifdef B57600
00172  case 57600:
00173   hardware_bit_baud = B57600;
00174   break;
00175 
00176 # endif
00177 
00178 # ifdef B115200
00179  case 115200:
00180   hardware_bit_baud = B115200;
00181   break;
00182 
00183 # endif
00184 
00185 # ifdef B230400
00186  case 230400:
00187   hardware_bit_baud = B230400;
00188   break;
00189 
00190 # endif
00191 
00192 # ifdef B460800
00193  case 460800:
00194   hardware_bit_baud = B460800;
00195   break;
00196 
00197 # endif
00198 
00199 # ifdef B500000
00200  case 500000:
00201   hardware_bit_baud = B500000;
00202   break;
00203 
00204 # endif
00205 
00206 # ifdef B576000
00207  case 576000:
00208   hardware_bit_baud = B576000;
00209   break;
00210 
00211 # endif
00212 
00213 # ifdef B921600
00214  case 921600:
00215   hardware_bit_baud = B921600;
00216   break;
00217 
00218 # endif
00219 
00220 # ifdef B1000000
00221  case 1000000:
00222   hardware_bit_baud = B1000000;
00223   break;
00224 
00225 # endif
00226 
00227 # ifdef B1152000
00228  case 1152000:
00229   hardware_bit_baud = B1152000;
00230   break;
00231 
00232 # endif
00233 
00234 # ifdef B2000000
00235  case 2000000:
00236   hardware_bit_baud = B2000000;
00237   break;
00238 
00239 # endif
00240 
00241 # ifdef B3000000
00242  case 3000000:
00243   hardware_bit_baud = B3000000;
00244   break;
00245 
00246 # endif
00247 
00248 # ifdef B3500000
00249  case 3500000:
00250   hardware_bit_baud = B3500000;
00251   break;
00252 
00253 # endif
00254 
00255 # ifdef B4000000
00256  case 4000000:
00257   hardware_bit_baud = B4000000;
00258   break;
00259 
00260 # endif
00261  //Unsupported baud specified
00262  default:
00263   printf("Unsupported baud specified\n");
00264   return MIP_USER_FUNCTION_ERROR;
00265 
00266  }
00267 
00268  //Get the current settings for the port...
00269  tcgetattr(local_port_handle, &options);
00270 
00271  //set the baud rate
00272  cfsetospeed(&options, hardware_bit_baud);
00273  cfsetispeed(&options, hardware_bit_baud);
00274 
00275  //set the number of data bits.
00276  options.c_cflag &= ~CSIZE; // Mask the character size bits
00277  options.c_cflag |= CS8;
00278 
00279  //set the number of stop bits to 1
00280  options.c_cflag &= ~CSTOPB;
00281 
00282  //Set parity to None
00283  options.c_cflag &=~PARENB;
00284 
00285  //set for non-canonical (raw processing, no echo, etc.)
00286  options.c_iflag = IGNPAR; // ignore parity check close_port(int
00287  options.c_oflag = 0; // raw output
00288  options.c_lflag = 0; // raw input
00289 
00290  //Time-Outs -- won't work with NDELAY option in the call to open
00291  options.c_cc[VMIN] = 0;  // block reading until RX x characers. If x = 0, 
00292                // it is non-blocking.
00293  options.c_cc[VTIME] = 1;  // Inter-Character Timer -- i.e. timeout= x*.1 s
00294 
00295  //Set local mode and enable the receiver
00296  options.c_cflag |= (CLOCAL | CREAD);
00297 
00298  //purge serial port buffers
00299  if(purge(local_port_handle) != MIP_USER_FUNCTION_OK){
00300   printf("Flushing old serial buffer data failed\n");
00301   return MIP_USER_FUNCTION_ERROR;
00302  }
00303  //attempt to apply settings
00304  status=tcsetattr(local_port_handle, TCSANOW, &options); 
00305 
00306  if (status != 0){ //For error message
00307 
00308   printf("Configuring comport failed\n");
00309   return MIP_USER_FUNCTION_ERROR;
00310 
00311  }
00312 
00313  //Purge serial port buffers
00314  if(purge(local_port_handle) != MIP_USER_FUNCTION_OK){
00315   printf("Post configuration serial buffer flush failed\n");
00316   return MIP_USER_FUNCTION_ERROR;
00317  }
00318 
00319  //assign external pointer to port handle pointer
00320  *port_handle = &local_port_handle;
00321 
00322  return MIP_USER_FUNCTION_OK;
00323 }
00324 
00325 
00327 //
00330 //
00333 //
00337 //
00340 //
00346 //
00348 
00349 u16 mip_sdk_port_close(void *port_handle)
00350 {
00351  int local_port_handle = *((int *)port_handle);
00352  
00353  if(port_handle == NULL)
00354   return MIP_USER_FUNCTION_ERROR;
00355  
00356  //Close the serial port
00357  close(local_port_handle);
00358  
00359  return MIP_USER_FUNCTION_OK; 
00360 }
00361 
00362 
00364 //
00367 //
00370 //
00378 //
00381 //
00387 //
00389 
00390 u16 mip_sdk_port_write(void *port_handle, u8 *buffer, u32 num_bytes, u32 *bytes_written, u32 timeout_ms)
00391 {
00392  int local_port_handle = *((int *)port_handle);
00393  static char print_once = 0;
00394 
00395  int local_bytes_written = write(local_port_handle, buffer, num_bytes); 
00396 
00397  if(local_bytes_written == -1)
00398   return MIP_USER_FUNCTION_ERROR;
00399 
00400  *bytes_written = local_bytes_written;
00401  if(*bytes_written == num_bytes)
00402   return MIP_USER_FUNCTION_OK;
00403  else
00404   return MIP_USER_FUNCTION_ERROR;
00405 }
00406 
00407 
00409 //
00412 //
00415 //
00423 //
00426 //
00432 //
00434 
00435 u16 mip_sdk_port_read(void *port_handle, u8 *buffer, u32 num_bytes, u32 *bytes_read, u32 timeout_ms)
00436 {
00437  int local_port_handle = *((int *)port_handle);
00438 
00439  int local_bytes_read = read(local_port_handle, buffer, num_bytes);
00440 
00441  *bytes_read = local_bytes_read;
00442  if(*bytes_read == num_bytes)
00443   return MIP_USER_FUNCTION_OK;
00444  else
00445   return MIP_USER_FUNCTION_ERROR;
00446 }
00447 
00448 
00450 //
00453 //
00456 //
00460 //
00463 //
00469 //
00471 
00472 u32 mip_sdk_port_read_count(void *port_handle)
00473 {
00474  int local_port_handle = *((int *)port_handle);
00475 
00476  int bytes_available;
00477  ioctl(local_port_handle, FIONREAD, &bytes_available);
00478  return (u32)bytes_available;
00479 }
00480 
00481 
00483 //
00486 //
00489 //
00493 //
00495 //
00506 //
00508 
00509 u32 mip_sdk_get_time_ms()
00510 {
00511 
00512  struct timespec ts;                                                            
00513                                                                                 
00514  if(clock_gettime(CLOCK_MONOTONIC,&ts) != 0) {                                  
00515   return -1;                                                                    
00516  }                                                                              
00517                                                                                 
00518  return (u32)(ts.tv_sec* 1000ll + ((ts.tv_nsec / 1000000)));
00519 }


microstrain_3dm_gx5_45
Author(s): Brian Bingham
autogenerated on Tue Apr 18 2017 02:59:09