ImuTestUtility.cc

Go to the documentation of this file.

 
#ifdef WIN32
#ifndef WIN32_LEAN_AND_MEAN
#define WIN32_LEAN_AND_MEAN 1
#endif
 
#include <windows.h>
#include <winsock2.h>
#else
#include <unistd.h>
#endif
 
#include <stdio.h>
#include <stdlib.h>
#include <signal.h>
#include <string.h>
#include <errno.h>
#include <iostream>
#include <iomanip>
 
#include <MultiSense/details/utility/Portability.hh>
#include <MultiSense/MultiSenseChannel.hh>
 
#include <getopt/getopt.h>
 
using namespace crl::multisense;
 
namespace {  // anonymous
 
volatile bool doneG         = false;
FILE*         logFileP      = stdout;
uint32_t      accel_samples = 0;
uint32_t      gyro_samples  = 0;
uint32_t      mag_samples   = 0;
int64_t       sequence      = -1;
uint32_t      messages      = 0;
uint32_t      dropped       = 0;
 
void usage(const char *programNameP)
{
    std::cerr << "USAGE: " << programNameP << " [<options>]" << std::endl;
    std::cerr << "Where <options> are:" << std::endl;
    std::cerr << "\t-a <ip_address>    : IPV4 address (default=10.66.171.21)" << std::endl;
    std::cerr << "\t-m <mtu>           : default=1500" << std::endl;
    std::cerr << "\t-f <log_file>      : FILE to log IMU data (stdout by default)" << std::endl;
 
    exit(1);
}
 
#ifdef WIN32
BOOL WINAPI signalHandler(DWORD dwCtrlType)
{
    CRL_UNUSED (dwCtrlType);
    std::cerr << "Shutting down on signal: CTRL-C" << std::endl;
    doneG = true;
    return TRUE;
}
#else
void signalHandler(int sig)
{
    std::cerr << "Shutting down on signal: " << strsignal(sig) << std::endl;
    doneG = true;
}
#endif
 
void imuCallback(const imu::Header& header,
                 void              *userDataP)
{
    (void) userDataP;
    std::vector<imu::Sample>::const_iterator it = header.samples.begin();
 
    for(; it!=header.samples.end(); ++it) {
 
        const imu::Sample& s = *it;
 
        switch(s.type) {
        case imu::Sample::Type_Accelerometer: accel_samples ++; break;
        case imu::Sample::Type_Gyroscope:     gyro_samples ++;  break;
        case imu::Sample::Type_Magnetometer:  mag_samples ++;   break;
        }
 
        if (logFileP)
            fprintf(logFileP, "%d %.6f %.6f %.6f %.6f\n",
                    s.type,
                    s.timeSeconds + 1e-6 * s.timeMicroSeconds,
                    s.x, s.y, s.z);
    }
 
    if (-1 == sequence)
        sequence = header.sequence;
    else if ((sequence + 1) != header.sequence) {
        const int32_t d = static_cast<int32_t> (header.sequence - (sequence + 1));
        dropped += d;
    }
 
    sequence = header.sequence;
    messages ++;
}
 
} // anonymous
 
int main(int    argc,
         char **argvPP)
{
    std::string currentAddress = "10.66.171.21";
    const char *logFileNameP   = NULL;
    uint32_t    mtu            = 0;
 
#if WIN32
    SetConsoleCtrlHandler (signalHandler, TRUE);
#else
    signal(SIGINT, signalHandler);
#endif
 
    //
    // Parse args
 
    int c;
 
    while(-1 != (c = getopt(argc, argvPP, "a:f:m:v")))
        switch(c) {
        case 'a': currentAddress = std::string(optarg);    break;
        case 'f': logFileNameP   = optarg;                 break;
        case 'm': mtu            = atoi(optarg);           break;
        default: usage(*argvPP);                           break;
        }
 
    //
    // Initialize communications.
 
    Channel *channelP = Channel::Create(currentAddress);
    if (NULL == channelP) {
        std::cerr << "Failed to establish communications with \"" << currentAddress << "\"" << std::endl;
        return -1;
    }
 
    //
    // Query firmware version
 
    system::VersionInfo v;
 
    Status status = channelP->getVersionInfo(v);
    if (Status_Ok != status) {
        std::cerr << "Failed to query sensor version: " << Channel::statusString(status) << std::endl;
        goto clean_out;
    }
 
    //
    // Make sure firmware supports IMU
 
    if (v.sensorFirmwareVersion <= 0x0202) {
        std::cerr << "IMU support requires sensor firmware version v2.3 or greater, sensor is " <<
                "running v" << (v.sensorFirmwareVersion >> 8) << "." << (v.sensorFirmwareVersion & 0xff) << std::endl;
        goto clean_out;
    }
 
    //
    // Turn off all streams by default
 
    status = channelP->stopStreams(Source_All);
    if (Status_Ok != status) {
        std::cerr << "Failed to stop streams: " << Channel::statusString(status) << std::endl;
        goto clean_out;
    }
 
    //
    // Was logging requested ?
 
    if (NULL != logFileNameP) {
 
        //
        // Open the log file
 
        logFileP = fopen(logFileNameP, "w+");
        if (NULL == logFileP) {
            std::cerr << "Failed to open \"" << logFileNameP << "\" for writing: " << strerror(errno) << std::endl;
            goto clean_out;
        }
 
    }
 
    //
    // Change MTU
    if (mtu >= 1500)
        status = channelP->setMtu(mtu);
    else 
        status = channelP->setBestMtu();
    if (Status_Ok != status) {
        std::cerr << "Failed to set MTU: " << Channel::statusString(status) << std::endl;
        goto clean_out;
    }
 
    //
    // Add callbacks
 
    channelP->addIsolatedCallback(imuCallback);
 
    //
    // Start streaming
 
    status = channelP->startStreams(Source_Imu);
    if (Status_Ok != status) {
        std::cerr << "Failed to start streams: " << Channel::statusString(status) << std::endl;
        goto clean_out;
    }
 
    while(!doneG)
        usleep(100000);
 
    //
    // Stop streaming
 
    status = channelP->stopStreams(Source_All);
    if (Status_Ok != status) {
        std::cerr << "Failed to stop streams: " << Channel::statusString(status) << std::endl;
    }
 
    //
    // Report simple stats
 
    {
        int64_t imu_total = accel_samples + gyro_samples + mag_samples;
        if (imu_total > 0) {
            std::cerr << "IMU samples : " <<
                         "total: " << imu_total << ", " <<
                         "accel: " << std::fixed << std::setprecision(1) << (100.0 * static_cast<double>(accel_samples) / static_cast<double>(imu_total)) << "%, " <<
                         "gyro: " << std::fixed << std::setprecision(1) << (100.0 * static_cast<double>(gyro_samples) / static_cast<double>(imu_total)) << "%, " <<
                         "mag: " << std::fixed << std::setprecision(1) << (100.0 * static_cast<double>(mag_samples) / static_cast<double>(imu_total)) << "%" << std::endl;
        }
 
        if (messages > 0)
            std::cerr << "IMU messages: total: " << messages << ", " <<
                         "dropped: " << dropped << "(" << std::fixed << std::setprecision(6) << (100* static_cast<double>(dropped) / static_cast<double>(messages+dropped)) << "%)" << std::endl;
    }
 
clean_out:
 
    if (logFileNameP)
        fclose(logFileP);
 
    Channel::Destroy(channelP);
    return 0;
}