sot-core  4.11.4
Hierarchical task solver plug-in for dynamic-graph.
madgwickahrs.hh
Go to the documentation of this file.
1 //========================================================================
2 //
3 // Implementation of Madgwick's IMU and AHRS algorithms.
4 // See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
5 //
6 // Date Author Notes
7 // 29/09/2011 SOH Madgwick Initial release
8 // 02/10/2011 SOH Madgwick Optimised for reduced CPU load
9 // 11/05/2017 T Flayols Make it a dynamic graph entity
10 // 26/03/2019 G Buondonno Converted to double
11 //
12 //========================================================================
13 
14 /*
15  * Copyright 2017, Thomas Flayols, LAAS-CNRS
16  *
17  * This file is part of sot-torque-control.
18  * sot-torque-control is free software: you can redistribute it and/or
19  * modify it under the terms of the GNU Lesser General Public License
20  * as published by the Free Software Foundation, either version 3 of
21  * the License, or (at your option) any later version.
22  * sot-torque-control is distributed in the hope that it will be
23  * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
24  * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
25  * GNU Lesser General Public License for more details. You should
26  * have received a copy of the GNU Lesser General Public License along
27  * with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
28  */
29 
30 #ifndef __sot_torque_control_madgwickahrs_H__
31 #define __sot_torque_control_madgwickahrs_H__
32 
33 /* --------------------------------------------------------------------- */
34 /* --- API ------------------------------------------------------------- */
35 /* --------------------------------------------------------------------- */
36 
37 #if defined(WIN32)
38 #if defined(madgwickahrs_EXPORTS)
39 #define SOTMADGWICKAHRS_EXPORT __declspec(dllexport)
40 #else
41 #define SOTMADGWICKAHRS_EXPORT __declspec(dllimport)
42 #endif
43 #else
44 #define SOTMADGWICKAHRS_EXPORT
45 #endif
46 
47 /* --------------------------------------------------------------------- */
48 /* --- INCLUDE --------------------------------------------------------- */
49 /* --------------------------------------------------------------------- */
50 
51 #include "boost/assign.hpp"
52 #include <dynamic-graph/signal-helper.h>
53 #include <map>
55 
56 #define betaDef 0.01 // 2 * proportional g
57 
58 namespace dynamicgraph {
59 namespace sot {
83 class SOTMADGWICKAHRS_EXPORT MadgwickAHRS : public ::dynamicgraph::Entity {
85  DYNAMIC_GRAPH_ENTITY_DECL();
86 
87 public:
88  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
89 
90  /* --- CONSTRUCTOR ---- */
91  MadgwickAHRS(const std::string &name);
92 
93  void init(const double &dt);
94  void set_beta(const double &beta);
95 
97  void set_imu_quat(const dynamicgraph::Vector &imu_quat);
98 
99  /* --- SIGNALS --- */
101  DECLARE_SIGNAL_IN(accelerometer, dynamicgraph::Vector);
103  DECLARE_SIGNAL_IN(gyroscope, dynamicgraph::Vector);
105  DECLARE_SIGNAL_OUT(imu_quat, dynamicgraph::Vector);
106 
107 protected:
108  /* --- COMMANDS --- */
109  /* --- ENTITY INHERITANCE --- */
110  virtual void display(std::ostream &os) const;
111 
112  /* --- METHODS --- */
113  double invSqrt(double x);
114  void madgwickAHRSupdateIMU(double gx, double gy, double gz, double ax,
115  double ay, double az);
116 
117 protected:
121  double m_beta;
123  double m_q0, m_q1, m_q2, m_q3;
125  double m_sampleFreq;
126 
127 }; // class MadgwickAHRS
128 } // namespace sot
129 } // namespace dynamicgraph
130 
131 #endif // #ifndef __sot_torque_control_madgwickahrs_H__
dynamicgraph::sot::MadgwickAHRS::m_initSucceeded
bool m_initSucceeded
true if the entity has been successfully initialized
Definition: madgwickahrs.hh:119
dynamicgraph
Definition: abstract-sot-external-interface.hh:17
SOTMADGWICKAHRS_EXPORT
#define SOTMADGWICKAHRS_EXPORT
Definition: madgwickahrs.hh:44
dynamicgraph::sot::MadgwickAHRS::m_q3
double m_q3
Definition: madgwickahrs.hh:123
dynamicgraph::sot::MadgwickAHRS::m_sampleFreq
double m_sampleFreq
sample frequency in Hz
Definition: madgwickahrs.hh:125
dynamicgraph::sot::MadgwickAHRS
Definition: madgwickahrs.hh:83
matrix-geometry.hh
dynamicgraph::sot::MadgwickAHRS::m_beta
double m_beta
2 * proportional gain (Kp)
Definition: madgwickahrs.hh:121