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

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

add AltitudeSensor class
failsafe altitude sensor in changeable

File size: 2.5 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: 2013/04/08
6// filename: MetaUsRangeFinder.cpp
7//
8// author: Guillaume Sanahuja
9// Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11// version: $Id: $
12//
13// purpose: objet integrant une dérivée
14// d'euler, un passe bas et un generateur de consigne
15// -> adapté pour un capteur d'altitude
16//
17//
18/*********************************************************************/
19
20#include "MetaUsRangeFinder.h"
21#include "UsRangeFinder.h"
22#include <LowPassFilter.h>
23#include <ButterworthLowPass.h>
24#include <Layout.h>
25#include <DataPlot1D.h>
26#include <Tab.h>
27#include <EulerDerivative.h>
28#include <GroupBox.h>
29#include <Matrix.h>
30
31using std::string;
32using namespace flair::core;
33using namespace flair::gui;
34using namespace flair::filter;
35using namespace flair::sensor;
36
37namespace flair {
38namespace meta {
39
40
41MetaUsRangeFinder::MetaUsRangeFinder(UsRangeFinder *us)
42 : AltitudeSensor(us, us->ObjectName()) {
43 //TODO: mettre une rotation entre la sortie US et le low pass
44 //pour eviter les fausses mesures quand le drone est orienté
45 /* ex
46 Vector3Df test(0,0,altitude+0.04);
47 Printf("altitude av %f %f %f %f\n",altitude,test.x,test.y,test.z);
48 test.Rotate(currentQuaternion);
49 Printf("altitude ap %f %f %f %f\n",altitude,test.x,test.y,test.z-0.04);
50 */
51 this->us = us;
52 pbas_z =
53 new ButterworthLowPass(us, us->GetLayout()->NewRow(), "Passe bas", 3);
54 vz_euler = new EulerDerivative(pbas_z, us->GetLayout()->NewRow(), "Vz");
55 pbas_vz = new ButterworthLowPass(vz_euler, us->GetLayout()->NewRow(),
56 "Passe bas v", 3);
57}
58
59MetaUsRangeFinder::~MetaUsRangeFinder() {}
60
61UsRangeFinder* MetaUsRangeFinder::GetUsRangeFinder() {
62 return us;
63}
64
65void MetaUsRangeFinder::UseDefaultPlot(void) {
66 us->UseDefaultPlot();
67
68 us->GetPlot()->AddCurve(pbas_z->GetMatrix()->Element(0), DataPlot::Blue);
69
70 vz_plot = new DataPlot1D(us->GetPlotTab()->LastRowLastCol(), "vz", -2, 2);
71 vz_plot->AddCurve(vz_euler->GetMatrix()->Element(0));
72 vz_plot->AddCurve(pbas_vz->GetMatrix()->Element(0), DataPlot::Blue);
73}
74
75gui::DataPlot1D *MetaUsRangeFinder::GetZPlot() { return us->GetPlot(); }
76
77gui::DataPlot1D *MetaUsRangeFinder::GetVzPlot() { return vz_plot; }
78
79float MetaUsRangeFinder::z(void) const { return pbas_z->Output(); }
80
81float MetaUsRangeFinder::Vz(void) const { return pbas_vz->Output(); }
82
83} // end namespace sensor
84} // end namespace flair
Note: See TracBrowser for help on using the repository browser.