source: flair-src/trunk/lib/FlairSensorActuator/src/NEDPosition.cpp@ 425

Last change on this file since 425 was 425, checked in by Sanahuja Guillaume, 21 months ago

add nedposition sensor

  • Property svn:eol-style set to native
File size: 4.0 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: 2021/05/20
6// filename: NEDPosition.cpp
7//
8// author: Guillaume Sanahuja
9// Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11// version: $Id: $
12//
13// purpose: Class for a NED position sensor
14//
15//
16/*********************************************************************/
17
18#include "NEDPosition.h"
19#include <FrameworkManager.h>
20#include <Tab.h>
21#include <DataPlot1D.h>
22#include <Matrix.h>
23
24using std::string;
25using namespace flair::core;
26using namespace flair::gui;
27
28
29class NEDPosition_impl {
30public:
31 NEDPosition_impl(flair::sensor::NEDPosition* self,string name) {
32
33 MatrixDescriptor *desc = new MatrixDescriptor(6, 1);
34 desc->SetElementName(0, 0, "x");
35 desc->SetElementName(1, 0, "y");
36 desc->SetElementName(2, 0, "z");
37 desc->SetElementName(3, 0, "vx");
38 desc->SetElementName(4, 0, "vy");
39 desc->SetElementName(5, 0, "vz");
40 output = new Matrix(self, desc, floatType);
41 delete desc;
42
43 // ui
44 plot_tab = new Tab(getFrameworkManager()->GetTabWidget(), name);
45 x_plot = new DataPlot1D(plot_tab->NewRow(), "x", -10, 10);
46 x_plot->AddCurve(output->Element(0));
47 y_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "y", -10, 10);
48 y_plot->AddCurve(output->Element(1));
49 z_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "z", -2, 0);
50 z_plot->AddCurve(output->Element(2));
51 vx_plot = new DataPlot1D(plot_tab->NewRow(), "vx", -5, 5);
52 vx_plot->AddCurve(output->Element(3));
53 vy_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "vy", -5, 5);
54 vy_plot->AddCurve(output->Element(4));
55 vz_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "vz", -2, 2);
56 vz_plot->AddCurve(output->Element(5));
57 }
58
59 ~NEDPosition_impl() {}
60
61 void GetPosition(Vector3Df &point) {
62 output->GetMutex();
63 point.x = output->ValueNoMutex(0, 0);
64 point.y = output->ValueNoMutex(1, 0);
65 point.z = output->ValueNoMutex(2, 0);
66 output->ReleaseMutex();
67 }
68 void GetVelocity(Vector3Df &point) {
69 output->GetMutex();
70 point.x = output->ValueNoMutex(3, 0);
71 point.y = output->ValueNoMutex(4, 0);
72 point.z = output->ValueNoMutex(5, 0);
73 output->ReleaseMutex();
74 }
75 Matrix *output;
76 Tab *plot_tab;
77 DataPlot1D *x_plot,*y_plot,*z_plot,*vx_plot,*vy_plot,*vz_plot;
78
79private:
80
81};
82
83
84namespace flair {
85namespace sensor {
86
87NEDPosition::NEDPosition(string name)
88 : IODevice(getFrameworkManager(), name) {
89 pimpl_ = new ::NEDPosition_impl(this, name);
90 AddDataToLog(pimpl_->output);
91
92 SetIsReady(true);
93}
94
95NEDPosition::~NEDPosition(void) {
96 delete pimpl_;
97}
98
99Matrix *NEDPosition::Output(void) const { return pimpl_->output; }
100
101Tab *NEDPosition::GetPlotTab(void) const { return pimpl_->plot_tab; }
102
103DataPlot1D *NEDPosition::xPlot(void) const { return pimpl_->x_plot; }
104
105DataPlot1D *NEDPosition::yPlot(void) const { return pimpl_->y_plot; }
106
107DataPlot1D *NEDPosition::zPlot(void) const { return pimpl_->z_plot; }
108
109DataPlot1D *NEDPosition::vxPlot(void) const { return pimpl_->vx_plot; }
110
111DataPlot1D *NEDPosition::vyPlot(void) const { return pimpl_->vy_plot; }
112
113DataPlot1D *NEDPosition::vzPlot(void) const { return pimpl_->vz_plot; }
114
115void NEDPosition::GetPosition(Vector3Df &point) const {
116 pimpl_->GetPosition(point);
117}
118
119void NEDPosition::GetVelocity(Vector3Df &point) const {
120 pimpl_->GetVelocity(point);
121}
122
123void NEDPosition::SetDatas(const Vector3Df &position,const Vector3Df &velocity,const Time time) {
124 pimpl_->output->GetMutex();
125 pimpl_->output->SetValueNoMutex(0,0,position.x);
126 pimpl_->output->SetValueNoMutex(1,0,position.y);
127 pimpl_->output->SetValueNoMutex(2,0,position.z);
128 pimpl_->output->SetValueNoMutex(3,0,velocity.x);
129 pimpl_->output->SetValueNoMutex(4,0,velocity.y);
130 pimpl_->output->SetValueNoMutex(5,0,velocity.z);
131 pimpl_->output->ReleaseMutex();
132 pimpl_->output->SetDataTime(time);
133 ProcessUpdate(pimpl_->output);
134}
135
136
137} // end namespace sensor
138} // end namespace flair
Note: See TracBrowser for help on using the repository browser.