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

Last change on this file since 10 was 10, checked in by Sanahuja Guillaume, 6 years ago

lic

File size: 3.3 KB
Line 
1// %flair:license{
2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
4// %flair:license}
5//  created:    2014/01/14
6//  filename:   Ahrs_impl.cpp
7//
8//  author:     Guillaume Sanahuja
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    Virtual class for Ahrs_impl
14//
15//
16/*********************************************************************/
17
18#include "Ahrs_impl.h"
19#include "Ahrs.h"
20#include <Imu.h>
21#include <Tab.h>
22#include <TabWidget.h>
23#include <DataPlot1D.h>
24#include <AhrsData.h>
25
26using std::string;
27using namespace flair::core;
28using namespace flair::gui;
29using namespace flair::filter;
30using namespace flair::sensor;
31
32Ahrs_impl::Ahrs_impl(Ahrs* inSelf): self(inSelf) {
33    rollPlot=NULL;
34    pitchPlot=NULL;
35    yawPlot=NULL;
36    wXPlot=NULL;
37    wYPlot=NULL;
38    wZPlot=NULL;
39    q0Plot=NULL;
40    q1Plot=NULL;
41    q2Plot=NULL;
42    q3Plot=NULL;
43
44    eulerTab=NULL;
45    quaternionTab=NULL;
46
47    ahrsData=new AhrsData(self);
48}
49
50Ahrs_impl::~Ahrs_impl() {
51}
52
53void Ahrs_impl::UseDefaultPlot(void) {
54
55    eulerTab=new Tab(((Imu*)(self->Parent()))->tab,"AHRS");
56    rollPlot=new DataPlot1D(eulerTab->NewRow(),"roll",-30,30);
57    rollPlot->AddCurve(ahrsData->Element(AhrsData::RollDeg));
58    pitchPlot=new DataPlot1D(eulerTab->LastRowLastCol(),"pitch",-30,30);
59    pitchPlot->AddCurve(ahrsData->Element(AhrsData::PitchDeg));
60    yawPlot=new DataPlot1D(eulerTab->LastRowLastCol(),"yaw",-180,180);
61    yawPlot->AddCurve(ahrsData->Element(AhrsData::YawDeg));
62
63    wXPlot=new DataPlot1D(eulerTab->NewRow(),"w_x",-200,200);
64    wXPlot->AddCurve(ahrsData->Element(AhrsData::WxDeg));
65    wYPlot=new DataPlot1D(eulerTab->LastRowLastCol(),"w_y",-200,200);
66    wYPlot->AddCurve(ahrsData->Element(AhrsData::WyDeg));
67    wZPlot=new DataPlot1D(eulerTab->LastRowLastCol(),"w_z",-200,200);
68    wZPlot->AddCurve(ahrsData->Element(AhrsData::WzDeg));
69
70    quaternionTab=new Tab(((Imu*)(self->Parent()))->tab,"Quaternion");
71    q0Plot=new DataPlot1D(quaternionTab->NewRow(),"q0",-1,1);
72    q0Plot->AddCurve(ahrsData->Element(AhrsData::Q0));
73    q1Plot=new DataPlot1D(quaternionTab->NewRow(),"q1",-1,1);
74    q1Plot->AddCurve(ahrsData->Element(AhrsData::Q1));
75    q2Plot=new DataPlot1D(quaternionTab->LastRowLastCol(),"q2",-1,1);
76    q2Plot->AddCurve(ahrsData->Element(AhrsData::Q2));
77    q3Plot=new DataPlot1D(quaternionTab->LastRowLastCol(),"q3",-1,1);
78    q3Plot->AddCurve(ahrsData->Element(AhrsData::Q3));
79
80}
81
82void Ahrs_impl::AddPlot(const AhrsData *ahrsData,DataPlot::Color_t color) {
83    if(rollPlot!=NULL) {
84        rollPlot->AddCurve(ahrsData->Element(AhrsData::RollDeg),color);
85        pitchPlot->AddCurve(ahrsData->Element(AhrsData::PitchDeg),color);
86        yawPlot->AddCurve(ahrsData->Element(AhrsData::YawDeg),color);
87    }
88    if(wXPlot!=NULL) {
89        wXPlot->AddCurve(ahrsData->Element(AhrsData::WxDeg),color);
90        wYPlot->AddCurve(ahrsData->Element(AhrsData::WyDeg),color);
91        wZPlot->AddCurve(ahrsData->Element(AhrsData::WzDeg),color);
92    }
93    if(quaternionTab!=NULL) {
94        q0Plot->AddCurve(ahrsData->Element(AhrsData::Q0),color);
95        q1Plot->AddCurve(ahrsData->Element(AhrsData::Q1),color);
96        q2Plot->AddCurve(ahrsData->Element(AhrsData::Q2),color);
97        q3Plot->AddCurve(ahrsData->Element(AhrsData::Q3),color);
98    }
99}
Note: See TracBrowser for help on using the repository browser.