iCub-main
ImuFilter.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014 iCub Facility - Istituto Italiano di Tecnologia
3  * Author: Ugo Pattacini, Nicolo' Genesio
4  * email: ugo.pattacini@iit.it, nicolo.genesio@iit.it
5  * Permission is granted to copy, distribute, and/or modify this program
6  * under the terms of the GNU General Public License, version 2 or any
7  * later version published by the Free Software Foundation.
8  *
9  * A copy of the license can be found at
10  * http://www.robotcub.org/icub/license/gpl.txt
11  *
12  * This program is distributed in the hope that it will be useful, but
13  * WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
15  * Public License for more details
16 */
17 
18 #ifndef IMU_FILTER_H
19 #define IMU_FILTER_H
20 
21 
22 #include <yarp/os/PeriodicThread.h>
23 #include <yarp/os/Stamp.h>
24 #include <yarp/dev/DeviceDriver.h>
25 #include <yarp/dev/PolyDriver.h>
26 #include <yarp/sig/Vector.h>
27 
28 #include "PassThroughInertial.h"
29 
30 #include <yarp/math/Math.h>
31 
33 #include <iCub/ctrl/filters.h>
34 #include <iCub/ctrl/pids.h>
35 
36 #include <vector>
37 #include <mutex>
38 
39 #include <iostream>
40 
41 namespace yarp {
42  namespace dev {
43  class ImuFilter;
44  }
45 }
46 
158 class yarp::dev::ImuFilter : public yarp::os::PeriodicThread,
160 {
161 
162  yarp::os::BufferedPort<yarp::sig::Vector> bPort;
163 
164  iCub::ctrl::MedianFilter gyroFilt;
165  iCub::ctrl::MedianFilter magFilt;
167  iCub::ctrl::Integrator biasInt;
168 
169  yarp::dev::IThreeAxisGyroscopes *iGyro{nullptr};
170 
171  yarp::sig::Vector gyroBias, gyro;
172  double mag_vel_thres_up{0.0};
173  double mag_vel_thres_down{0.0};
174  double bias_gain{0.0};
175  bool verbose{false};
176  bool adaptGyroBias{false};
177  int m_period_ms{20};
178  double gyroTs{0.0};
179  mutable std::mutex m_mutex;
180  yarp::os::Stamp stampBias;
181  double prevTs{0.0};
182 
183 public:
184  ImuFilter();
185  virtual ~ImuFilter() = default;
186 
187  /* PeriodicThread methods */
188  bool threadInit() override;
189  void run() override;
190  void threadRelease() override;
191 
192  /* Device Driver methods */
193  bool open(yarp::os::Searchable& config) override;
194  bool close() override;
195 
196  /* IMultipleWrapper methods */
197  bool attachAll(const yarp::dev::PolyDriverList &p) override;
198  bool detachAll() override;
199 
200 
201  bool getThreeAxisGyroscopeMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const override;
202 
203 };
204 
205 #endif // IMU_FILTER_H
iCub::ctrl::Integrator
Definition: pids.h:47
yarp::dev::ImuFilter::threadInit
bool threadInit() override
Definition: ImuFilter.cpp:39
out
out
Definition: sine.m:8
yarp::dev::ImuFilter::open
bool open(yarp::os::Searchable &config) override
Definition: ImuFilter.cpp:111
yarp::dev::ImuFilter::close
bool close() override
Definition: ImuFilter.cpp:155
p
p
Definition: show_eyes_axes.m:23
filters.h
yarp::dev::ImuFilter::attachAll
bool attachAll(const yarp::dev::PolyDriverList &p) override
Definition: ImuFilter.cpp:162
yarp::dev::ImuFilter::run
void run() override
Definition: ImuFilter.cpp:42
yarp::dev::PassThroughInertial
Definition: PassThroughInertial.h:16
yarp::dev::ImuFilter::ImuFilter
ImuFilter()
Definition: ImuFilter.cpp:33
yarp::dev::ImuFilter::getThreeAxisGyroscopeMeasure
bool getThreeAxisGyroscopeMeasure(size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
Definition: ImuFilter.cpp:177
adaptWinPolyEstimator.h
iCub::ctrl::AWLinEstimator
Definition: adaptWinPolyEstimator.h:184
yarp
Copyright (C) 2008 RobotCub Consortium.
Definition: DebugInterfaces.h:51
yarp::dev::ImuFilter::~ImuFilter
virtual ~ImuFilter()=default
yarp::dev::ImuFilter::threadRelease
void threadRelease() override
Definition: ImuFilter.cpp:108
iCub::ctrl::MedianFilter
Definition: filters.h:318
yarp::dev::ImuFilter
Definition: ImuFilter.h:158
PassThroughInertial.h
pids.h
yarp::dev::ImuFilter::detachAll
bool detachAll() override
Definition: ImuFilter.cpp:172