private void IntegratePosition(float delta_t)
{
Vector earth_velocity;
velocity.IncrementX(delta_t * acceleration.X);
velocity.IncrementY(delta_t * acceleration.Y);
velocity.IncrementZ(delta_t * acceleration.Z);
if (velocity.X > 48.0)
{
velocity.X = 48.0;
acceleration.X = -1.0;
}
if (velocity.X < 0.0)
{
velocity.X = 0.0;
acceleration.X = 0.5;
}
attitude = attitude + (attitude_rate * delta_t);
attitude.Limits();
earth_velocity = new Vector(velocity);
attitude.RotateAtoE(earth_velocity);
position.IncrementX(delta_t * earth_velocity.X);
position.IncrementY(delta_t * earth_velocity.Y);
position.IncrementZ(delta_t * earth_velocity.Z);
// Debug.WriteLine("accelZ = " + acceleration.Z + " velz = " + velocity.Z + " posz = " + position.Z);
mph = (float)velocity.X * 3600.0f / 5280.0f;
}
