예제 #1
0
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;

}
예제 #2
0
/**
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;
}