vos/corelibs/tracking/test_isense.cc

Go to the documentation of this file.

#include <stdio.h>
#include <unistd.h>
#include <stdexcept>
#include "intersenseorientation.hh"

using namespace std;

int main(int argc, char** argv) {
    long t = 1000000;
    for(int i = 1; i < argc; i++) {
        if(!strcmp(argv[i], "--flood") )
            t = 0;
        if(!strcmp(argv[i], "-t"))
            t = atoi(argv[++i]);
    }

    printf("Waiting %d microseconds between queries.\n", t);

    double p = 0, y = 0, r = 0;
    IntersenseOrientation* ori;
    try {
        ori = new IntersenseOrientation("/dev/isense", 115200);
    } catch(exception& e) {
        printf("Error connecting to tracker on /dev/isense at 115200 baud: %s\n", e.what());
    }

    while(1) {
        try {
            ori->getEulerAngles(&y, &p, &r);
        } catch(exception& e) {
            printf("Error: %s\n", e.what());
            usleep(250000); // sleep 1/4 second

            /*
            delete ori;
            ori = 0;
            while(!ori) {
                usleep(t*2);
                try {
                    printf("Reconnecting...\n");
                    ori = new IntersenseOrientation("/dev/isense", 115200);
                } catch(exception& e) {
                    printf("Error connecting to tracker on /dev/isense at 115200 baud: %s\n", e.what());
                    ori = 0;
                }
            }
            */
        }
        printf("Yaw=%.10f, Pitch=%.10f, Roll=%.10f\n", y, p, r);
        usleep(t);
    }

}

---