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