An Integrated, Modular Simulation System for Education ... - Cal Poly

* current aircraft rotations, and checks for special case conditions* such as pitch exceeding +/-90 degrees */void UpdateState( ){float xDot, yDot, zDot, PsiDot, ThetaDot, PhiDot;static float OlduDot, OldvDot, OldwDot, OldpDot,OldqDot, OldrDot, OldPhiDot, OldThetaDot, OldPsiDot,OldxDot, OldyDot, OldzDot;/***** Initialize the previous velocities for the integrator *****/OldxDot=0.0;OldyDot=0.0;OldzDot=0.0;OlduDot=0.0;OldvDot=0.0;OldwDot=0.0;OldpDot=0.0;OldqDot=0.0;OldrDot=0.0;Fx=0.0; /***** Initialize the force and moment variables. *****/Fy=0.0;Fz=0.0;PitchingMoment=0.0;RollingMoment=0.0;YawingMoment=0.0;u+=(3.0*uDot-OlduDot)/2.0*dt;v+=(3.0*vDot-OldvDot)/2.0*dt;w+=(3.0*wDot-OldwDot)/2.0*dt;p+=(3.0*pDot-OldpDot)/2.0*dt;q+=(3.0*qDot-OldqDot)/2.0*dt;r+=(3.0*rDot-OldrDot)/2.0*dt;/***** Precalculate trig functions *****/STheta=sin(Theta);SPhi=sin(Phi);SPsi=sin(Psi);CTheta=cos(Theta);CPhi=cos(Phi);CPsi=cos(Psi);TTheta=STheta/CTheta;SecTheta=1.0;if (CTheta!=0.0)SecTheta = 1.0/CTheta;PhiDot=p+(q*SPhi+r*CPhi)*TTheta; /***** This is the Euler angle transformation *****/ThetaDot=q*CPhi-r*SPhi; /***** from body to world coordinates *****/PsiDot=(q*SPhi+r*CPhi)*SecTheta;/***** This transforms velocity components to world coordinates *****/149