SensorFusion::SensorFusion(SensorDevice* sensor)
: Stage(0), RunningTime(0), DeltaT(0.001f),
Handler(getThis()), pDelegate(0),
Gain(0.05f), EnableGravity(true),
EnablePrediction(true), PredictionDT(0.03f), PredictionTimeIncrement(0.001f),
FRawMag(10), FAngV(20),
GyroOffset(), TiltAngleFilter(1000),
EnableYawCorrection(false), MagCalibrated(false), MagNumReferences(0), MagRefIdx(-1), MagRefScore(0),
MotionTrackingEnabled(true)
{
if (sensor)
AttachToSensor(sensor);
MagCalibrationMatrix.SetIdentity();
}
SensorFusion::SensorFusion(SensorDevice* sensor)
: Handler(getThis()), pDelegate(0),
Gain(0.05f), YawMult(1), EnableGravity(true), Stage(0), DeltaT(0.001f),
EnablePrediction(false), PredictionDT(0.03f),
FRawMag(10), FAccW(20), FAngV(20),
TiltCondCount(0), TiltErrorAngle(0),
TiltErrorAxis(0,1,0),
MagCondCount(0), MagReady(false), MagCalibrated(false), MagReferenced(false),
MagRefQ(0, 0, 0, 1), MagRefM(0), MagRefYaw(0), YawErrorAngle(0), MagRefDistance(0.15f),
YawErrorCount(0), YawCorrectionActivated(false), YawCorrectionInProgress(false),
EnableYawCorrection(false)
{
if (sensor)
AttachToSensor(sensor);
MagCalibrationMatrix.SetIdentity();
}
