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 <dynamic-graph/signal-helper.h> 56 #include "boost/assign.hpp" 58 #define betaDef 0.01 // 2 * proportional g 87 DYNAMIC_GRAPH_ENTITY_DECL();
90 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
95 void init(
const double &dt);
96 void set_beta(
const double &beta);
99 void set_imu_quat(
const dynamicgraph::Vector &imu_quat);
103 DECLARE_SIGNAL_IN(accelerometer, dynamicgraph::Vector);
105 DECLARE_SIGNAL_IN(gyroscope, dynamicgraph::Vector);
107 DECLARE_SIGNAL_OUT(imu_quat, dynamicgraph::Vector);
112 virtual void display(std::ostream &os)
const;
115 double invSqrt(
double x);
116 void madgwickAHRSupdateIMU(
double gx,
double gy,
double gz,
double ax,
117 double ay,
double az);
133 #endif // #ifndef __sot_torque_control_madgwickahrs_H__ bool m_initSucceeded
true if the entity has been successfully initialized
Definition: madgwickahrs.hh:121
#define SOTMADGWICKAHRS_EXPORT
Definition: madgwickahrs.hh:44
double m_q3
Definition: madgwickahrs.hh:125
double m_sampleFreq
sample frequency in Hz
Definition: madgwickahrs.hh:127
Definition: madgwickahrs.hh:85
double m_beta
2 * proportional gain (Kp)
Definition: madgwickahrs.hh:123
Definition: abstract-sot-external-interface.hh:17