6 #include <dynamic-graph/factory.h>
8 #include <sot/core/debug.hh>
11 #include <sot/core/stop-watch.hh>
16 namespace dg = ::dynamicgraph;
18 using namespace dg::command;
21 #define CALIBRATION_FILE_NAME "/opt/imu_calib.txt"
23 #define PROFILE_IMU_OFFSET_COMPENSATION_COMPUTATION "ImuOffsetCompensation computation"
25 #define INPUT_SIGNALS m_accelerometer_inSIN << m_gyrometer_inSIN
26 #define OUTPUT_SIGNALS m_accelerometer_outSOUT << m_gyrometer_outSOUT
40 CONSTRUCT_SIGNAL_IN(accelerometer_in,
dynamicgraph::Vector),
42 CONSTRUCT_SIGNAL_OUT(accelerometer_out,
dynamicgraph::Vector, m_accelerometer_inSIN),
43 CONSTRUCT_SIGNAL_OUT(gyrometer_out,
dynamicgraph::Vector, m_gyrometer_inSIN),
44 m_initSucceeded(false),
46 m_update_cycles_left(0),
48 m_a_gyro_DC_blocker(1.0f)
60 docCommandVoid1(
"Initialize the entity.",
"Timestep in seconds (double)")));
62 docCommandVoid1(
"Update the IMU offsets.",
63 "Duration of the update phase in seconds (double)")));
64 addCommand(
"setGyroDCBlockerParameter",
66 docCommandVoid1(
"Set DC Blocker filter parameter.",
"alpha (double)")));
74 if (dt <= 0.0)
return SEND_MSG(
"Timestep must be positive", MSG_TYPE_ERROR);
75 m_dt =
static_cast<float>(dt);
81 if (!infile.is_open())
82 return SEND_MSG(
"Error trying to read calibration results from file " + toString(
CALIBRATION_FILE_NAME),
95 ". Not enough values: " + toString(i),
100 while (infile >> z) {
109 ". Not enough values: " + toString(i),
113 SEND_MSG(
"Offset read finished:\n* acc offset: " + toString(
m_acc_offset.transpose()) +
119 if (alpha > 1.0 || alpha <= 0.0)
return SEND_MSG(
"GyroDCBlockerParameter must be > 0 and <= 1", MSG_TYPE_ERROR);
124 if (duration <
m_dt)
return SEND_MSG(
"Duration must be greater than the time step", MSG_TYPE_ERROR);
130 const dynamicgraph::Vector& accelerometer = m_accelerometer_inSIN(iter);
131 const dynamicgraph::Vector& gyrometer = m_gyrometer_inSIN(iter);
137 Vector3 g, new_acc_offset, new_gyro_offset;
143 SEND_MSG(
"Offset computation finished:" + (
"\n* old acc offset: " + toString(
m_acc_offset.transpose())) +
144 "\n* new acc offset: " + toString(new_acc_offset.transpose()) +
"\n* old gyro offset: " +
145 toString(
m_gyro_offset.transpose()) +
"\n* new gyro offset: " + toString(new_gyro_offset.transpose()),
153 return SEND_MSG(
"Error trying to save calibration results on file " + toString(
CALIBRATION_FILE_NAME),
156 for (
unsigned long int i = 0; i < 3; i++) aof <<
m_gyro_offset[i] <<
" ";
158 for (
unsigned long int i = 0; i < 3; i++) aof <<
m_acc_offset[i] <<
" ";
170 if (!m_initSucceeded) {
171 SEND_WARNING_STREAM_MSG(
"Cannot compute signal accelerometer before initialization!");
175 if (m_update_cycles_left > 0) update_offset_impl(iter);
177 const dynamicgraph::Vector& accelerometer = m_accelerometer_inSIN(iter);
178 if (s.size() != 3) s.resize(3);
179 s = accelerometer - m_acc_offset;
184 if (!m_initSucceeded) {
185 SEND_WARNING_STREAM_MSG(
"Cannot compute signal gyrometer before initialization!");
188 const dynamicgraph::Vector& gyrometer = m_gyrometer_inSIN(iter);
189 if (s.size() != 3) s.resize(3);
191 if (m_a_gyro_DC_blocker != 1.0)
192 m_gyro_offset = m_gyro_offset * m_a_gyro_DC_blocker + (1. - m_a_gyro_DC_blocker) * gyrometer;
193 s = gyrometer - m_gyro_offset;
202 os <<
"ImuOffsetCompensation " << getName();
204 getProfiler().report_all(3, os);
205 }
catch (ExceptionSignal e) {