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

Last change on this file since 7 was 7, checked in by Sanahuja Guillaume, 5 years ago

filter and meta

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