30 #ifndef __sot_torque_control_madgwickahrs_H__
31 #define __sot_torque_control_madgwickahrs_H__
38 #if defined(madgwickahrs_EXPORTS)
39 #define SOTMADGWICKAHRS_EXPORT __declspec(dllexport)
41 #define SOTMADGWICKAHRS_EXPORT __declspec(dllimport)
44 #define SOTMADGWICKAHRS_EXPORT
51 #include "boost/assign.hpp"
52 #include <dynamic-graph/signal-helper.h>
56 #define betaDef 0.01 // 2 * proportional g
85 DYNAMIC_GRAPH_ENTITY_DECL();
88 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
93 void init(
const double &dt);
94 void set_beta(
const double &beta);
97 void set_imu_quat(
const dynamicgraph::Vector &imu_quat);
101 DECLARE_SIGNAL_IN(accelerometer, dynamicgraph::Vector);
103 DECLARE_SIGNAL_IN(gyroscope, dynamicgraph::Vector);
105 DECLARE_SIGNAL_OUT(imu_quat, dynamicgraph::Vector);
110 virtual void display(std::ostream &os)
const;
113 double invSqrt(
double x);
114 void madgwickAHRSupdateIMU(
double gx,
double gy,
double gz,
double ax,
115 double ay,
double az);
131 #endif // #ifndef __sot_torque_control_madgwickahrs_H__