rmp_ftd2xx.cc
Go to the documentation of this file.
00001 #include "segwayrmp/segwayrmp.h"
00002 #include "segwayrmp/impl/rmp_ftd2xx.h"
00003 
00004 #include <iostream>
00005 #include <sstream>
00006 
00007 using namespace segwayrmp;
00008 
00009 static const bool ftd2xx_devices_debug = false;
00010 
00011 inline std::string
00012 getErrorMessageByFT_STATUS(FT_STATUS result, std::string what)
00013 {
00014   std::stringstream msg;
00015   msg << "FTD2XX error while " << what << ": ";
00016   switch (result) {
00017     case FT_INVALID_HANDLE:
00018         msg << "FT_INVALID_HANDLE";
00019         break;
00020     case FT_DEVICE_NOT_FOUND:
00021         msg << "FT_DEVICE_NOT_FOUND";
00022         break;
00023     case FT_DEVICE_NOT_OPENED:
00024         msg << "FT_DEVICE_NOT_OPENED";
00025         break;
00026     case FT_IO_ERROR:
00027         msg << "FT_IO_ERROR";
00028         break;
00029     case FT_INSUFFICIENT_RESOURCES:
00030         msg << "FT_INSUFFICIENT_RESOURCES";
00031         break;
00032     case FT_INVALID_PARAMETER:
00033         msg << "FT_INVALID_PARAMETER";
00034         break;
00035     case FT_INVALID_BAUD_RATE:
00036         msg << "FT_INVALID_BAUD_RATE";
00037         break;
00038     case FT_DEVICE_NOT_OPENED_FOR_ERASE:
00039         msg << "FT_DEVICE_NOT_OPENED_FOR_ERASE";
00040         break;
00041     case FT_DEVICE_NOT_OPENED_FOR_WRITE:
00042         msg << "FT_DEVICE_NOT_OPENED_FOR_WRITE";
00043         break;
00044     case FT_FAILED_TO_WRITE_DEVICE:
00045         msg << "FT_FAILED_TO_WRITE_DEVICE";
00046         break;
00047     case FT_EEPROM_READ_FAILED:
00048         msg << "FT_EEPROM_READ_FAILED";
00049         break;
00050     case FT_EEPROM_WRITE_FAILED:
00051         msg << "FT_EEPROM_WRITE_FAILED";
00052         break;
00053     case FT_EEPROM_ERASE_FAILED:
00054         msg << "FT_EEPROM_ERASE_FAILED";
00055         break;
00056     case FT_EEPROM_NOT_PRESENT:
00057         msg << "FT_EEPROM_NOT_PRESENT";
00058         break;
00059     case FT_EEPROM_NOT_PROGRAMMED:
00060         msg << "FT_EEPROM_NOT_PROGRAMMED";
00061         break;
00062     case FT_INVALID_ARGS:
00063         msg << "FT_INVALID_ARGS";
00064         break;
00065     case FT_NOT_SUPPORTED:
00066         msg << "FT_NOT_SUPPORTED";
00067         break;
00068     case FT_OTHER_ERROR:
00069         msg << "FT_OTHER_ERROR";
00070         break;
00071     default:
00072         msg << "Unknown FTD2XX Error.";
00073         break;
00074   }
00075   return msg.str();
00076 }
00077 
00078 std::vector<FT_DEVICE_LIST_INFO_NODE> segwayrmp::enumerateUSBDevices() {
00079     FT_STATUS result;
00080     FT_DEVICE_LIST_INFO_NODE *device_info;
00081     DWORD number_of_devices;
00082 
00083     // If mac or linux you must set VID/PID
00084     #ifndef _WIN32
00085     DWORD FTDI_VID = 0x403;
00086     DWORD SEGWAY_PID = 0xe729;
00087 
00088     try {
00089       result = FT_SetVIDPID(FTDI_VID, SEGWAY_PID);
00090     } catch(std::exception &e) {
00091       RMP_THROW_MSG(ReadFailedException, e.what());
00092     }
00093     if (result != FT_OK) {
00094       RMP_THROW_MSG(ReadFailedException,
00095         getErrorMessageByFT_STATUS(result, "setting vid and pid").c_str());
00096     }
00097     #endif
00098 
00099     try {
00100       result = FT_CreateDeviceInfoList(&number_of_devices);
00101     } catch(std::exception &e) {
00102       RMP_THROW_MSG(ReadFailedException, e.what());
00103     }
00104     if (result != FT_OK) {
00105       RMP_THROW_MSG(ReadFailedException,
00106         getErrorMessageByFT_STATUS(result, "enumerating devices").c_str());
00107     }
00108 
00109     if (ftd2xx_devices_debug)
00110       std::cout << "Number of devices is: " << number_of_devices << std::endl;
00111 
00112     std::vector<FT_DEVICE_LIST_INFO_NODE> devices;
00113     if (number_of_devices > 0) {
00114       device_info = (FT_DEVICE_LIST_INFO_NODE*)malloc(
00115           sizeof(FT_DEVICE_LIST_INFO_NODE)*number_of_devices
00116         );
00117       try {
00118         result = FT_GetDeviceInfoList(device_info, &number_of_devices);
00119       } catch(std::exception &e) {
00120         RMP_THROW_MSG(ReadFailedException, e.what());
00121       }
00122       if (result != FT_OK) {
00123         RMP_THROW_MSG(ReadFailedException,
00124           getErrorMessageByFT_STATUS(result, "enumerating devices").c_str());
00125       }
00126       for (int i = 0; i < number_of_devices; i++) {
00127           devices.push_back(device_info[i]);
00128         if (ftd2xx_devices_debug) {
00129           printf("Dev %d:\n", i);
00130           printf(" Flags=0x%x\n", device_info[i].Flags);
00131           printf(" Type=0x%x\n", device_info[i].Type);
00132           printf(" ID=0x%x\n", device_info[i].ID);
00133           printf(" LocId=0x%x\n", device_info[i].LocId);
00134           printf(" SerialNumber=%s\n", device_info[i].SerialNumber);
00135           printf(" Description=%s\n", device_info[i].Description);
00136         }
00137       }
00138     }
00139     return devices;
00140 }
00141 
00143 // FTD2XXRMPIO
00144 
00145 FTD2XXRMPIO::FTD2XXRMPIO()
00146 : configured(false), config_type(by_none), baudrate(460800), is_open(false)
00147 {
00148   this->port_serial_number = "";
00149   this->port_description = "";
00150   this->port_index = 0;
00151   this->connected = false;
00152   this->usb_port_handle = NULL;
00153 }
00154 
00155 FTD2XXRMPIO::~FTD2XXRMPIO() {
00156   this->disconnect();
00157 }
00158 
00159 void FTD2XXRMPIO::connect() {
00160   if(!this->configured) {
00161     RMP_THROW_MSG(ConnectionFailedException, "The usb port must be "
00162       "configured before connecting.");
00163   }
00164   
00165   FT_STATUS result;
00166   
00167   this->enumerateUSBDevices_();
00168   
00169   // Select connection method and open
00170   switch (this->config_type) {
00171     case by_serial_number:
00172       this->connectBySerial();
00173       break;
00174     case by_description:
00175       this->connectByDescription();
00176       break;
00177     case by_index:
00178       this->connectByIndex();
00179       break;
00180     case by_none:
00181     default:
00182       RMP_THROW_MSG(ConnectionFailedException, "The usb port must be "
00183         "configured before connecting.");
00184         break;
00185   }
00186   
00187   this->is_open = true;
00188   
00189   // Configure the Baudrate
00190   try {
00191     result = FT_SetBaudRate(this->usb_port_handle, this->baudrate);
00192   } catch(std::exception &e) {
00193     RMP_THROW_MSG(ConnectionFailedException, e.what());
00194   }
00195   if (result != FT_OK) {
00196     RMP_THROW_MSG(ConnectionFailedException,
00197       getErrorMessageByFT_STATUS(result, "setting the baudrate").c_str());
00198   }
00199   
00200   // Set default timeouts
00201   try {
00202     // 1 sec read and 1 sec write
00203     result = FT_SetTimeouts(this->usb_port_handle, 1000, 1000);
00204   } catch(std::exception &e) {
00205     RMP_THROW_MSG(ConnectionFailedException, e.what());
00206   }
00207   if (result != FT_OK) {
00208     RMP_THROW_MSG(ConnectionFailedException,
00209       getErrorMessageByFT_STATUS(result, "setting timeouts").c_str());
00210   }
00211   
00212   // Set Latency Timer
00213   try {
00214     result = FT_SetLatencyTimer(this->usb_port_handle, 1);
00215   } catch(std::exception &e) {
00216     RMP_THROW_MSG(ConnectionFailedException, e.what());
00217   }
00218   if (result != FT_OK) {
00219     RMP_THROW_MSG(ConnectionFailedException,
00220       getErrorMessageByFT_STATUS(result, "setting latency timer").c_str());
00221   }
00222   
00223   // Set flowcontrol
00224   try {
00225     result = FT_SetFlowControl(this->usb_port_handle, FT_FLOW_NONE, 0, 0);
00226   } catch(std::exception &e) {
00227     RMP_THROW_MSG(ConnectionFailedException, e.what());
00228   }
00229   if (result != FT_OK) {
00230     RMP_THROW_MSG(ConnectionFailedException,
00231       getErrorMessageByFT_STATUS(result, "setting flowcontrol").c_str());
00232   }
00233   
00234   // Purge the I/O buffers of the usb device
00235   try {
00236     result = FT_Purge(this->usb_port_handle, FT_PURGE_RX | FT_PURGE_TX);
00237   } catch(std::exception &e) {
00238     RMP_THROW_MSG(ConnectionFailedException, e.what());
00239   }
00240   if (result != FT_OK) {
00241     RMP_THROW_MSG(ConnectionFailedException,
00242       getErrorMessageByFT_STATUS(result, "purging the buffers").c_str());
00243   }
00244   
00245   this->connected = true;
00246 }
00247 
00248 void FTD2XXRMPIO::disconnect() {
00249   if(this->connected) {
00250     if (this->is_open) {
00251       FT_Close(this->usb_port_handle);
00252     }
00253     this->connected = false;
00254   }
00255 }
00256 
00257 int FTD2XXRMPIO::read(unsigned char* buffer, int size) {
00258   FT_STATUS result;
00259   DWORD bytes_read;
00260   
00261   try {
00262     result = FT_Read(this->usb_port_handle, buffer, size, &bytes_read);
00263   } catch(std::exception &e) {
00264     RMP_THROW_MSG(ReadFailedException, e.what());
00265   }
00266   if (result != FT_OK) {
00267     RMP_THROW_MSG(ReadFailedException,
00268       getErrorMessageByFT_STATUS(result, "reading").c_str());
00269   }
00270   
00271   return bytes_read;
00272 }
00273 
00274 int FTD2XXRMPIO::write(unsigned char* buffer, int size) {
00275   FT_STATUS result;
00276   DWORD bytes_written;
00277   
00278   try {
00279     result = FT_Write(this->usb_port_handle, buffer, size, &bytes_written);
00280   } catch(std::exception &e) {
00281     RMP_THROW_MSG(ReadFailedException, e.what());
00282   }
00283   if (result != FT_OK) {
00284     RMP_THROW_MSG(ReadFailedException,
00285       getErrorMessageByFT_STATUS(result, "reading").c_str());
00286   }
00287   
00288   return bytes_written;
00289 }
00290 
00291 std::vector<FT_DEVICE_LIST_INFO_NODE> FTD2XXRMPIO::enumerateUSBDevices_() {
00292   return enumerateUSBDevices();
00293 }
00294 
00295 void
00296 FTD2XXRMPIO::configureUSBBySerial(std::string serial_number, int baudrate)
00297 {
00298   this->config_type = by_serial_number;
00299   this->port_serial_number = serial_number;
00300   this->configured = true;
00301   this->baudrate = baudrate;
00302 }
00303 
00304 void FTD2XXRMPIO::connectBySerial() {
00305   FT_STATUS result;
00306   DWORD number_of_devices;
00307   
00308   try {
00309     // Open the usb port
00310     result = FT_OpenEx((PVOID)this->port_serial_number.c_str(),
00311                        (DWORD)FT_OPEN_BY_SERIAL_NUMBER,
00312                        &(this->usb_port_handle));
00313   } catch(std::exception &e) {
00314     RMP_THROW_MSG(ConnectionFailedException, e.what());
00315   }
00316   if (this->usb_port_handle == NULL) {
00317     // Failed to open port
00318     RMP_THROW_MSG(ConnectionFailedException,
00319       getErrorMessageByFT_STATUS(result, "opening the usb port").c_str());
00320   }
00321 }
00322 
00323 void
00324 FTD2XXRMPIO::configureUSBByDescription(std::string description, int baudrate)
00325 {
00326   this->config_type = by_description;
00327   this->port_description = description;
00328   this->configured = true;
00329   this->baudrate = baudrate;
00330 }
00331 
00332 void FTD2XXRMPIO::connectByDescription() {
00333   FT_STATUS result;
00334   
00335   try {
00336     // Open the usb port
00337     result = FT_OpenEx(const_cast<char *>(this->port_description.c_str()), 
00338                        FT_OPEN_BY_DESCRIPTION, 
00339                        &(this->usb_port_handle));
00340   } catch(std::exception &e) {
00341     RMP_THROW_MSG(ConnectionFailedException, e.what());
00342   }
00343   if (result != FT_OK) {
00344     // Failed to open port
00345     RMP_THROW_MSG(ConnectionFailedException,
00346       getErrorMessageByFT_STATUS(result, "opening the usb port").c_str());
00347   }
00348 }
00349 
00350 void
00351 FTD2XXRMPIO::configureUSBByIndex(unsigned int device_index, int baudrate)
00352 {
00353   this->config_type = by_index;
00354   this->port_index = device_index;
00355   this->configured = true;
00356   this->baudrate = baudrate;
00357 }
00358 
00359 void FTD2XXRMPIO::connectByIndex() {
00360   FT_STATUS result;
00361   
00362   try {
00363     // Open the usb port
00364     result = FT_Open(this->port_index, &(this->usb_port_handle));
00365   } catch(std::exception &e) {
00366     RMP_THROW_MSG(ConnectionFailedException, e.what());
00367   }
00368   if (this->usb_port_handle == NULL) {
00369     // Failed to open port
00370     RMP_THROW_MSG(ConnectionFailedException,
00371       getErrorMessageByFT_STATUS(result, "opening the usb port").c_str());
00372   }
00373 }
00374 


libsegwayrmp
Author(s): William Woodall
autogenerated on Fri Jan 3 2014 11:28:21