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

// %flair:license{

// This file is part of the Flair framework distributed under the

// CECILL-C License, Version 1.0.

// %flair:license}

//  created:    2013/04/08

//  filename:   MetaUsRangeFinder.cpp

//

//  author:     Guillaume Sanahuja

//              Copyright Heudiasyc UMR UTC/CNRS 7253

//

//  version:    $Id: $

//

//  purpose:    objet integrant une dérivée

//              d'euler, un passe bas et un generateur de consigne

//              -> adapté pour un capteur d'altitude

//

//

/*********************************************************************/



#include "MetaUsRangeFinder.h"

#include "UsRangeFinder.h"

#include <LowPassFilter.h>

#include <ButterworthLowPass.h>

#include <Layout.h>

#include <DataPlot1D.h>

#include <Tab.h>

#include <EulerDerivative.h>

#include <GroupBox.h>

#include <Matrix.h>



using std::string;

using namespace flair::core;

using namespace flair::gui;

using namespace flair::filter;

using namespace flair::sensor;



namespace flair {

namespace meta {

  



MetaUsRangeFinder::MetaUsRangeFinder(UsRangeFinder *us)

    : AltitudeSensor(us, us->ObjectName()) {

  //TODO: mettre une rotation entre la sortie US et le low pass

  //pour eviter les fausses mesures quand le drone est orienté

  /* ex

  Vector3Df test(0,0,altitude+0.04);

  Printf("altitude av %f %f %f %f\n",altitude,test.x,test.y,test.z);

  test.Rotate(currentQuaternion);

  Printf("altitude ap %f %f %f %f\n",altitude,test.x,test.y,test.z-0.04);

   */

  this->us = us;

  pbas_z =

      new ButterworthLowPass(us, us->GetLayout()->NewRow(), "Passe bas", 3);

  vz_euler = new EulerDerivative(pbas_z, us->GetLayout()->NewRow(), "Vz");

  pbas_vz = new ButterworthLowPass(vz_euler, us->GetLayout()->NewRow(),

                                   "Passe bas v", 3);

}



MetaUsRangeFinder::~MetaUsRangeFinder() {}



UsRangeFinder* MetaUsRangeFinder::GetUsRangeFinder() {

    return us;

}



void MetaUsRangeFinder::UseDefaultPlot(void) {

  us->UseDefaultPlot();



  us->GetPlot()->AddCurve(pbas_z->GetMatrix()->Element(0), DataPlot::Blue);



  vz_plot = new DataPlot1D(us->GetPlotTab()->LastRowLastCol(), "vz", -2, 2);

  vz_plot->AddCurve(vz_euler->GetMatrix()->Element(0));

  vz_plot->AddCurve(pbas_vz->GetMatrix()->Element(0), DataPlot::Blue);

}



gui::DataPlot1D *MetaUsRangeFinder::GetZPlot() { return us->GetPlot(); }



gui::DataPlot1D *MetaUsRangeFinder::GetVzPlot() { return vz_plot; }



float MetaUsRangeFinder::z(void) const { return pbas_z->Output(); }



float MetaUsRangeFinder::Vz(void) const { return pbas_vz->Output(); }



} // end namespace sensor

} // end namespace flair