source: flair-src/trunk/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
RevLine 
[10]1// %flair:license{
[15]2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
[10]4// %flair:license}
[7]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
[15]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;
[7]43
[15]44 eulerTab = NULL;
45 quaternionTab = NULL;
[7]46
[15]47 ahrsData = new AhrsData(self);
[7]48}
49
[15]50Ahrs_impl::~Ahrs_impl() {}
[7]51
52void Ahrs_impl::UseDefaultPlot(void) {
53
[15]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));
[7]61
[15]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));
[7]68
[15]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));
[7]78}
79
[15]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 }
[7]97}
Note: See TracBrowser for help on using the repository browser.