| 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 |
|
|---|
| 26 | using std::string;
|
|---|
| 27 | using namespace flair::core;
|
|---|
| 28 | using namespace flair::gui;
|
|---|
| 29 | using namespace flair::filter;
|
|---|
| 30 | using namespace flair::sensor;
|
|---|
| 31 |
|
|---|
| 32 | Ahrs_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 |
|
|---|
| 50 | Ahrs_impl::~Ahrs_impl() {}
|
|---|
| 51 |
|
|---|
| 52 | void 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 |
|
|---|
| 80 | void 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 | }
|
|---|
