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, 7 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.