Re: Flight dynamic model

Dear Davy,

Thank you very much for sharing your experience and code with me/us.
I'm sure I will learn a lot from your experience.

THANK YOU

Lora Lee


> Hi Lora,
> Inspired by FMS I started writing a R/C simulator last year. Never got
> around to finishing it though. But I figured out a working flightmodel.
> Mostly by directly implementing some basic physics. Of course I haven't
> documented my code well :-) but maybe it's a start for you. If I remember
> correctly it worked pretty well, but some tweaking is needed when pitch
> approaches -90 degrees. Sorry about the long post.
>
> Good luck!!
> Davy
>
> Some declarations :
>
> struct AirplaneParameters
> {
> Float wingspan;
> Float wingsurface;
> Float Cl0; // lift at alpha=0
> Float Cla; // lift coefficient (=lift curve slope)
> Float Cd0; // drag at alpha=0
> Float Cda; // drag coefficient (=drag curve slope)
> Float Clb; // lift coefficient of fuselage (f(beta))
> Float Cdb; // drag coefficient of fuselage (f(beta))
> Float Cdx; // drag coefficient x-axis
> Float Cdy; // drag coefficient y-axis
> Float Cdz; // drag coefficient z-axis
> Float maxThrust; // maximum thrust
> Float thrustAngle; // angle of thrust vector and fuselage.
> Float weight; // weight
> Float aileron; // aileron effect
> Float rudder; // rudder effect
> Float elevator; // elevator effect
> Float yawDamp; // yaw damping
> Float pitchDamp; // pitch damping
> Float rollDamp; // roll damping
> };
>
> class FlightModel
> {
> /* .... */
>
> private:
> AirplaneParameters m_par;
> // Position
> Float m_dX;
> Float m_dY;
> Float m_dZ;
> // Speed
> Float m_U;
> Float m_V;
> Float m_W;
> // Acceleration
> Float m_aU;
> Float m_aV;
> Float m_aW;
> // Attitude
> Float m_phi; // roll
> Float m_theta; // pitch
> Float m_psi; // yaw
> // Worldattitude
> Float m_wphi; // roll
> Float m_wtheta; // pitch
> Float m_wpsi; // yaw
> // Angular velocity
> Float m_P;
> Float m_Q;
> Float m_R;
> // Angular acceleration
> Float m_aP;
> Float m_aQ;
> Float m_aR;
> // Moments
> Float m_L;
> Float m_M;
> Float m_N;
> // Forces
> Float m_Fx;
> Float m_Fy;
> Float m_Fz;
> // Cumulated movements
> Float m_dXcumul;
> Float m_dYcumul;
> Float m_dZcumul;
> DirectX::Vector3 m_velocity;
>
> private:
> Float m_speedsq;
> Float m_alpha;
> Float m_beta;
> Float m_thrust;
> Int m_throttle; /* 0..100 */
> Int m_yokeHor; /*-100...+100*/
> Int m_yokeVer; /*-100...+100*/
> Int m_pedals; /*-100...+100*/
> };
>
> Implementation of the main function (needs to be run about every 5 ms).
> Some
> debug code is still present in there
>
> const float _rho = 10.0f;
> const float _grav = 29.0f;
>
> DWORD WINAPI runFlightModel(void* flightModel)
> {
> FlightModel* fm = reinterpret_cast<FlightModel*>(flightModel);
> DirectX::Timer timer;
> timer.microElapsed( );
> CRITICAL_SECTION cs;
> ::InitializeCriticalSection(&cs);
>
> while (fm->running( ))
> {
> ::EnterCriticalSection(&cs );
> Huge helapsed = timer.microElapsed( );
> Float elapsed = helapsed / 1000000.0f;
> if (elapsed > 0.5f)
> elapsed = 0.0f;
> fm->updateInput( );
> // initialize some basic numbers
> fm->m_speedsq = fm->m_U*fm->m_U + fm->m_V*fm->m_V +
> fm->m_W*fm->m_W;
> Float speed = (Float)(sqrt(fm->m_speedsq));
> if (absval<Float>(fm->m_U) < 0.01f)
> {
> fm->m_alpha = 0; //DirectX::Pi / 2;
> fm->m_beta = 0; //DirectX::Pi / 2;
> }
> else
> {
> fm->m_alpha = atanf(fm->m_W/fm->m_U);
> fm->m_beta = atanf(fm->m_V/fm->m_U);
> }
>
> fm->m_thrust = fm->m_par.maxThrust*0.1f*(fm->m_throttle);
>
>
>
> // calculate lift and drag
> Float Dragx = fm->m_par.Cdx*fm->m_U*fm->m_U*sign<Float>(fm->m_U);
> Float Dragy = fm->m_par.Cdy*fm->m_V*fm->m_V*sign<Float>(fm->m_V);
> Float Dragz = fm->m_par.Cdz*fm->m_W*fm->m_W*sign<Float>(fm->m_W);
> Float Lift = (fm->m_par.Cl0 +
> fm->m_par.Cla*fm->m_alpha)*fm->m_U*fm->m_U;
>
>
> if (absval<Float>(fm->m_alpha) > 0.2f)
> Lift = Lift - Lift*(sinf(fm->m_alpha - 0.2f));
> if (fm->m_U < 0.0f)
> Lift = fm->m_par.Cl0;
>
>
> Float FLift = (fm->m_par.Clb*fm->m_beta)*fm->m_U*fm->m_U;
>
> fm->m_Fx = -_grav *sinf(fm->m_theta)*fm->m_par.weight -
> Dragx + fm->m_thrust*cosf(fm->m_par.thrustAngle);
> fm->m_Fy =
> _grav*sinf(fm->m_phi)*cosf(fm->m_theta)*fm->m_par.weight - Dragy - FLift;
> fm->m_Fz =
> _grav*cosf(fm->m_phi)*cosf(fm->m_theta)*fm->m_par.weight - Dragz - Lift -
> fm->m_thrust*sinf(fm->m_par.thrustAngle);
>
> fm->m_aU = (fm->m_V*fm->m_R - fm->m_W*fm->m_Q)/1.0f +
> fm->m_Fx/(fm->m_par.weight);
> fm->m_aV = (fm->m_W*fm->m_P - fm->m_U*fm->m_R)/1.0f +
> fm->m_Fy/(fm->m_par.weight);
> fm->m_aW = (fm->m_U*fm->m_Q - fm->m_V*fm->m_P)/1.0f +
> fm->m_Fz/(fm->m_par.weight);
>
> if (absval<Float>(fm->m_aU) > 10000) { fm->m_aU = 0.0f; fm->m_U =
> 0.0f; }
> if (absval<Float>(fm->m_aV) > 10000) { fm->m_aV = 0.0f; fm->m_V =
> 0.0f; }
> if (absval<Float>(fm->m_aW) > 10000) { fm->m_aW = 0.0f; fm->m_W =
> 0.0f; }
>
> // calculate new speeds
> fm->m_U += elapsed*fm->m_aU;
> fm->m_V += elapsed*fm->m_aV;
> fm->m_W += elapsed*fm->m_aW;
>
> // calculate new position
> fm->m_dX = elapsed*fm->m_U;
> fm->m_dY = elapsed*fm->m_V;
> fm->m_dZ = elapsed*fm->m_W;
>
>
> Float ailDefl = fm->m_yokeHor / 200.0f;
> Float elDefl = fm->m_yokeVer / 300.0f;
> Float rdDefl = fm->m_pedals / 300.0f;
> fm->m_L = (fm->m_par.rollDamp*fm->m_P/(fm->m_U + 0.1f) +
> fm->m_par.aileron*ailDefl)*fm->m_U*fm->m_U;
> fm->m_M = (fm->m_par.pitchDamp*fm->m_Q/(fm->m_U + 0.1f) +
> fm->m_par.elevator*elDefl)*fm->m_U*fm->m_U -
> 0.2f*fm->m_alpha*sign<Float>(fm->m_U)*fm->m_speedsq;
> fm->m_N = (fm->m_par.yawDamp*fm->m_R/(fm->m_U + 0.1f) +
> 0.2f*fm->m_beta*sign<Float>(fm->m_U) +
> fm->m_par.rudder*rdDefl)*fm->m_U*fm->m_U;
>
> fm->m_aP = fm->m_L*elapsed;
> fm->m_aQ = fm->m_M*elapsed;
> fm->m_aR = fm->m_N*elapsed;
>
> if (absval<Float>(fm->m_aP) > 1000) { fm->m_aP = 0.0f; fm->m_P =
> 0.0f; }
> if (absval<Float>(fm->m_aQ) > 1000) { fm->m_aQ = 0.0f; fm->m_Q =
> 0.0f; }
> if (absval<Float>(fm->m_aR) > 1000) { fm->m_aR = 0.0f; fm->m_R =
> 0.0f; }
>
> fm->m_P += elapsed*fm->m_aP;
> fm->m_Q += elapsed*fm->m_aQ;// -
> sign<Float>(fm->m_U)*fm->m_alpha*(0.0003f/(elapsed*elapsed));
> fm->m_R += elapsed*fm->m_aR;// -
> fm->m_beta*(0.0003f/(elapsed*elapsed));
>
> Float phi = fm->m_phi;
> Float theta = fm->m_theta;
> Float psi = fm->m_psi;
>
> if (absval<Float>(cosf(theta)) < 0.00001f)
> if (cosf(theta) < 0.0f)
> {
> theta = acosf(-0.00001f);
> fm->m_theta = theta;
> }
> else
> {
> theta = acosf(0.00001f);
> fm->m_theta = theta;
> }
>
> fm->m_phi += (fm->m_P + (fm->m_Q*sinf(phi) +
> fm->m_R*cosf(phi))*tanf(theta))*elapsed;
> fm->m_theta += (fm->m_Q*cosf(phi) - fm->m_R*sinf(phi))*elapsed;
> fm->m_psi += ((fm->m_Q*sinf(phi) +
> fm->m_R*cosf(phi))/cosf(theta))*elapsed;
>
> fm->m_phi = modulo<Float>(fm->m_phi, 2*DirectX::Pi);
> fm->m_theta = modulo<Float>(fm->m_theta,2*DirectX::Pi);
> fm->m_psi = modulo<Float>(fm->m_psi, 2*DirectX::Pi);
>
> // update cumulative movements
> Float X = 0.01f*fm->m_dX, Y = 0.01f*fm->m_dY, Z = 0.01f*fm->m_dZ;
> Float dX = cosf(fm->m_theta)*sinf(fm->m_psi)*X +
> (sinf(fm->m_phi)*sinf(fm->m_theta)*sinf(fm->m_psi) -
> cosf(fm->m_phi)*cosf(fm->m_psi))*Y +
> (cosf(fm->m_phi)*sinf(fm->m_theta)*sinf(fm->m_psi) -
> sinf(fm->m_phi)*cosf(fm->m_psi))*Z;
> Float dY = -sinf(fm->m_theta)*X +
> sinf(fm->m_phi)*cosf(fm->m_theta)*Y + cosf(fm->m_phi)*cosf(fm->m_theta)*Z;
> Float dZ = cosf(fm->m_theta)*cosf(fm->m_psi)*X +
> (sinf(fm->m_phi)*sinf(fm->m_theta)*cosf(fm->m_psi) -
> cosf(fm->m_phi)*sinf(fm->m_psi))*Y +
> (cosf(fm->m_phi)*sinf(fm->m_theta)*cosf(fm->m_psi) +
> sinf(fm->m_phi)*sinf(fm->m_psi))*Z;
>
> fm->m_dXcumul -= dX;
> fm->m_dYcumul -= dY;
> fm->m_dZcumul -= dZ;
>
> fm->m_velocity =
> DirectX::Vector3(-dX/elapsed, -dY/elapsed, -dZ/elapsed);
>
> ::LeaveCriticalSection(&cs);
> ::Sleep(5);
>
> }
> ::DeleteCriticalSection(&cs);
> return S_OK;
> }
>
>
> Some example parameters for an aircraft :
>
> AirplaneParameters _parsExtra300 =
> {
> 1.4f,
> 0.4f,
> 0.0003f, // lift at alpha=0
> 0.0180f, // lift coefficient (=lift curve slope)
> 0.02f, // drag at alpha=0
> 0.04f, // drag coefficient (=drag curve slope)
> 0.0180f, // lift coefficient of fuselage (f(beta))
> 0.10f, // drag coefficient of fuselage (f(beta))
> 0.002f, // drag coefficient x-axis
> 0.002f, // drag coefficient y-axis
> 0.050f, // drag coefficient z-axis
> 10.0f, // maximum thrust
> 0.0175f, // thrustangle
> 2.2f, // weight
> 0.150f, // aileron effect
> 0.150f, // rudder effect
> 0.650f, // elevator effect
> -4.0f, // yaw damping
> -4.0f, // pitch damping
> -2.0f // roll damping
> };
>

.