SensorCalUtility.cc
Go to the documentation of this file.
00001 
00037 #ifdef WIN32
00038 #ifndef WIN32_LEAN_AND_MEAN
00039 #define WIN32_LEAN_AND_MEAN 1
00040 #endif
00041 
00042 #include <windows.h>
00043 #include <winsock2.h>
00044 #else
00045 #include <unistd.h>
00046 #endif
00047 
00048 #include <stdio.h>
00049 #include <stdlib.h>
00050 #include <sys/types.h>
00051 #include <sys/stat.h>
00052 #include <fstream>
00053 #include <map>
00054 #include <string.h>
00055 #include <sstream>
00056 
00057 #include <Utilities/portability/getopt/getopt.h>
00058 #include <LibMultiSense/MultiSenseChannel.hh>
00059 
00060 using namespace crl::multisense;
00061 
00062 namespace {  // anonymous
00063 
00064 std::string byte_to_binary(uint8_t x)
00065 {
00066     std::stringstream b;
00067 
00068     uint8_t z;
00069     for (z = 128; z > 0; z >>= 1)
00070     {
00071        b << (((x & z) == z) ? "1" : "0");
00072     }
00073 
00074     return b.str();
00075 }
00076 
00077 std::string bytes_to_binary(uint64_t x, int bits)
00078 {
00079     std::string b;
00080     uint8_t currentByte;
00081     for(currentByte = 0; currentByte*8<bits; currentByte++)
00082     {
00083         b = byte_to_binary((x >> currentByte*8)) + " " + b;
00084     }
00085 
00086     return b.erase(0,(currentByte*8 - bits));
00087 }
00088 
00089 void usage(const char *programNameP) 
00090 {
00091     fprintf(stderr, 
00092             "USAGE: %s [<options>]\n", 
00093             programNameP);
00094     fprintf(stderr, "Where <options> are:\n");
00095     fprintf(stderr, "\t-a <ip_address>      : ip address (default=10.66.171.21)\n");
00096     fprintf(stderr, "\t-s                   : set the calibration (default is query)\n");
00097     fprintf(stderr, "\t-l                   : set the left calibration \n");
00098     fprintf(stderr, "\t-r                   : set the right calibration \n");
00099     fprintf(stderr, "\t-e                   : set the right black level \n");
00100     fprintf(stderr, "\t-k                   : set the left black level \n");
00101     
00102     exit(-1);
00103 }
00104 
00105 bool fileExists(const std::string& name)
00106 {
00107     struct stat sbuf;
00108     return (0 == stat(name.c_str(), &sbuf));
00109 }
00110 
00111 }; // anonymous
00112 
00113 int main(int    argc, 
00114          char **argvPP)
00115 {
00116     Status status = Status_Ok;
00117     std::string ipAddress = "10.66.171.21";
00118     std::string leftIn = "50";
00119     std::string rightIn = "50";
00120     std::string leftBlackIn = "-61";
00121     std::string rightBlackIn = "-61";
00122         uint8_t left;
00123         uint8_t right;
00124     int16_t leftBlack;
00125     int16_t rightBlack;
00126     bool        setCal=false;
00127         bool        setLeft=false;
00128         bool        setRight=false;
00129         bool        setLeftBlack=false;
00130         bool        setRightBlack=false;
00131 
00132     //
00133     // Parse args
00134 
00135     int inArgument;
00136 
00137     while(-1 != (inArgument = getopt(argc, argvPP, "l:r:k:e:a:sy")))
00138         switch(inArgument) {
00139         case 'a': ipAddress      = std::string(optarg);                      break;
00140         case 'l': leftIn         = std::string(optarg); setLeft=true;        break;
00141         case 'r': rightIn        = std::string(optarg); setRight=true;       break;
00142         case 'k': leftBlackIn    = std::string(optarg); setLeftBlack=true;    break;
00143         case 'e': rightBlackIn   = std::string(optarg); setRightBlack=true;   break;
00144         case 's': setCal         = true;                   break;
00145         default: usage(*argvPP);                           break;
00146     }
00147 
00148     //
00149     // Verify options
00150 
00151     if (setCal && (!setLeft && !setRight && !setLeftBlack && !setRightBlack)) {
00152         fprintf(stderr, "Please specify a value to set using -l, -r, -e, or -k\n");
00153         usage(*argvPP);
00154     }
00155 
00156     //
00157     // Initialize communications.
00158     fprintf(stdout, "Attempting to establish communications with \"%s\"\n",
00159                 ipAddress.c_str());
00160 
00161     Channel *channelP = Channel::Create(ipAddress);
00162     if (NULL == channelP) {
00163         fprintf(stderr, "Failed to establish communications with \"%s\"\n",
00164             ipAddress.c_str());
00165         return -1;
00166     }
00167 
00168     //
00169     // Query the current device calibration
00170     image::SensorCalibration sensorCalibration;
00171 
00172     status = channelP->getSensorCalibration(sensorCalibration);
00173     if (Status_Ok != status) {
00174         fprintf(stderr, "failed to query sensor calibration: %s\n", 
00175                 Channel::statusString(status));
00176         goto clean_out;
00177     }
00178 
00179     //
00180     // Modify the elements of the queried calibration depending on which
00181     // parameters will be set
00182     if (setCal && setLeft) {
00183         left = atoi(leftIn.c_str());
00184 
00185         if (left > 255 || left < 0)
00186         {
00187             fprintf(stderr, "Left sensor gain range is 0-255\n");
00188             usage(*argvPP);
00189             goto clean_out;
00190         }
00191 
00192         sensorCalibration.adc_gain[0] = left;
00193     }
00194 
00195     if (setCal && setRight) {
00196         right = atoi(rightIn.c_str());
00197 
00198         if (right > 255 || right < 0)
00199         {
00200             fprintf(stderr, "Right sensor gain range is 0-255\n");
00201             usage(*argvPP);
00202             goto clean_out;
00203         }
00204 
00205         sensorCalibration.adc_gain[1] = right;
00206     }
00207 
00208     if (setCal && setLeftBlack)
00209     {
00210         leftBlack = atoi(leftBlackIn.c_str());
00211         sensorCalibration.bl_offset[0] = leftBlack;
00212     }
00213 
00214     if (setCal && setRightBlack)
00215     {
00216         rightBlack = atoi(rightBlackIn.c_str());
00217         sensorCalibration.bl_offset[1] = rightBlack;
00218     }
00219 
00220 
00221     if (false == setCal) {
00222 
00223         fprintf(stdout,"left sensor gain: %hu\nright sensor gain %hu\nleft sensor black level: %d\nright sensor black level %d\n", 
00224                     sensorCalibration.adc_gain[0], sensorCalibration.adc_gain[1],
00225                     sensorCalibration.bl_offset[0], sensorCalibration.bl_offset[1]);
00226 
00227     } else {
00228 
00229         fprintf(stdout,"Setting :\nleft sensor gain: %5hu binary: %s\n"
00230                                    "right sensor gain %5hu binary: %s\n"
00231                                    "left sensor black level: %d binary: %s\n"
00232                                    "right sensor black level %d binary: %s\n", 
00233                     sensorCalibration.adc_gain[0],byte_to_binary(sensorCalibration.adc_gain[0]).c_str(),
00234                     sensorCalibration.adc_gain[1],byte_to_binary(sensorCalibration.adc_gain[1]).c_str(),
00235                     sensorCalibration.bl_offset[0],bytes_to_binary(sensorCalibration.bl_offset[0], 14).c_str(),
00236                     sensorCalibration.bl_offset[1],bytes_to_binary(sensorCalibration.bl_offset[1], 14).c_str());
00237 
00238         status = channelP->setSensorCalibration(sensorCalibration);
00239         if (Status_Ok != status) {
00240             fprintf(stderr, "failed to set sensor calibration: %s\n", 
00241                     Channel::statusString(status));
00242             goto clean_out;
00243         }
00244 
00245         fprintf(stdout, "Sensor calibration successfully updated\n");
00246     }
00247 
00248 clean_out:
00249 
00250     Channel::Destroy(channelP);
00251     return 0;
00252 }


multisense_lib
Author(s):
autogenerated on Fri Apr 5 2019 02:28:24