source: flair-src/trunk/lib/FlairFilter/src/Ahrs_impl.cpp@ 10

// %flair:license{

// This file is part of the Flair framework distributed under the

// CECILL-C License, Version 1.0.

// %flair:license}

//  created:    2014/01/14

//  filename:   Ahrs_impl.cpp

//

//  author:     Guillaume Sanahuja

//              Copyright Heudiasyc UMR UTC/CNRS 7253

//

//  version:    $Id: $

//

//  purpose:    Virtual class for Ahrs_impl

//

//

/*********************************************************************/



#include "Ahrs_impl.h"

#include "Ahrs.h"

#include <Imu.h>

#include <Tab.h>

#include <TabWidget.h>

#include <DataPlot1D.h>

#include <AhrsData.h>



using std::string;

using namespace flair::core;

using namespace flair::gui;

using namespace flair::filter;

using namespace flair::sensor;



Ahrs_impl::Ahrs_impl(Ahrs* inSelf): self(inSelf) {

    rollPlot=NULL;

    pitchPlot=NULL;

    yawPlot=NULL;

    wXPlot=NULL;

    wYPlot=NULL;

    wZPlot=NULL;

    q0Plot=NULL;

    q1Plot=NULL;

    q2Plot=NULL;

    q3Plot=NULL;



    eulerTab=NULL;

    quaternionTab=NULL;



    ahrsData=new AhrsData(self);

}



Ahrs_impl::~Ahrs_impl() {

}



void Ahrs_impl::UseDefaultPlot(void) {



    eulerTab=new Tab(((Imu*)(self->Parent()))->tab,"AHRS");

    rollPlot=new DataPlot1D(eulerTab->NewRow(),"roll",-30,30);

    rollPlot->AddCurve(ahrsData->Element(AhrsData::RollDeg));

    pitchPlot=new DataPlot1D(eulerTab->LastRowLastCol(),"pitch",-30,30);

    pitchPlot->AddCurve(ahrsData->Element(AhrsData::PitchDeg));

    yawPlot=new DataPlot1D(eulerTab->LastRowLastCol(),"yaw",-180,180);

    yawPlot->AddCurve(ahrsData->Element(AhrsData::YawDeg));



    wXPlot=new DataPlot1D(eulerTab->NewRow(),"w_x",-200,200);

    wXPlot->AddCurve(ahrsData->Element(AhrsData::WxDeg));

    wYPlot=new DataPlot1D(eulerTab->LastRowLastCol(),"w_y",-200,200);

    wYPlot->AddCurve(ahrsData->Element(AhrsData::WyDeg));

    wZPlot=new DataPlot1D(eulerTab->LastRowLastCol(),"w_z",-200,200);

    wZPlot->AddCurve(ahrsData->Element(AhrsData::WzDeg));



    quaternionTab=new Tab(((Imu*)(self->Parent()))->tab,"Quaternion");

    q0Plot=new DataPlot1D(quaternionTab->NewRow(),"q0",-1,1);

    q0Plot->AddCurve(ahrsData->Element(AhrsData::Q0));

    q1Plot=new DataPlot1D(quaternionTab->NewRow(),"q1",-1,1);

    q1Plot->AddCurve(ahrsData->Element(AhrsData::Q1));

    q2Plot=new DataPlot1D(quaternionTab->LastRowLastCol(),"q2",-1,1);

    q2Plot->AddCurve(ahrsData->Element(AhrsData::Q2));

    q3Plot=new DataPlot1D(quaternionTab->LastRowLastCol(),"q3",-1,1);

    q3Plot->AddCurve(ahrsData->Element(AhrsData::Q3));



}



void Ahrs_impl::AddPlot(const AhrsData *ahrsData,DataPlot::Color_t color) {

    if(rollPlot!=NULL) {

        rollPlot->AddCurve(ahrsData->Element(AhrsData::RollDeg),color);

        pitchPlot->AddCurve(ahrsData->Element(AhrsData::PitchDeg),color);

        yawPlot->AddCurve(ahrsData->Element(AhrsData::YawDeg),color);

    }

    if(wXPlot!=NULL) {

        wXPlot->AddCurve(ahrsData->Element(AhrsData::WxDeg),color);

        wYPlot->AddCurve(ahrsData->Element(AhrsData::WyDeg),color);

        wZPlot->AddCurve(ahrsData->Element(AhrsData::WzDeg),color);

    }

    if(quaternionTab!=NULL) {

        q0Plot->AddCurve(ahrsData->Element(AhrsData::Q0),color);

        q1Plot->AddCurve(ahrsData->Element(AhrsData::Q1),color);

        q2Plot->AddCurve(ahrsData->Element(AhrsData::Q2),color);

        q3Plot->AddCurve(ahrsData->Element(AhrsData::Q3),color);

    }

}