/home/jgoppert/Projects/uvsim/uvsim/src/apps/insTest.cc

Go to the documentation of this file.

/*
 * insTest.cc
 * Copyright (C) James Goppert 2009 <jgoppert@users.sourceforge.net>
 *
 * insTest.cc is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * insTest.cc is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program.  If not, see <http://www.gnu.org/licenses/>.
 */


#include <iostream>
#include <string>
#include "uvsim/navigation/Ins.h"
#include "uvsim/navigation/GeoMag.h"
#include "uvsim/utilities/utilities.h"
#include "uvsim/visualization/Frame.h"
#include <osg/PositionAttitudeTransform>

int main()
{
    // constants
    const static double refreshRate = 0.05;
    const static int freq = 500;

    // cutoff frequencies
    vector<double> freqCutMb(3), freqCutFb(3), freqCutWb(3);

    // magnetometer cutoff freq
    freqCutMb(0) = 1; //312;
    freqCutMb(1) = 1; //312;
    freqCutMb(2) = 1; //312;

    // accel cutoff freq
    freqCutFb(0) = 350; //350;
    freqCutFb(1) = 350; //350;
    freqCutFb(2) = 150; //150;

    // gyro cutoff freq
    freqCutWb(0) = 120; //120;
    freqCutWb(1) = 120; //120;
    freqCutWb(2) = 120; //120;

    // variables
    float dec, dip, ti, gv;

    // Purdue airport
    float alt, glat, glon, time;
    alt = 184.7; // m
    glat = 40.41194444; // deg
    glon = -86.93361111; // deg
    time = 2009.7; // decimal years

    // test ins
    uvsim::Imu6DofSensor imu("/dev/rfcomm0", freqCutMb, freqCutFb, freqCutWb);
    uvsim::Ins ins(std::string(DATADIR)+="/data/Ins15g.dat",
                   std::string(DATADIR)+="/data/Gyro.dat",
                   std::string(DATADIR)+="/data/Mag.dat",freq,&imu);
    ins.initializeStatic(alt,glat,glon);

    // osg
    osg::Vec3 center(0,0,0), eye(0,20,-10), up(0,0,-1);
    vector<double> pos(3);
    osgViewer::Viewer *viewer = new osgViewer::Viewer;
    viewer->setUpViewInWindow(0,0,300,300);


    osg::Group *root = new osg::Group;
    uvsim::Frame *frame = new uvsim::Frame(5);
    osg::PositionAttitudeTransform *frameTrans = new osg::PositionAttitudeTransform;
    frameTrans->addChild(frame->root);
    root->addChild(frameTrans);

    viewer->setSceneData(root);
    viewer->realize();

    // loop
    for (int i=0; !viewer->done(); i++)
    {
        //display
        if ((i % int(freq*refreshRate)) == 0)
        {
            // display to terminal
            uvsim::clear();
            ins.print();

            // update visual
            vector<double> quat = ins.getQuat();
            frameTrans->setAttitude(osg::Quat(quat(1),quat(2),quat(3),quat(0)));
            pos = ins.getGeo().getCartesian();
            //center.set(pos(0),pos(1),pos(2));

            //frameTrans->setPosition(osg::Vec3(pos(0), pos(1), pos(2)));
            viewer->getCamera()->setViewMatrixAsLookAt(eye, center, up);
            viewer->frame();
        }

        // update
        ins.update();
    }
    ins.stop();
    return(0);
}

// vim:ts=4:sw=4

---