source: flair-src/tags/latest/lib/FlairFilter/src/Ahrs_impl.cpp

Last change on this file was 15, checked in by Bayard Gildas, 6 years ago

sources reformatted with flair-format-dir script

File size: 3.2 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
52void Ahrs_impl::UseDefaultPlot(void) {
53
54  eulerTab = new Tab(((Imu *)(self->Parent()))->tab, "AHRS");
55  rollPlot = new DataPlot1D(eulerTab->NewRow(), "roll", -30, 30);
56  rollPlot->AddCurve(ahrsData->Element(AhrsData::RollDeg));
57  pitchPlot = new DataPlot1D(eulerTab->LastRowLastCol(), "pitch", -30, 30);
58  pitchPlot->AddCurve(ahrsData->Element(AhrsData::PitchDeg));
59  yawPlot = new DataPlot1D(eulerTab->LastRowLastCol(), "yaw", -180, 180);
60  yawPlot->AddCurve(ahrsData->Element(AhrsData::YawDeg));
61
62  wXPlot = new DataPlot1D(eulerTab->NewRow(), "w_x", -200, 200);
63  wXPlot->AddCurve(ahrsData->Element(AhrsData::WxDeg));
64  wYPlot = new DataPlot1D(eulerTab->LastRowLastCol(), "w_y", -200, 200);
65  wYPlot->AddCurve(ahrsData->Element(AhrsData::WyDeg));
66  wZPlot = new DataPlot1D(eulerTab->LastRowLastCol(), "w_z", -200, 200);
67  wZPlot->AddCurve(ahrsData->Element(AhrsData::WzDeg));
68
69  quaternionTab = new Tab(((Imu *)(self->Parent()))->tab, "Quaternion");
70  q0Plot = new DataPlot1D(quaternionTab->NewRow(), "q0", -1, 1);
71  q0Plot->AddCurve(ahrsData->Element(AhrsData::Q0));
72  q1Plot = new DataPlot1D(quaternionTab->NewRow(), "q1", -1, 1);
73  q1Plot->AddCurve(ahrsData->Element(AhrsData::Q1));
74  q2Plot = new DataPlot1D(quaternionTab->LastRowLastCol(), "q2", -1, 1);
75  q2Plot->AddCurve(ahrsData->Element(AhrsData::Q2));
76  q3Plot = new DataPlot1D(quaternionTab->LastRowLastCol(), "q3", -1, 1);
77  q3Plot->AddCurve(ahrsData->Element(AhrsData::Q3));
78}
79
80void Ahrs_impl::AddPlot(const AhrsData *ahrsData, DataPlot::Color_t color) {
81  if (rollPlot != NULL) {
82    rollPlot->AddCurve(ahrsData->Element(AhrsData::RollDeg), color);
83    pitchPlot->AddCurve(ahrsData->Element(AhrsData::PitchDeg), color);
84    yawPlot->AddCurve(ahrsData->Element(AhrsData::YawDeg), color);
85  }
86  if (wXPlot != NULL) {
87    wXPlot->AddCurve(ahrsData->Element(AhrsData::WxDeg), color);
88    wYPlot->AddCurve(ahrsData->Element(AhrsData::WyDeg), color);
89    wZPlot->AddCurve(ahrsData->Element(AhrsData::WzDeg), color);
90  }
91  if (quaternionTab != NULL) {
92    q0Plot->AddCurve(ahrsData->Element(AhrsData::Q0), color);
93    q1Plot->AddCurve(ahrsData->Element(AhrsData::Q1), color);
94    q2Plot->AddCurve(ahrsData->Element(AhrsData::Q2), color);
95    q3Plot->AddCurve(ahrsData->Element(AhrsData::Q3), color);
96  }
97}
Note: See TracBrowser for help on using the repository browser.