ObserverMain.cpp

Go to the documentation of this file.

#include <Utils/Measurement.h>
#include <matrix/Matrix.h>
#include <Base/Device.h>
#include <Utils/DeviceNames.h>
#include <Base/Sensor.h>
#include <Sensors/AttitudeSensor.h>
#include <Sensors/RateGyro.h>
#include <Sensors/DMURateGyro.h>
#include <Sensors/DMU.h>
#include <Base/Whorl.h>
#include "DefaultObserver.h"
#include <iostream>

using namespace O_SESSAME;
using namespace std;

int main() {
        
        Whorl WhorlOne;
        
        /** Add the rate gyros to the Whorl object */
        DMURateGyro* rg1 = new DMURateGyro(1);
        DMURateGyro* rg2 = new DMURateGyro(2);
        DMURateGyro* rg3 = new DMURateGyro(3);
        WhorlOne.SetSensor(string("RG1"), rg1);
        WhorlOne.SetSensor(string("RG2"), rg2);
        WhorlOne.SetSensor(string("RG3"), rg3);
        
        /** Set Whorl object initial conditions */
        Vector zeroVector(3);   
        WhorlOne.SetState(zeroVector);
        WhorlOne.SetControl(zeroVector);
        
        /** Set Whorl object parameters */
        Vector params(6);
        params(1) = 2.0;
        params(4) = 3.0;
        params(6) = 4.0;
        WhorlOne.SetParameter(params);
        
        struct timeval t0;
        WhorlOne.GetSensor(string("RG1"))->GetMeasurement().GetTime(t0);
        WhorlOne.SetTimeOfEstimate(t0);
        
        DefaultObserver myObserver;
        myObserver.Initialize(&WhorlOne);
        
        myObserver.Run();
        
        delete rg1;
        delete rg2;
        delete rg3;
        
        cout << "I made it all the way through!" << endl;
        
}

---