source: flair-src/trunk/lib/FlairMeta/src/MetaVrpnObject.cpp

Last change on this file was 432, checked in by Sanahuja Guillaume, 5 months ago

add AltitudeSensor? class
failsafe altitude sensor in changeable

File size: 4.1 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:    2013/04/08
6//  filename:   MetaVrpnObject.cpp
7//
8//  author:     Guillaume Sanahuja
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    objet integrant objet vrpn et une dérivée
14//              d'euler
15//
16//
17/*********************************************************************/
18
19#include "MetaVrpnObject.h"
20#include <EulerDerivative.h>
21#include <LowPassFilter.h>
22#include <IODevice.h>
23#include <GridLayout.h>
24#include <DataPlot1D.h>
25#include <DataPlot2D.h>
26#include <Tab.h>
27#include <TabWidget.h>
[214]28#include <Matrix.h>
[432]29#include <AltitudeSensor.h>
[7]30
[15]31using std::string;
[7]32using namespace flair::core;
33using namespace flair::gui;
34using namespace flair::sensor;
35using namespace flair::filter;
36
[15]37namespace flair {
[432]38namespace sensor {
39class MetaVrpnObjectAltitudeSensor : public AltitudeSensor {
40  public:
41    MetaVrpnObjectAltitudeSensor(meta::MetaVrpnObject* parent,std::string name): AltitudeSensor(parent,name) {
42      this->parent=parent;
43    }
44    ~MetaVrpnObjectAltitudeSensor() {
45    }
46
47    //z and dz must be in uav's frame
48    float z(void) const {
49      Vector3Df uav_pos;
50      parent->GetPosition(uav_pos);
51      return -uav_pos.z;
52    }
53    float Vz(void) const {
54      Vector3Df uav_vel;
55      parent->GetSpeed(uav_vel);
56      return -uav_vel.z;
57    }
58
59  private:
60     meta::MetaVrpnObject* parent;
61     
62};
63}// end namespace sensor
64}// end namespace flair
65
66namespace flair {
[15]67namespace meta {
68
[286]69MetaVrpnObject::MetaVrpnObject(string name,VrpnClient *client)
70    : VrpnObject( name, client->GetTabWidget(),client) {
71  ConstructorCommon(name,client);
[7]72}
73
[310]74MetaVrpnObject::MetaVrpnObject(string name,uint8_t id,VrpnClient *client)
[286]75    : VrpnObject(name, id, client->GetTabWidget(),client) {
76  ConstructorCommon( name,client);
[7]77}
78
[286]79void MetaVrpnObject::ConstructorCommon(string name,VrpnClient *client) {
[318]80  MatrixDescriptor *desc = new MatrixDescriptor(7, 1);
[135]81  for (int i = 0; i < desc->Rows(); i++) {
[15]82    desc->SetElementName(i, 0, Output()->Name(i, 0));
83  }
[214]84  Matrix *prev_value = new Matrix(this, desc, floatType, name);
[148]85  delete desc;
[7]86
[286]87  pbas = new LowPassFilter(this, client->GetLayout()->NewRow(),
[15]88                           name + " Passe bas", prev_value);
[147]89  delete prev_value;
[7]90
[318]91  desc = new MatrixDescriptor(7, 1);
[135]92  for (int i = 0; i < desc->Rows(); i++) {
[15]93    desc->SetElementName(i, 0, "d" + Output()->Name(i, 0));
94  }
[214]95  prev_value = new Matrix(this, desc, floatType, name);
[148]96  delete desc;
[7]97
[286]98  euler = new EulerDerivative(pbas, client->GetLayout()->NewRow(),
[15]99                              name + "_euler", prev_value);
[147]100  delete prev_value;
[7]101
[15]102  vx_opti_plot = new DataPlot1D(GetPlotTab()->NewRow(), "vx", -3, 3);
[214]103  vx_opti_plot->AddCurve(euler->GetMatrix()->Element(4));
[15]104  vy_opti_plot = new DataPlot1D(GetPlotTab()->LastRowLastCol(), "vy", -3, 3);
[214]105  vy_opti_plot->AddCurve(euler->GetMatrix()->Element(5));
[15]106  vz_opti_plot = new DataPlot1D(GetPlotTab()->LastRowLastCol(), "vz", -2, 2);
[214]107  vz_opti_plot->AddCurve(euler->GetMatrix()->Element(6));
[7]108
[286]109  plot_tab = new Tab(client->GetTabWidget(), "Mesures (xy) " + name);
[15]110  xy_plot = new DataPlot2D(plot_tab->NewRow(), "xy", "y", -5, 5, "x", -5, 5);
[135]111  xy_plot->AddCurve(Output()->Element(5, 0), Output()->Element(4, 0));
[432]112 
113  altitudeSensor=new MetaVrpnObjectAltitudeSensor(this,name);
[7]114}
115
[432]116sensor::AltitudeSensor *MetaVrpnObject::GetAltitudeSensor(void) const {
117  return altitudeSensor;
118}
119
[15]120MetaVrpnObject::~MetaVrpnObject() { delete plot_tab; }
[7]121
[15]122DataPlot1D *MetaVrpnObject::VxPlot(void) const { return vx_opti_plot; }
[7]123
[15]124DataPlot1D *MetaVrpnObject::VyPlot(void) const { return vy_opti_plot; }
[7]125
[15]126DataPlot1D *MetaVrpnObject::VzPlot(void) const { return vz_opti_plot; }
[7]127
[15]128DataPlot2D *MetaVrpnObject::XyPlot(void) const { return xy_plot; }
[7]129
[167]130void MetaVrpnObject::GetSpeed(Vector3Df &speed) const {
[135]131  speed.x = euler->Output(4, 0);
132  speed.y = euler->Output(5, 0);
133  speed.z = euler->Output(6, 0);
[7]134}
135
[420]136EulerDerivative* MetaVrpnObject::GetEulerDerivative(void) const {
137        return euler;
138}
139
[432]140} // end namespace meta
[170]141} // end namespace flair
Note: See TracBrowser for help on using the repository browser.