ezn64_usb_control_lib.cpp
Go to the documentation of this file.
00001 /*********************************************************************************************/
00031 /* Author: Frantisek Durovsky */
00032 
00033 #ifndef EZN64_USB_CONTROL_LIB_CPP
00034 #define EZN64_USB_CONTROL_LIB_CPP
00035 
00036 #include <ezn64_usb_control.h>
00037 namespace schunk_ezn64
00038 {
00039 
00040 EZN64_usb::EZN64_usb(ros::NodeHandle *nh) :
00041     act_position_(-1),
00042     ezn64_error_(0xff)
00043 {
00044   //Read launch file params
00045   nh->getParam("schunk_ezn64/gripper_id", gripper_id_);
00046   nh->getParam("schunk_ezn64/vendor_id",  vendor_id_);
00047   nh->getParam("schunk_ezn64/product_id", product_id_);
00048      
00049   //Look for Schunk EZN64 controller on USB
00050   ezn64_dev_ = find_ezn64_dev(vendor_id_, product_id_);
00051   print_libusb_dev(ezn64_dev_);
00052   
00053   //If EZN64 found successfully, initialize interface
00054   if(ezn64_dev_ != 0)
00055   {   
00056     ezn64_handle_ = open_ezn64_dev(ezn64_dev_);
00057     
00058     //Get initial state and discard input buffer
00059     while (ezn64_error_ == 0xff)
00060     {
00061       ezn64_error_ = getError(ezn64_handle_);
00062       ros::Duration(WAIT_FOR_RESPONSE_INTERVAL).sleep();
00063     }
00064     
00065     //If emergency stop error occured, acknowledge
00066     if(ezn64_error_ == 0xd9)
00067     {
00068       ROS_INFO_STREAM("Acknowleding Emergency stop error ...");
00069       acknowledgeError(ezn64_handle_);
00070     }
00071   
00072     //Start periodic gripper position reading
00073     getPeriodicPositionUpdate(ezn64_handle_, TF_UPDATE_PERIOD);     
00074   } 
00075 }
00076 
00077 EZN64_usb::~EZN64_usb()
00078 {
00079   close_ezn64_dev(ezn64_handle_, usb_context_);
00080 }
00081 
00082 void
00083 EZN64_usb::reference(libusb_device_handle *handle)
00084 {
00085   ROS_INFO("EZN64: Referencing");
00086   std::vector<uint8_t> output;
00087   output.push_back(0x01);           //Data Length
00088   output.push_back(0x92);           //Command reference
00089 
00090   //Send message to the gripper
00091   usb_write(handle, output);
00092 }
00093 
00094 uint8_t
00095 EZN64_usb::getError(libusb_device_handle *handle)
00096 {
00097   std::vector<uint8_t> output;
00098   output.push_back(0x06);                 //Data Length
00099   output.push_back(0x95);                 //Command get state
00100   output.push_back(0x00);
00101   output.push_back(0x00);
00102   output.push_back(0x00);
00103   output.push_back(0x00);
00104   output.push_back(0x01);
00105 
00106   //Send get_state message to the module and recieve response
00107   usb_write(handle, output);
00108 
00109   ros::Duration(WAIT_FOR_RESPONSE_INTERVAL).sleep();
00110   std::vector<uint8_t> input;
00111   input = usb_read(handle);
00112 
00113   //Read and process current state message
00114   int get_state_index = 0;
00115   if(input.size() > 0)
00116   {
00117     if(input.size() > 7)
00118     {
00119       for(size_t i = 0; i < input.size(); i++)
00120         if ((input[i] == 0x07) && (input[i+1] == 0x95)) get_state_index = i;
00121 
00122       int ezn64_error_ = input[get_state_index + 7];
00123 
00124       switch (ezn64_error_)
00125       {
00126         case 0x00: ROS_INFO("EZN64: No error detected"); break;
00127         case 0xC8: ROS_ERROR("EZN64: Error: 0xC8 detected: Wrong ramp type"); break;
00128         case 0xD2: ROS_ERROR("EZN64: Error: 0xD2 detected: Config memory"); break;
00129         case 0xD3: ROS_ERROR("EZN64: Error: 0xD3 detected: Program memory"); break;
00130         case 0xD4: ROS_ERROR("EZN64: Error: 0xD4 detected: Invalid phrase"); break;
00131         case 0xD5: ROS_ERROR("EZN64: Error: 0xD5 detected: Soft low"); break;
00132         case 0xD6: ROS_ERROR("EZN64: Error: 0xD6 detected: Soft high"); break;
00133         case 0xD7: ROS_ERROR("EZN64: Error: 0xD7 detected: Pressure"); break;
00134         case 0xD8: ROS_ERROR("EZN64: Error: 0xD8 detected: Service required"); break;
00135         case 0xD9: ROS_ERROR("EZN64: Error: 0xD9 detected: Emergency stop"); break;
00136         case 0xDA: ROS_ERROR("EZN64: Error: 0xDA detected: Tow"); break;
00137         case 0xE4: ROS_ERROR("EZN64: Error: 0xE4 detected: Too fast"); break;
00138         case 0xEC: ROS_ERROR("EZN64: Error: 0xEC detected: Math error"); break;
00139         case 0xDB: ROS_ERROR("EZN64: Error: 0xDB detected: VPC3"); break;
00140         case 0xDC: ROS_ERROR("EZN64: Error: 0xDC detected: Fragmentation"); break;
00141         case 0xDE: ROS_ERROR("EZN64: Error: 0xDE detected: Current"); break;
00142         case 0xDF: ROS_ERROR("EZN64: Error: 0xDF detected: I2T"); break;
00143         case 0xE0: ROS_ERROR("EZN64: Error: 0xE0 detected: Initialize"); break;
00144         case 0xE1: ROS_ERROR("EZN64: Error: 0xE1 detected: Internal"); break;
00145         case 0xE2: ROS_ERROR("EZN64: Error: 0xE2 detected: Hard low"); break;
00146         case 0xE3: ROS_ERROR("EZN64: Error: 0xE3 detected: Hard high"); break;
00147         case 0x70: ROS_ERROR("EZN64: Error: 0x70 detected: Temp low"); break;
00148         case 0x71: ROS_ERROR("EZN64: Error: 0x71 detected: Temp high"); break;
00149         case 0x72: ROS_ERROR("EZN64: Error: 0x72 detected: Logic low"); break;
00150         case 0x73: ROS_ERROR("EZN64: Error: 0x73 detected: Logic high"); break;
00151         case 0x74: ROS_ERROR("EZN64: Error: 0x74 detected: Motor voltage low"); break;
00152         case 0x75: ROS_ERROR("EZN64: Error: 0x75 detected: Motor voltage high"); break;
00153         case 0x76: ROS_ERROR("EZN64: Error: 0x76 detected: Cable break"); break;
00154         case 0x78: ROS_ERROR("EZN64: Error: 0x78 detected: Motor temp "); break;
00155       }
00156       //Reset periodic position reading
00157        getPeriodicPositionUpdate(handle, TF_UPDATE_PERIOD);
00158    
00159        return((uint8_t)ezn64_error_);
00160     }
00161     else
00162     {
00163       ROS_WARN("EZN64: Not get_state response");
00164             
00165       //Reset periodic position reading
00166       getPeriodicPositionUpdate(handle, TF_UPDATE_PERIOD);
00167       return(0xff) ;
00168     }
00169   }
00170   output.clear();
00171 }
00172 
00173 void
00174 EZN64_usb::acknowledgeError(libusb_device_handle *handle)
00175 {
00176   std::vector<uint8_t> output;
00177   output.push_back(0x01);      //Data Length
00178   output.push_back(0x8b);      //Command cmd ack
00179 
00180   //Send message to the module
00181   usb_write(handle, output);
00182 }
00183 
00184 void
00185 EZN64_usb::setPosition(libusb_device_handle *handle, float goal_position)
00186 {
00187   ROS_INFO_STREAM("EZN64: Moving from: " << act_position_ << " [mm] to " << goal_position << " [mm]");
00188 
00189   std::vector<uint8_t> output;
00190   output.push_back(0x05);                //Data Length
00191   output.push_back(0xb0);                //Command mov pos
00192 
00193   unsigned int IEEE754_bytes[4];
00194   float_to_IEEE_754(goal_position, IEEE754_bytes);
00195 
00196   output.push_back(IEEE754_bytes[0]);    //Position first byte
00197   output.push_back(IEEE754_bytes[1]);    //Position second byte
00198   output.push_back(IEEE754_bytes[2]);    //Position third byte
00199   output.push_back(IEEE754_bytes[3]);    //Position fourth byte
00200 
00201   //Send message to the module and recieve response
00202   usb_write(handle, output);       
00203 }
00204 
00205 float
00206 EZN64_usb::getPosition(libusb_device_handle *handle)
00207 {
00208   std::vector<uint8_t> output;
00209   output.push_back(0x06);                 //Data Length
00210   output.push_back(0x95);                 //Command get state
00211   output.push_back(0x00);
00212   output.push_back(0x00);
00213   output.push_back(0x00);
00214   output.push_back(0x00);
00215   output.push_back(0x01);                //Only once
00216 
00217   //Send get_state message to the module and recieve response
00218   usb_write(handle, output);
00219 
00220   ros::Duration(WAIT_FOR_RESPONSE_INTERVAL).sleep();
00221   std::vector<uint8_t> input;
00222   input = usb_read(handle);
00223 
00224   float act_position;
00225 
00226   if(input.size() > 0)
00227   {
00228     for(size_t i = 0; i < input.size(); i++)
00229     {
00230       //Handle both possible response messages
00231       if((input[i] == 0x07) && (input[i+1] == 0x95))
00232       {
00233         uint8_t raw[4] = {input[i+5], input[i+4], input[i+3], input[i+2]};
00234         act_position = IEEE_754_to_float(raw);
00235         ROS_INFO_STREAM("EZN64 INFO: Actual position: " << act_position << "[mm]");
00236         return(act_position);
00237       }
00238       else if ((input[i] == 0x05) && (input[i+1] == 0x94))
00239       {
00240         uint8_t raw[4] = {input[i+5], input[i+4], input[i+3], input[i+2]};
00241         act_position = IEEE_754_to_float(raw);
00242         ROS_INFO_STREAM("EZN64 INFO: Actual position: " << act_position << "[mm]");
00243         return(act_position);
00244       }
00245     }
00246   }
00247 }
00248 
00249 void
00250 EZN64_usb::getPeriodicPositionUpdate(libusb_device_handle *handle, float update_period)
00251 {
00252   std::vector<uint8_t> output;
00253   output.push_back(0x06);                 //Data Length
00254   output.push_back(0x95);                 //Command get state
00255     
00256   unsigned int IEEE754_bytes[4];
00257   float_to_IEEE_754(update_period, IEEE754_bytes);
00258       
00259   output.push_back(IEEE754_bytes[0]);    //period first byte
00260   output.push_back(IEEE754_bytes[1]);    //period second byte
00261   output.push_back(IEEE754_bytes[2]);    //period third byte
00262   output.push_back(IEEE754_bytes[3]);    //period fourth byte
00263   output.push_back(0x07);
00264 
00265   //Send get_state message to the module 
00266   usb_write(handle, output);   
00267 }
00268 
00269 void
00270 EZN64_usb::stop(libusb_device_handle *handle)
00271 {
00272   std::vector<uint8_t> output;
00273   output.push_back(0x01);       //Data Length
00274   output.push_back(0x91);       //Command stop
00275 
00276   //Send message to the module
00277   usb_write(handle, output);
00278 }
00279 
00281 //CALLBACKS
00283 
00284 bool
00285 EZN64_usb::referenceCallback(schunk_ezn64::reference::Request &req,
00286                              schunk_ezn64::reference::Response &res)
00287 {
00288   ROS_INFO("EZN64: Reference Cmd recieved ");
00289   reference(ezn64_handle_);           
00290 }
00291 
00292 bool
00293 EZN64_usb::setPositionCallback(schunk_ezn64::set_position::Request &req,
00294                                schunk_ezn64::set_position::Response &res)
00295 {
00296   //Check if goal request respects gripper limits <0-12> mm
00297   if ((req.goal_position >= MIN_GRIPPER_POS_LIMIT)
00298      && (req.goal_position <= MAX_GRIPPER_POS_LIMIT))
00299   {
00300     setPosition(ezn64_handle_,req.goal_position);  
00301     res.goal_accepted = true;
00302   }
00303   else
00304   {
00305     ROS_WARN("EZN64: Goal position rejected!");
00306     res.goal_accepted = false;
00307   }
00308 }
00309 
00310 bool
00311 EZN64_usb::getErrorCallback(schunk_ezn64::get_error::Request &req,
00312                             schunk_ezn64::get_error::Response &res)
00313 {
00314   ROS_INFO:("EZN64: Get Error request recieved");
00315   res.error_code = getError(ezn64_handle_);
00316 }
00317 
00318 bool
00319 EZN64_usb::getPositionCallback(schunk_ezn64::get_position::Request &req,
00320                                schunk_ezn64::get_position::Response &res)
00321 {
00322   ROS_INFO("EZN64: Get position request recieved");
00323   res.actual_position = act_position_;    
00324 }
00325 
00326 bool
00327 EZN64_usb::acknowledgeErrorCallback(schunk_ezn64::acknowledge_error::Request &req,
00328                                     schunk_ezn64::acknowledge_error::Response &res)
00329 {
00330   ROS_INFO("EZN64: Cmd Acknowledge error recieved");
00331   acknowledgeError(ezn64_handle_);
00332   ros::Duration(WAIT_FOR_RESPONSE_INTERVAL).sleep();
00333   if(getError(ezn64_handle_) == 0x00)
00334     res.acknowledge_response = true;
00335   else
00336     res.acknowledge_response = false;   
00337 }
00338 
00339 
00340 bool
00341 EZN64_usb::stopCallback(schunk_ezn64::stop::Request &req,
00342                         schunk_ezn64::stop::Response &res)
00343 {
00344   ROS_INFO("EZN64: Cmd Stop recieved");
00345   stop(ezn64_handle_);
00346   ros::Duration(WAIT_FOR_RESPONSE_INTERVAL).sleep();
00347   res.stop_result = getPosition(ezn64_handle_); 
00348 }
00349 
00350 void
00351 EZN64_usb::timerCallback(const ros::TimerEvent &event)
00352 {
00353   std::vector<uint8_t> input;
00354   input = usb_read(ezn64_handle_);
00355     
00356   //Detect position reached response
00357   for(size_t i = 0; i < input.size(); i++)
00358   {
00359     if((input[i] == 0x07) && (input[i+1] == 0x95))
00360     {
00361       uint8_t raw[4] = {input[i+5], input[i+4], input[i+3], input[i+2]};
00362       act_position_ = IEEE_754_to_float(raw);      
00363     }
00364     else if ((input[i] == 0x0f) && (input[i+2] == 0x95))
00365     {
00366       uint8_t raw[4] = {input[i+6], input[i+5], input[i+4], input[i+3]};
00367       act_position_ = IEEE_754_to_float(raw);      
00368     }
00369     else if ((input[i] == 0x05) && (input[i+1] == 0x94))
00370     {
00371       uint8_t raw[4] = {input[i+5], input[i+4], input[i+3], input[i+2]};
00372       act_position_ = IEEE_754_to_float(raw);      
00373     }
00374   }
00375   
00376   //Publish TF
00377   ezn64_joint_state_.header.stamp = ros::Time::now();
00378   ezn64_joint_state_.name.clear();
00379   ezn64_joint_state_.position.clear();
00380     
00381   ezn64_joint_state_.name.push_back("ezn64_finger_1_joint");
00382   ezn64_joint_state_.position.push_back(act_position_/URDF_SCALE_FACTOR);
00383   
00384   ezn64_joint_state_.name.push_back("ezn64_finger_2_joint");
00385   ezn64_joint_state_.position.push_back(act_position_/URDF_SCALE_FACTOR);
00386   
00387   ezn64_joint_state_.name.push_back("ezn64_finger_3_joint");
00388   ezn64_joint_state_.position.push_back(act_position_/URDF_SCALE_FACTOR);
00389    
00390   joint_pub.publish(ezn64_joint_state_);         
00391   
00392 }
00393 
00395 //LIBUSB FUNCTIONS
00397 
00398 libusb_device*
00399 EZN64_usb::find_ezn64_dev(int VendorID, int ProductID)
00400 {
00401   int r;
00402 
00403   //Initialize USB session
00404   r= libusb_init(&usb_context_);
00405   if (r < 0)
00406   {
00407     ROS_ERROR("EZN64: Init Error");
00408     return 0;
00409   }
00410   libusb_set_debug(usb_context_, LIBUSB_VERBOSITY_LEVEL);        //set verbosity level to 3, as suggested in the documentation
00411 
00412   //Get list of devices
00413   static size_t dev_cnt;          //number of devices
00414   libusb_device **devs;           //pointer to pointer of device used to retrieve a list of devices
00415   dev_cnt = libusb_get_device_list(usb_context_, &devs);
00416   if(dev_cnt < 0)
00417   {
00418     ROS_ERROR("EZN64: Get device Error");
00419     return 0;
00420   }
00421   else
00422     ROS_INFO_STREAM("EZN64: Number of devices: " << dev_cnt);
00423 
00424   for(size_t i = 0; i < dev_cnt; i++)
00425   {
00426     struct libusb_device_descriptor descriptor;
00427     if(libusb_get_device_descriptor(devs[i], &descriptor))
00428     {
00429       ROS_ERROR("EZN64: Not able to read descriptor");
00430       return 0;
00431     }
00432   
00433     if((descriptor.idVendor == VendorID) && (descriptor.idProduct == ProductID))
00434     {
00435       ROS_INFO("EZN64: Gripper found");
00436       return devs[i];
00437     }
00438   }
00439   libusb_free_device_list(devs, 1 );     /* Free list and all not open devices */
00440 }
00441 
00442 libusb_device_handle*
00443 EZN64_usb::open_ezn64_dev(libusb_device *dev)
00444 {
00445   libusb_device_handle* handle;
00446 
00447   //Open device
00448   if(libusb_open(dev, &handle))
00449   {
00450     ROS_ERROR("EZN64: Port not oppened!");
00451     return 0;
00452   }
00453   else
00454   {
00455     ROS_INFO("EZN64: USB interface oppened successfully! ");
00456     return handle;
00457   }
00458 }
00459 
00460 int
00461 EZN64_usb::close_ezn64_dev(libusb_device_handle *handle, libusb_context *usb_context_)
00462 {
00463   ROS_INFO("EZN64: Closing USB port");
00464   libusb_close(handle);
00465   libusb_exit(usb_context_);
00466   return 0;
00467 }
00468 
00469 void
00470 EZN64_usb::usb_write(libusb_device_handle *handle, std::vector<uint8_t> output)
00471 {
00472   int r;           //temp variable
00473   int byte_cnt;    //bytes counter
00474 
00475   //find out if kernel driver is attached
00476   if(libusb_kernel_driver_active(handle, 0) == 1)
00477   {
00478     ROS_INFO("EZN64: Kernel Driver Active");
00479 
00480     if(libusb_detach_kernel_driver(handle, 0) == 0) //detach it
00481       ROS_INFO("EZN64: Kernel Driver Detached!");
00482   }
00483 
00484   r = libusb_claim_interface(handle, 0); //claim interface 0 (the first) of device (mine had jsut 1)
00485 
00486   if(r < 0) ROS_INFO("EZN64: Cannot Claim Interface");
00487 
00488   //Write
00489 
00490   //Convert std::vector<uint8_t> to unsigned char array as requested by libusb API
00491   unsigned char *data_out = new unsigned char[output.size()];
00492   for(size_t i = 0; i < output.size(); i++)
00493   {
00494     data_out[i] = output[i];
00495     ROS_DEBUG("Output[%d]: %x", i,data_out[i]);
00496   }
00497 
00498   r = libusb_bulk_transfer(handle, 1, data_out, output.size(), &byte_cnt, LIBUSB_TIMEOUT);
00499   if (byte_cnt == 0) ROS_ERROR("EZN64: Write error");
00500 
00501   //Release interface
00502 
00503   r = libusb_release_interface(handle, 0);  //release the claimed interface
00504   if(r != 0)
00505     ROS_ERROR("EZN64: Cannot release the claimed interface");
00506    
00507   delete[] data_out;
00508 
00509 }
00510 
00511 std::vector<uint8_t>
00512 EZN64_usb::usb_read(libusb_device_handle *handle)
00513 {
00514   int r;           //temp variable
00515   int byte_cnt;    //bytes counter
00516 
00517   //find out if kernel driver is attached
00518   if(libusb_kernel_driver_active(handle, 0) == 1)
00519   {
00520     ROS_INFO("EZN64: Kernel Driver Active");
00521 
00522     if(libusb_detach_kernel_driver(handle, 0) == 0) //detach it
00523       ROS_INFO("EZN64: Kernel Driver Detached!");
00524   }
00525 
00526   r = libusb_claim_interface(handle, 0); //claim interface 0 (the first) of device (mine had just 1)
00527 
00528   if(r < 0) ROS_ERROR("EZN64: Cannot Claim Interface");
00529 
00530   //Read
00531   std::vector<uint8_t> input;
00532   unsigned char *data_in = new unsigned char[INPUT_BUFFER_SIZE];
00533 
00534   r = libusb_bulk_transfer(handle, LIBUSB_ENDPOINT, data_in, INPUT_BUFFER_SIZE, &byte_cnt, LIBUSB_TIMEOUT);
00535   if(r == 0)
00536   {
00537     for(size_t i = 0; i < byte_cnt; i++)
00538     {
00539       input.push_back(data_in[i]);
00540       ROS_DEBUG("Input[%d]: %x", i, data_in[i]);
00541     }
00542   }
00543 
00544   //Release interface
00545   r = libusb_release_interface(handle, 0);  //release the claimed interface
00546   if(r != 0)
00547     ROS_ERROR("EZN64: Cannot release the claimed interface");
00548    
00549   delete[] data_in;
00550   return input;
00551 }
00552 
00553 
00554 void
00555 EZN64_usb::print_libusb_dev(libusb_device *dev)
00556 {
00557   libusb_device_descriptor desc;
00558   int r = libusb_get_device_descriptor(dev, &desc);
00559   if (r < 0) 
00560   {
00561     ROS_ERROR("EZN64: failed to get device descriptor");
00562     return;
00563   }
00564 
00565   if((int)desc.bNumConfigurations > 0)
00566   {
00567     ROS_INFO_STREAM("EZN64: Number of possible configurations: " << (int)desc.bNumConfigurations);
00568     ROS_INFO_STREAM("EZN64: VendorID: "  << desc.idVendor);
00569     ROS_INFO_STREAM("EZN64: ProductID: " << desc.idProduct);
00570 
00571     libusb_config_descriptor *config;
00572     libusb_get_config_descriptor(dev, 0, &config);
00573     ROS_INFO_STREAM("EZN64: Interfaces: " << (int)config->bNumInterfaces);
00574 
00575     const libusb_interface *inter;
00576     const libusb_interface_descriptor *interdesc;
00577     const libusb_endpoint_descriptor *epdesc;
00578 
00579     for(int i = 0; i < (int)config->bNumInterfaces; i++) 
00580     {
00581       inter = &config->interface[i];
00582       ROS_DEBUG_STREAM("Number of alternate settings: " << inter->num_altsetting );
00583       for(int j = 0; j < inter->num_altsetting; j++)
00584       {
00585         interdesc = &inter->altsetting[j];
00586         ROS_DEBUG_STREAM("Interface Number: " << (int)interdesc->bInterfaceNumber);
00587         ROS_DEBUG_STREAM("Number of endpoints: " << (int)interdesc->bNumEndpoints);
00588         for(int k = 0; k < (int)interdesc->bNumEndpoints; k++) 
00589         {
00590           epdesc = &interdesc->endpoint[k];
00591           ROS_DEBUG_STREAM("Descriptor Type: "<< (int)epdesc->bDescriptorType);
00592           ROS_DEBUG_STREAM("EP Address: "<< (int)epdesc->bEndpointAddress);
00593         }
00594       }
00595     }
00596     libusb_free_config_descriptor(config);
00597   }
00598   else
00599   {
00600     ROS_ERROR("No Schunk EZN64 gripper found! Aborting...");
00601     exit(-1);
00602   }
00603     
00604 
00605 }
00606 
00607 float
00608 EZN64_usb::IEEE_754_to_float(uint8_t *raw)
00609 {
00610   int sign = (raw[0] >> 7) ? -1 : 1;
00611   int8_t exponent = (raw[0] << 1) + (raw[1] >> 7) - 126;
00612 
00613   uint32_t fraction_bits = ((raw[1] & 0x7F) << 16) + (raw[2] << 8) + raw[3];
00614 
00615   float fraction = 0.5f;
00616   for (uint8_t ii = 0; ii < 24; ++ii)
00617     fraction += ldexpf((fraction_bits >> (23 - ii)) & 1, -(ii + 1));
00618 
00619   float significand = sign * fraction;
00620 
00621   return ldexpf(significand, exponent);
00622 }
00623 
00624 void
00625 EZN64_usb::float_to_IEEE_754(float position, unsigned int *output_array)
00626 {
00627   unsigned char *p_byte = (unsigned char*)(&position);
00628 
00629   for(size_t i = 0; i < sizeof(float); i++)
00630     output_array[i] = (static_cast<unsigned int>(p_byte[i]));
00631 }
00632 
00633 } //schunk_ezn64
00634 
00635 #endif //EZN64_USB_CONTROL_LIB_CPP


schunk_ezn64
Author(s): Frantisek Durovsky
autogenerated on Fri Feb 12 2016 00:21:41