bool StepDetector::pushAndDetect(IMUData &accelData) {
TIMESTAMP_T t = accelData.time();
IMUDATA_T magnitude = sqrt(accelData.x() * accelData.x() + accelData.y() * accelData.y() + accelData.z() * accelData.z());
if (!Adir_ && magnitude > Aref_)
{
Aref_ = magnitude;
prev_time_ = t;
}
else if (!Adir_ && Aref_ - magnitude >= parameter_ptr->step_delta_mag_thresh())
{
Adir_ = !Adir_;
Aref_ = magnitude;
}
else if(Adir_ && magnitude < Aref_)
{
Aref_ = magnitude;
}
else if(Adir_ && magnitude - Aref_ >= parameter_ptr->step_delta_mag_thresh())
{
if (t - prev_time_ >= parameter_ptr->step_delta_t_thresh()) {
Adir_ = !Adir_;
Aref_ = magnitude;
prev_time_ = t;
return true;
}
}
return false;
}
/**
Updates robot position information using IMU data
**/
int RobotPosition::update(IMUData newData)
{
double time = newData.time();
_Roll.push(DataPoint<double>(newData.Roll, time));
_Pitch.push(DataPoint<double>(newData.Pitch, time));
_Yaw.push(DataPoint<double>(newData.Yaw, time));
return 0;
}