sub20_ADC.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Robert Bosch LLC
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Robert Bosch LLC nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 /*
00030  * adc_sub20.cpp
00031  *
00032  *  Created on: Jun 12, 2011
00033  *      Author: Nikhil Deshpande, Robert Bosch LLC|
00034  */
00035 
00036 #include <iostream>
00037 #include <string>
00038 #include "ros/ros.h"
00039 #include <sstream>
00040 #include "ros/time.h"
00041 #include <math.h>
00042 #include "adc_sub20/sub20_ADC.h"
00043 #include <vector>
00044 #include <map>
00045 
00046 Sub20_ADC::Sub20_ADC(std::string ADC_MUXCONFIG, double dRate_Hz, double dVRef):
00047         bSubDeviceOpen        (false),
00048         bSubDeviceConfigured  (false)
00049 {
00050         // TODO Auto-generated constructor stub
00051         int               iADCErr;              //Error code for std Sub20 API
00052         int               iADC_cfg_set;         //Set ADC config
00053         std::stringstream ss_errmsg;
00054         std::string       ss_config;
00055         sub_device        sub20dev;
00056         sub_handle        sub20handle;
00057         OneSub20Config    OneSub20Configuration;
00058 
00059         ROS_INFO("--------------------------------------");
00060         initADC_MUX_MAP();                              // Initialize the ADC_MUX mapping for String Values of enum entries
00062         // Open connected Sub20 Devices
00064         sub20dev = 0;
00065         sub20dev = sub_find_devices(sub20dev);
00066         while( sub20dev != 0 ) {
00067             sub20handle = sub_open( sub20dev );
00068             // on success, sub_open returns non-zero handle
00069             if (sub20handle > 0) {
00070                 subhndl = sub20handle;
00072                 // Configure Sub20 device
00074                 std::cout << "---Initializing ADC Interface---" << std::endl;
00075                 // Read and set current ADC configuration
00076                 iADC_cfg_set = ADC_ENABLE;
00077                 ss_config = "ADC_ENABLE | ";
00078                 if((dVRef==VCC_5V)||(dVRef==VCC_3V3)){
00079                         iADC_cfg_set |= ADC_REF_VCC;
00080                         ss_config += "ADC_REF_VCC";
00081                 } else {
00082                         iADC_cfg_set |= ADC_REF_2_56;
00083                         ss_config += "ADC_REF_2_56";
00084                 }
00085                 //Configure ADC
00086                 iADCErr = sub_adc_config( sub20handle, iADC_cfg_set);
00087                 if(iADCErr == 0) {
00088                         std::cout << "ADC Configuration   : " << iADC_cfg_set << " successfully stored \n";
00089                         std::cout << "Configuration Details   : " << std::endl;
00090                         std::cout << "          Register Settings               : " << ss_config << std::endl;
00091                         std::cout << "          ADC Readings Rate (/sec)        : " << dRate_Hz << std::endl;
00092 
00093                         //ADC Read Configuration
00094                         get_ADC_MuxCode(ADC_MUXCONFIG, OneSub20Configuration.sub20_ADC_Config.sub20_ADC_MUX, OneSub20Configuration.sub20_ADC_Config.sub20_ADC_TYPE, &OneSub20Configuration.sub20_ADC_Config.Num_Chans);
00095                         std::cout << "          ADC Channels Selected           : " << ADC_MUXCONFIG << std::endl;
00096                         OneSub20Configuration.bSubDevConfigured = true;
00097 
00098                         //string needs to be cleared otherwise conversion is going wrong
00099                         strSerial.clear();
00100                         strSerial.resize(sub20_ADC_cmd::uSERIALNUMLENGTH);
00101                         sub_get_serial_number(sub20handle, const_cast<char*>(strSerial.c_str()), strSerial.size());
00102                         OneSub20Configuration.strSub20Serial    = strSerial;
00103                         OneSub20Configuration.handle_subdev     = sub20handle;
00104                         OneSub20Configuration.subdev            = sub20dev;
00105                         std::cout << "Device Handle   : " << OneSub20Configuration.handle_subdev << std::endl;
00106                         std::cout << "Serial Number   : " << OneSub20Configuration.strSub20Serial << std::endl;
00107 
00109                         // Push element onto list of subdevices
00111                         Sub20Device_list.push_back (OneSub20Configuration);
00112                         std::cout << "Serial : " << OneSub20Configuration.strSub20Serial << "\n";
00113                 } else {
00114                         ROS_INFO("ERROR - Configuration : %d not accepted by device", iADC_cfg_set);
00115                         //Subdevice could not be configured
00116                         OneSub20Configuration.bSubDevConfigured = false;
00117                 }
00118             }
00119             //find next device, sub_find_devices using current provides next
00120             sub20dev = sub_find_devices(sub20dev);
00121         }
00122         ROS_INFO("--------------------------------------");
00123         std::cout << "  ************* Publishing to rostopic - /sub20_ADC *************  " << std::endl;
00124 }
00125 
00126 Sub20_ADC::~Sub20_ADC() {
00127         // TODO Auto-generated destructor stub
00128         int iSpiErr;
00129         OneSub20Config OneSub20Configuration;
00130         //close opened subdevices
00131         // Disable SPI
00132         iSpiErr = sub_spi_config( subhndl, 0, 0 );
00133         // Close USB device
00134         sub_close( subhndl );
00135         while (!Sub20Device_list.empty()) {
00136                 OneSub20Configuration = Sub20Device_list.back ();
00137                 std::cout << "Sub device removed " << OneSub20Configuration.strSub20Serial << "\n";
00138                 Sub20Device_list.pop_back ();
00139         }
00140 }
00141 
00142 // Map the string input to enum ADC channel types
00143 void Sub20_ADC::initADC_MUX_MAP(){
00144         sub20_ADC_cmd::adcMuxMap["ADC_S0"] = ADC_S0;
00145         sub20_ADC_cmd::adcMuxMap["ADC_S1"] = ADC_S1;
00146         sub20_ADC_cmd::adcMuxMap["ADC_S2"] = ADC_S2;
00147         sub20_ADC_cmd::adcMuxMap["ADC_S3"] = ADC_S3;
00148         sub20_ADC_cmd::adcMuxMap["ADC_S4"] = ADC_S4;
00149         sub20_ADC_cmd::adcMuxMap["ADC_S5"] = ADC_S5;
00150         sub20_ADC_cmd::adcMuxMap["ADC_S6"] = ADC_S6;
00151         sub20_ADC_cmd::adcMuxMap["ADC_S7"] = ADC_S7;
00152 
00153         sub20_ADC_cmd::adcMuxMap["ADC_D10_10X"] = ADC_D10_10X;
00154         sub20_ADC_cmd::adcMuxMap["ADC_D10_200X"] = ADC_D10_200X;
00155         sub20_ADC_cmd::adcMuxMap["ADC_D32_10X"] = ADC_D32_10X;
00156         sub20_ADC_cmd::adcMuxMap["ADC_D32_200X"] = ADC_D32_200X;
00157 
00158         sub20_ADC_cmd::adcMuxMap["ADC_D01"] = ADC_D01;
00159         sub20_ADC_cmd::adcMuxMap["ADC_D21"] = ADC_D21;
00160         sub20_ADC_cmd::adcMuxMap["ADC_D31"] = ADC_D31;
00161         sub20_ADC_cmd::adcMuxMap["ADC_D41"] = ADC_D41;
00162         sub20_ADC_cmd::adcMuxMap["ADC_D51"] = ADC_D51;
00163         sub20_ADC_cmd::adcMuxMap["ADC_D61"] = ADC_D61;
00164         sub20_ADC_cmd::adcMuxMap["ADC_D71"] = ADC_D71;
00165         sub20_ADC_cmd::adcMuxMap["ADC_D02"] = ADC_D02;
00166         sub20_ADC_cmd::adcMuxMap["ADC_D12"] = ADC_D12;
00167         sub20_ADC_cmd::adcMuxMap["ADC_D32"] = ADC_D32;
00168         sub20_ADC_cmd::adcMuxMap["ADC_D42"] = ADC_D42;
00169         sub20_ADC_cmd::adcMuxMap["ADC_D52"] = ADC_D52;
00170 
00171         sub20_ADC_cmd::adcMuxMap["ADC_1_1V"] = ADC_1_1V;
00172         sub20_ADC_cmd::adcMuxMap["ADC_GND"] = ADC_GND;
00173 }
00174 
00175 // Get the Mux Code to output the channels being used for ADC
00176 void Sub20_ADC::get_ADC_MuxCode(std::string ADC_MUXCONFIG, int *adc_mux, int *adc_type, int *channels) {
00177 
00178         unsigned int i=0, j=0, numVal=0;
00179         size_t charLocn[sub20_ADC_cmd::iMAXNUM_OF_CHANNELS], found;
00180         std::string ADCMux_Cfg[sub20_ADC_cmd::iMAXNUM_OF_CHANNELS], tmpStr;
00181 
00182         size_t strlen = ADC_MUXCONFIG.length();
00183 
00184         charLocn[0] = ADC_MUXCONFIG.find("|");
00185         if(charLocn[0] != std::string::npos){
00186                 ADCMux_Cfg[0] = ADC_MUXCONFIG.substr(0,charLocn[0]);
00187                 adc_mux[j] = sub20_ADC_cmd::adcMuxMap[ADCMux_Cfg[0]];
00188 
00189                 tmpStr = ADCMux_Cfg[0];
00190                 found = tmpStr.find("S");
00191                 if(found != std::string::npos) {
00192                         adc_type[j] = 0;
00193                 } else {
00194                         found = tmpStr.find("D");
00195                         if(found != std::string::npos){
00196                                 adc_type[j] = 1;
00197                         } else {
00198                                 adc_type[j] = 2;
00199                         }
00200                 }
00201                 j++;    numVal++;
00202         }
00203         for(i=1; i<strlen; i++){
00204                 charLocn[i] = ADC_MUXCONFIG.find("|",(charLocn[i-1]+1));
00205                 if(charLocn[i] == std::string::npos){
00206                         break;
00207                 }
00208                 ADCMux_Cfg[i] = ADC_MUXCONFIG.substr((charLocn[i-1]+1),((charLocn[i]-charLocn[i-1])-1));
00209                 adc_mux[j] = sub20_ADC_cmd::adcMuxMap[ADCMux_Cfg[i]];
00210 
00211                 tmpStr = ADCMux_Cfg[i];
00212                 found = tmpStr.find("S");
00213                 if(found != std::string::npos) {
00214                         adc_type[j] = 0;
00215                 } else {
00216                         found = tmpStr.find("D");
00217                         if(found != std::string::npos){
00218                                 adc_type[j] = 1;
00219                         } else {
00220                                 adc_type[j] = 2;
00221                         }
00222                 }
00223                 j++;    numVal++;
00224         }
00225         *channels = numVal;
00226 }
00227 
00228 double Sub20_ADC::adc_To_Volts(int rawData, char convType, int muxCode, double dVRef){
00229         double value = 0, gain = 0;
00230         // convert adc to volts using 10-bit ADC and desired configuration
00231         // 'gains' are from the sub20-manual
00232         switch(convType){
00233                 case 0:
00234                         value = (rawData*dVRef)/1023;
00235                         break;
00236                 case 1:
00237                         if((muxCode==ADC_D10_10X)||(muxCode==ADC_D32_10X)){
00238                                 gain = 10;
00239                         } else if((muxCode==ADC_D10_200X)||(muxCode==ADC_D32_200X)){
00240                                 gain = 200;
00241                         } else {
00242                                 gain = 1;
00243                         }
00244                         value = (rawData*dVRef)/(512*gain);
00245                         break;
00246                 case 2:
00247                         value = rawData;
00248                         break;
00249                 default:
00250                         break;
00251         }
00252         return value;
00253 }
00254 
00255 void Sub20_ADC::GetMeasurements(std::list<OneSub20_ADC_Meas> &list_meas, double dVRef, int *channels) {
00256 
00257         int                                    iADCErr;
00258         OneSub20_ADC_Meas                      sMeas;
00259         ros::Time                              dtomeas;
00260         std::stringstream                      ss_errmsg;
00261         std::list<OneSub20Config>::iterator    iterat;
00262         int                                    sub20_ADC_BUF[16];       // Buffer for ADC Channels
00263         int                                    adc_buf, adc_mux;
00264         int                                    Num_Chans;
00265 
00266         //verify that a subdevice is connected
00267         if (1 > (int)Sub20Device_list.size()) {
00268                 throw std::string ("No SubDevice connected OR access rights to USB not given");
00269         }
00270 
00271 
00272         //Trace sub20 list
00273         for (iterat = Sub20Device_list.begin(); iterat != Sub20Device_list.end(); iterat++ ) {
00274                 //verify if the sub20 device is configured (which should be the case as only configured sub20ies are pushed on list)
00275                 if (iterat->bSubDevConfigured == true) {
00276                         Num_Chans = iterat->sub20_ADC_Config.Num_Chans;
00277 //                      iADCErr = sub_adc_read(iterat->handle_subdev, sub20_ADC_BUF, iterat->sub20_ADC_Config.sub20_ADC_MUX, Num_Chans);
00278 //                      if(iADCErr == 0){
00279                                 sMeas.bMeasAvailable = true;
00280                                 for (int i=0; i<Num_Chans; i++){
00281                                         adc_mux = iterat->sub20_ADC_Config.sub20_ADC_MUX[i];
00282                                         iADCErr = sub_adc_single(iterat->handle_subdev, &adc_buf, adc_mux);
00283                                         sub20_ADC_BUF[i] = adc_buf;
00284                                         if(iADCErr == 0){
00285                                               sMeas.uiADCRaw[i] = sub20_ADC_BUF[i];
00286                                               sMeas.fADCVolt[i] = adc_To_Volts(sub20_ADC_BUF[i], iterat->sub20_ADC_Config.sub20_ADC_TYPE[i], iterat->sub20_ADC_Config.sub20_ADC_MUX[i], dVRef);
00287                                         } else {
00288                                               sMeas.bMeasAvailable = true;
00289                                               throw std::string ("Error on ADC conversion - Unplug or reset Sub20 Device");
00290                                         }
00291                                 }
00292                                 sMeas.dtomeas   = ros::Time::now();
00293                                 sMeas.strSerial = iterat->strSub20Serial;
00294                                 // Push measurement onto heap once all available channels are read!
00295                                 list_meas.push_back (sMeas);
00296 //                      } else {
00297 //                              throw std::string ("Error on ADC conversion - Unplug or reset Sub20 Device");
00298 //                      }
00299                 } else {
00300                         throw std::string ("SUB20 connected but not configured - Restart of node suggested");
00301                 }
00302         }
00303         *channels = Num_Chans;
00304 }
00305 
00306 // ----------
00307 // -- MAIN --
00308 // ----------
00309 int main(int argc, char **argv) {
00310         //---------------------------------------------
00311         // Declarations
00312         //---------------------------------------------
00313         //General definitions
00314         int                             count = 1;
00315         //Publisher for sub20_ADC data
00316         ros::init(argc, argv, "adc_sub20");
00317         ros::NodeHandle n;
00318         ros::Publisher sub20_ADC_pub = n.advertise<adc_sub20::sub20_ADC_meas>("adc_sub20", 100);
00319         //Parameter Server values
00320         double                  dRate_Hz;
00321         double                  dVRef;
00322         int                     Num_Chans;
00323 
00324         std::string             ADC_MUXCONFIG;
00325 
00326         //---------------------------------------------
00327         // Read initialization parameters from server
00328         //---------------------------------------------
00329         //Read parameters from server - if parameters are not available, the node
00330         //is initialized with default
00331         if (n.getParam("/adc_sub20/ADC_MUX_CONFIG", ADC_MUXCONFIG)         == false ) {
00332                 ADC_MUXCONFIG = sub20_ADC_cmd::sDEFAULT_ADC_MUXCONFIG;
00333         };
00334         if (n.getParam("/adc_sub20/rate_Hz", dRate_Hz)                   == false ) {
00335                 dRate_Hz = sub20_ADC_cmd::dDEFAULT_RATE_Hz;
00336         };
00337         if (n.getParam("/adc_sub20/VoltRef", dVRef)               == false ) {
00338                 dVRef = sub20_ADC_cmd::dDEFAULT_VOLTREF;
00339         };
00340 
00341         Sub20_ADC                       sub20_ADC_attached(ADC_MUXCONFIG, dRate_Hz, dVRef);
00342         OneSub20_ADC_Meas               sOneMeas;
00343         adc_sub20::sub20_ADC_meas       msg;
00344         std::list<OneSub20_ADC_Meas>    measurements_list;
00345 
00347         // Run sensor node
00349         //set loop rate for measurement polling
00350         ros::Rate loop_rate(dRate_Hz);
00351 
00352         while (n.ok()) {
00353                 //initiate a measurement on ADC
00354                 try {
00355                   sub20_ADC_attached.GetMeasurements(measurements_list, dVRef, &Num_Chans);
00356                 }
00357                 catch ( std::string strType ) {
00358                         std::cout << " An exception occurred: " << strType << std::endl;
00359                         std::exit(1);
00360                 }
00361 
00362                 while (!measurements_list.empty()) {
00363                         sOneMeas = measurements_list.back ();
00364                         measurements_list.pop_back ();
00365                         //only publish if a successful measurement was received
00366                         if ( sOneMeas.bMeasAvailable == true ) {
00367                                 msg.strIdSubDev         = sOneMeas.strSerial;
00368 
00369                                 // collect all available channel values into one message
00370                                 for(int i=0; i<Num_Chans; i++){
00371                                         msg.uiRaw.push_back(sOneMeas.uiADCRaw[i]);
00372                                         msg.fVolts.push_back(sOneMeas.fADCVolt[i]);
00373                                 }
00374                                 msg.header.stamp        = sOneMeas.dtomeas;
00375                                 sub20_ADC_pub.publish(msg);         // publish to topic!
00376 
00377                                 // clear the message values after publishing to avoid accumulation of further values on subsequent messages.
00378                                 msg.uiRaw.clear();
00379                                 msg.fVolts.clear();
00380                         }
00381                 }
00382                 ros::spinOnce();
00383                 loop_rate.sleep();
00384                 ++count;
00385         }
00386 }


adc_sub20
Author(s): Nikhil Deshpande (Maintained by Philip Roan)
autogenerated on Sat Dec 28 2013 16:49:40