ImuConfigUtility.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 <string.h>
00051 #include <errno.h>
00052 #include <iostream>
00053 #include <iomanip>
00054 
00055 #include <LibMultiSense/MultiSenseChannel.hh>
00056 
00057 #include <Utilities/portability/getopt/getopt.h>
00058 
00059 #ifdef WIN32
00060 #define strcasecmp _stricmp
00061 #endif
00062 
00063 using namespace crl::multisense;
00064 
00065 namespace {  // anonymous
00066 
00067 std::vector<imu::Info>   sensor_infos;
00068 std::vector<imu::Config> sensor_configs;
00069 uint32_t                 sensor_samplesPerMessage    = 0;
00070 uint32_t                 sensor_maxSamplesPerMessage = 0;
00071 
00072 void usage(const char *programNameP) 
00073 {
00074         std::cerr << "USAGE: " << programNameP << " [<options>]" << std::endl;
00075         std::cerr << "Where <options> are:" << std::endl;
00076         std::cerr << "\t-a <ip_address>        : IPV4 address (default=10.66.171.21)" << std::endl;
00077         std::cerr << "\t-q                     : query and report IMU configuration" << std::endl;
00078         std::cerr << "\t-f                     : store IMU configuration in non-volatile flash" << std::endl;
00079         std::cerr << "\t-s <samples>           : set IMU samples-per-message" << std::endl;
00080         std::cerr << "\t-c \"<sensor_config>\"   : IMU sensor configuration string\n" << std::endl;
00081         std::cerr << "And \"<sensor_config>\" is of the following form:" << std::endl;
00082         std::cerr << "\t\"<sensor_name> <enabled> <rate_table_index> <range_table_index>\"\n" << std::endl;
00083         std::cerr << "For example, to enable the accelerometer, and have it use rate index 1 and range index 2:\n" << std::endl;
00084         std::cerr << "\t-c \"accelerometer true 1 2\"\n" << std::endl;
00085         std::cerr << "Multiple \"-c\" options may be specified to configure more than 1 sensor\n" << std::endl;
00086         std::cerr << "Please note that small values for samples-per-message combined with high IMU sensor rates" << std::endl;
00087         std::cerr << "may interfere with the acquisition and transmission of image and lidar data, if applicable" << std::endl;
00088 
00089     exit(-1);
00090 }
00091 
00092 bool imuInfoByName(const std::string& name, imu::Info& info)
00093 {
00094     std::vector<imu::Info>::const_iterator it = sensor_infos.begin();
00095 
00096     for(; it != sensor_infos.end(); ++it) 
00097         if (name == (*it).name) {
00098             info = (*it);
00099             return true;
00100         }
00101 
00102     return false;
00103 }
00104 
00105 }; // anonymous
00106 
00107 int main(int    argc, 
00108          char **argvPP)
00109 {
00110     std::string              currentAddress         = "10.66.171.21";
00111     int32_t                  user_samplesPerMessage = 0;
00112     bool                     query                  = false;
00113     bool                     storeInFlash           = false;
00114     std::vector<std::string> cli_configs;
00115     Status                   status;
00116 
00117     //
00118     // Parse args
00119 
00120     int c;
00121 
00122     while(-1 != (c = getopt(argc, argvPP, "a:s:c:qf")))
00123         switch(c) {
00124         case 'a': currentAddress         = std::string(optarg);    break;
00125         case 's': user_samplesPerMessage = atoi(optarg);           break;
00126         case 'q': query                  = true;                   break;
00127         case 'f': storeInFlash           = true;                   break;
00128         case 'c': cli_configs.push_back(optarg);                   break;
00129         default: usage(*argvPP);                                   break;
00130         }
00131 
00132     //
00133     // A setting of zero here tells the firmware to leave it alone, useful
00134     // if you do not want to change the default.
00135 
00136     if (user_samplesPerMessage < 0) {
00137                 std::cerr << "Invalid samples-per-message: " << user_samplesPerMessage << std::endl;
00138         exit(-2);
00139     }
00140 
00141     //
00142     // If no setting changes are requested, perform a query
00143 
00144     if (0 == cli_configs.size() && 0 == user_samplesPerMessage)
00145         query = true;
00146 
00147     //
00148     // Initialize communications.
00149 
00150     Channel *channelP = Channel::Create(currentAddress);
00151     if (NULL == channelP) {
00152                 std::cerr << "Failed to establish communications with \"" << currentAddress << "\"" << std::endl;
00153         return -1;
00154     }
00155 
00156     //
00157     // Query firmware version
00158 
00159     system::VersionInfo v;
00160 
00161     status = channelP->getVersionInfo(v);
00162     if (Status_Ok != status) {
00163                 std::cerr << "Failed to query sensor version: " << Channel::statusString(status) << std::endl;
00164         goto clean_out;
00165     }
00166 
00167     if (query) {
00168                 std::cout << "Version information:\n";
00169                 std::cout << "\tAPI build date      :  " << v.apiBuildDate << "\n";
00170         std::cout << "\tAPI version         :  0x" << std::hex << std::setw(4) << std::setfill('0') << v.apiVersion << "\n";
00171                 std::cout << "\tFirmware build date :  " << v.sensorFirmwareBuildDate << "\n";
00172                 std::cout << "\tFirmware version    :  0x" << std::hex << std::setw(4) << std::setfill('0') << v.sensorFirmwareVersion << "\n";
00173                 std::cout << "\tHardware version    :  0x" << std::hex << v.sensorHardwareVersion << "\n";
00174                 std::cout << "\tHardware magic      :  0x" << std::hex << v.sensorHardwareMagic << "\n";
00175                 std::cout << "\tFPGA DNA            :  0x" << std::hex << v.sensorFpgaDna << "\n";
00176                 std::cout << std::dec;
00177     }
00178 
00179     //
00180     // Make sure firmware supports IMU
00181 
00182     if (v.sensorFirmwareVersion <= 0x0202) {
00183                 std::cerr << "IMU support requires sensor firmware version v2.3 or greater, sensor is " <<
00184                         "running v" << (v.sensorFirmwareVersion >> 8) << "." << (v.sensorFirmwareVersion & 0xff) << std::endl;
00185             goto clean_out;
00186     }
00187 
00188     //
00189     // Query IMU info / configuration
00190 
00191     status = channelP->getImuInfo(sensor_maxSamplesPerMessage,
00192                                   sensor_infos);
00193     if (Status_Ok != status) {
00194                 std::cerr << "Failed to query imu info: " << Channel::statusString(status) << std::endl;
00195         goto clean_out;
00196     }
00197     status = channelP->getImuConfig(sensor_samplesPerMessage, sensor_configs);
00198     if (Status_Ok != status) {
00199                 std::cerr << "Failed to query imu config: " << Channel::statusString(status) << std::endl;
00200         goto clean_out;
00201     }
00202 
00203     if (query) {
00204                 std::cout << "\nMax IMU samples-per-message: " << sensor_maxSamplesPerMessage << "\n";
00205         std::cout << sensor_infos.size () << " IMU sensors:\n";
00206         for(uint32_t i=0; i<sensor_infos.size(); i++) {
00207 
00208         const imu::Info& m = sensor_infos[i];
00209 
00210                 std::cout << "\t" << m.name << ":\n";
00211                 std::cout << "\t\tdevice:   " << m.device << "\n";
00212                 std::cout << "\t\tunits :   " << m.units << "\n";
00213                 std::cout << "\t\trates :   " << m.rates.size() << ": rate (Hz), bandwidthCutoff (Hz)\n";
00214                 for (uint32_t j = 0; j < m.rates.size(); j++)
00215                         std::cout << "\t\t\t\t" << j << ": " << std::fixed << std::setprecision(1) << m.rates[j].sampleRate << "," <<
00216                                                               std::fixed << std::setprecision(3) << m.rates[j].bandwidthCutoff << "\n";
00217                 std::cout << "\t\tranges:   " << m.ranges.size() << ": range (+/- " << m.units << "), resolution (" << m.units << ")\n";
00218                 for (uint32_t j = 0; j < m.ranges.size(); j++)
00219                         std::cout << "\t\t\t\t" << j << ": " << std::fixed << std::setprecision(1) << m.ranges[j].range << ", " <<
00220                                                                 std::fixed << std::setprecision(6) << m.ranges[j].resolution << "\n";
00221         }
00222 
00223         std::cout << "\nCurrent IMU configuration:\n";
00224                 std::cout << "\t-s " << sensor_samplesPerMessage << " ";
00225         for(uint32_t i=0; i<sensor_configs.size(); i++) {
00226             
00227             const imu::Config& c = sensor_configs[i];
00228             
00229                         std::cout << "-c \"" << c.name << " " << (c.enabled ? "true" : "false") << " " <<
00230                                 c.rateTableIndex << " " << c.rangeTableIndex << "\" ";
00231         }
00232         std::cout << "\n";
00233     }
00234 
00235     //
00236     // Send current configuration
00237 
00238     if (user_samplesPerMessage > 0 || cli_configs.size() > 0) {
00239 
00240         std::vector<imu::Config> user_configs;
00241         bool                     configValid = true;
00242 
00243         if (user_samplesPerMessage > static_cast<int32_t>(sensor_maxSamplesPerMessage)) {
00244                         std::cerr << "Invalid samples-per-message " << user_samplesPerMessage << ", valid values are in [1," << sensor_maxSamplesPerMessage << "]" << std::endl;
00245             configValid = false;
00246         }
00247 
00248         //
00249         // Validate each command line config option against IMU information from the head
00250 
00251         for(uint32_t i=0; i<cli_configs.size(); i++) {
00252         
00253             char nameP[32]    = {0};
00254             char enabledP[32] = {0};
00255             int  rate         = -1;
00256             int range         = -1;
00257 
00258             if (4 != sscanf(cli_configs[i].c_str(), "%31s %31s %d %d",
00259                             nameP, enabledP, &rate, &range)) {
00260                                 std::cerr << "Malformed IMU config: \"" << cli_configs[i] << "\"" << std::endl;
00261                 configValid = false;
00262                 continue;  // keep parsing for maximum feedback
00263             }
00264 
00265             if (0 != strcasecmp(enabledP, "true") &&
00266                 0 != strcasecmp(enabledP, "false")) {
00267                                 std::cerr << "Malformed <enabled> string \"" << enabledP << "\", must be one of \"true\" or \"false\"" << std::endl;
00268                 configValid = false;
00269                 continue;
00270             }
00271 
00272             //
00273             // Find the IMU information for this name
00274 
00275             imu::Info info;
00276             if (false == imuInfoByName(nameP, info)) {
00277                                 std::cerr << "Unknown <sensor_name> \"" << nameP << "\", query config for a list of valid names" << std::endl;
00278                 configValid = false;
00279                 continue;
00280             }
00281 
00282             //
00283             // Validate the rate/range indices
00284 
00285             if (rate < 0 || rate >= static_cast<int32_t>(info.rates.size())) {
00286                                 std::cerr << "Invalid rate table index " << rate << " for \"" << nameP << "\", valid indices are in [0," << (info.rates.size() - 1) << "]" << std::endl;
00287                 configValid = false;
00288             }
00289             if (range < 0 || range >= static_cast<int32_t>(info.ranges.size())) {
00290                                 std::cerr << "Invalid range table index " << range << " for \"" << nameP << "\", " <<
00291                                                  "valid indices are in [0," << (info.ranges.size() - 1) << "]" << std::endl;
00292                 configValid = false;
00293             }
00294 
00295             if (false == configValid)
00296                 continue;
00297 
00298             //
00299             // We have a valid config, store it
00300 
00301             imu::Config c;
00302             c.name            = std::string(nameP);
00303             c.enabled         = (0 == strcasecmp(enabledP, "true"));
00304             c.rateTableIndex  = rate;
00305             c.rangeTableIndex = range;
00306 
00307             user_configs.push_back(c);
00308         }
00309 
00310                 if (false == configValid)
00311                         std::cerr << "Errors exist in configuration, aborting" << std::endl;
00312         else {
00313             status = channelP->setImuConfig(storeInFlash,
00314                                             user_samplesPerMessage,
00315                                             user_configs); // can be empty
00316                         if (Status_Ok != status)
00317                                 std::cerr << "Failed to set IMU configuration: " << Channel::statusString(status) << std::endl;
00318                         else
00319                                 std::cout << "IMU configuration updated successfully\n";
00320         }
00321     }
00322 
00323 clean_out:
00324         
00325     Channel::Destroy(channelP);
00326     return 0;
00327 }


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