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__ bool m_initSucceeded
true if the entity has been successfully initialized
Definition: madgwickahrs.hh:119
#define SOTMADGWICKAHRS_EXPORT
Definition: madgwickahrs.hh:44
double m_q3
Definition: madgwickahrs.hh:123
double m_sampleFreq
sample frequency in Hz
Definition: madgwickahrs.hh:125
Definition: madgwickahrs.hh:83
double m_beta
2 * proportional gain (Kp)
Definition: madgwickahrs.hh:121
Definition: abstract-sot-external-interface.hh:17