dynobud-1.1.0.0: examples/Glider/AeroCoeffs.hs
{-# OPTIONS_GHC -Wall #-}
{-# Language DeriveFunctor #-}
{-# Language DeriveFoldable #-}
{-# Language DeriveGeneric #-}
module Glider.AeroCoeffs where
import GHC.Generics ( Generic, Generic1 )
import Data.Foldable ( Foldable )
import Linear
import Dyno.Server.Accessors ( Lookup(..) )
import Dyno.Vectorize
atan2' :: Floating a => a -> a -> a
atan2' y x = 2 * atan (y / (sqrt(x*x + y*y + 1e-15) + x) )
data ControlSurfaces a =
ControlSurfaces { csElev :: a
, csRudder :: a
, csAil :: a
, csFlaps :: a
} deriving (Eq, Functor, Foldable, Generic, Generic1, Show)
instance Vectorize ControlSurfaces
instance (Lookup a, Generic a) => Lookup (ControlSurfaces a)
data AeroForceCoeffs a =
AeroForceCoeffs { af_cL0 :: a
, af_cL_A :: a
, af_cL_elev :: a
, af_cL_flaps :: a
, af_cD0 :: a
, af_cD_A :: a
, af_cD_A2 :: a
, af_cD_B2 :: a
, af_cD_elev :: a
, af_cD_elev2 :: a
, af_cD_A_elev :: a
, af_cD_flaps :: a
, af_cD_flaps2 :: a
, af_cD_A_flaps :: a
, af_cD_rudder :: a
, af_cD_rudder2 :: a
, af_cD_B_rudder :: a
, af_cD_ail :: a
, af_cD_ail2 :: a
, af_cD_B_ail :: a
, af_cY_B :: a
, af_cY_rudder :: a
} deriving (Functor, Generic, Generic1, Show)
instance Vectorize AeroForceCoeffs
data AeroMomentCoeffs a =
AeroMomentCoeffs { am_cm0 :: a
, am_cl_p :: a
, am_cl_q :: a
, am_cl_r :: a
, am_cm_p :: a
, am_cm_q :: a
, am_cm_r :: a
, am_cn_p :: a
, am_cn_q :: a
, am_cn_r :: a
, am_cl_B :: a
, am_cl_AB :: a
, am_cm_A :: a
, am_cn_B :: a
, am_cn_AB :: a
, am_cl_ail :: a
, am_cm_elev
, am_cm_flaps :: a
, am_cn_rudder :: a
} deriving (Functor, Generic, Generic1, Show)
instance Vectorize AeroMomentCoeffs
data AeroRefs a =
AeroRefs { ar_sref :: a
, ar_bref :: a
, ar_cref :: a
} deriving (Functor, Generic, Generic1, Show)
instance Vectorize AeroRefs
-- | Compute aerodynamic forces/moments in the body frame.
-- Parameters:
-- dcm_n2b: rotation matrix rotating vectors expressed in NED to vectors expressed in body
-- v_bw_b: body velocity in the wind frame, expressed in the body frame
-- w_bn_b: body angular velocity w.r.t. NED
aeroForcesMoments :: Floating a => AeroForceCoeffs a -> AeroMomentCoeffs a -> AeroRefs a ->
V3 a -> V3 a -> ControlSurfaces a -> (V3 a, V3 a)
aeroForcesMoments forceCoeffs momentCoeffs refs v_bw_b w_bn_b controlSurfaces = (forces, moments)
where
V3 cL cD cY = aeroForceCoeffs alpha beta controlSurfaces forceCoeffs
c_lmn = aeroMomentCoeffs alpha beta airspeed w_bn_b controlSurfaces momentCoeffs refs
-- alpha/beta
alpha = atan2' v_bw_b_z v_bw_b_x
beta = asin (v_bw_b_y / airspeed)
V3 v_bw_b_x v_bw_b_y v_bw_b_z = v_bw_b
airspeedSquared = quadrance v_bw_b
airspeed = sqrt airspeedSquared
moments = rho_sref_v2*^(V3 bref cref bref)*c_lmn
forces = dragForce + liftForce + sideForce
dragForce = (-rho_sref_v*cD) *^ v_bw_b
liftForce = rho_sref_v*cL *^ e_b2L_b_v
sideForce = rho_sref*cY *^ e_b2Y_b_v2
-- y axis of aircraft expressed in body frame
e_b2y_b = V3 0 1 0
-- lift axis normalized to airspeed
e_b2L_b_v = cross e_b2y_b v_bw_b
-- sideforces axis normalized to airspeed^2
e_b2Y_b_v2 = cross e_b2L_b_v (-v_bw_b)
rho_sref = 0.5*rho*sref
rho_sref_v2 = rho_sref*airspeedSquared
rho_sref_v = rho_sref*airspeed
sref = ar_sref refs
bref = ar_bref refs
cref = ar_cref refs
rho = 1.23
aeroForceCoeffs :: Num a => a -> a -> ControlSurfaces a -> AeroForceCoeffs a -> V3 a
aeroForceCoeffs alpha beta controlSurfaces coeffs = V3 cL cD cY
where
cL_wing = cL_A'*alpha + cL0'
cD_wing = cD_A'*alpha + cD_A2'*alpha*alpha + cD_B2'*beta*beta + cD0'
cY_wing = cY_B'*beta
cL_elev = cL_elev' * elev
cD_elev = cD_elev2' * elev * elev + cD_A_elev' * elev * alpha + cD_elev' * elev
cD_ail = cD_ail2'*ail*ail + cD_B_ail'*beta*ail + cD_ail'*ail
cL_flaps = cL_flaps'*flaps
cD_flaps = cD_flaps2'*flaps*flaps + cD_A_flaps'*alpha*flaps + cD_flaps'*flaps
cY_rudder = cY_rudder'*rudder
cD_rudder = cD_rudder2'*rudder*rudder + cD_B_rudder'*beta*rudder + cD_rudder'*rudder
cL = cL_wing + cL_elev + cL_flaps
cD = cD_wing + cD_elev + cD_ail + cD_flaps + cD_rudder
cY = cY_wing + cY_rudder
-- inputs
elev = csElev controlSurfaces
rudder = csRudder controlSurfaces
ail = csAil controlSurfaces
flaps = csFlaps controlSurfaces
-- unpack aero coeffs
cL_A' = af_cL_A coeffs
cL0' = af_cL0 coeffs
cD_A' = af_cD_A coeffs
cD_A2' = af_cD_A2 coeffs
cD_B2' = af_cD_B2 coeffs
cD0' = af_cD0 coeffs
cY_rudder' = af_cY_rudder coeffs
cD_rudder2' = af_cD_rudder2 coeffs
cD_flaps2' = af_cD_flaps2 coeffs
cD_elev2' = af_cD_elev2 coeffs
cD_flaps' = af_cD_flaps coeffs
cD_A_flaps' = af_cD_A_flaps coeffs
cD_A_elev' = af_cD_A_elev coeffs
cD_elev' = af_cD_elev coeffs
cD_ail2' = af_cD_ail2 coeffs
cD_ail' = af_cD_ail coeffs
cD_B_ail' = af_cD_B_ail coeffs
cD_B_rudder' = af_cD_B_rudder coeffs
cD_rudder' = af_cD_rudder coeffs
cL_elev' = af_cL_elev coeffs
cL_flaps' = af_cL_flaps coeffs
cY_B' = af_cY_B coeffs
aeroMomentCoeffs :: Fractional a => a -> a -> a -> V3 a -> ControlSurfaces a -> AeroMomentCoeffs a -> AeroRefs a -> V3 a
aeroMomentCoeffs alpha beta airspeed w_bn_b controlSurfaces coeffs refs =
momentCoeffs0 + momentCoeffs_pqr + momentCoeffs_AB + momentCoeffs_surf
where
elev = csElev controlSurfaces
rudder = csRudder controlSurfaces
ail = csAil controlSurfaces
flaps = csFlaps controlSurfaces
w_bn_b_hat = (V3 bref cref bref) * w_bn_b ^* (0.5/airspeed)
momentCoeffs0 = V3 cm0 0 0
momentCoeffs_pqr =
(V3
(V3 cl_p cl_q cl_r)
(V3 cm_p cm_q cm_r)
(V3 cn_p cn_q cn_r)) !* w_bn_b_hat
momentCoeffs_AB =
(V3
(V3 0 cl_B cl_AB)
(V3 cm_A 0 0)
(V3 0 cn_B cn_AB)) !* (V3 alpha beta (alpha*beta))
momentCoeffs_surf =
V3
(cl_ail * ail)
(cm_elev * elev + cm_flaps * flaps)
(cn_rudder * rudder)
-- unpack aero coeffs
cm0 = am_cm0 coeffs
cl_p = am_cl_p coeffs
cl_q = am_cl_q coeffs
cl_r = am_cl_r coeffs
cm_p = am_cm_p coeffs
cm_q = am_cm_q coeffs
cm_r = am_cm_r coeffs
cn_p = am_cn_p coeffs
cn_q = am_cn_q coeffs
cn_r = am_cn_r coeffs
cl_B = am_cl_B coeffs
cl_AB = am_cl_AB coeffs
cm_A = am_cm_A coeffs
cn_B = am_cn_B coeffs
cn_AB = am_cn_AB coeffs
cl_ail = am_cl_ail coeffs
cm_elev = am_cm_elev coeffs
cm_flaps = am_cm_flaps coeffs
cn_rudder = am_cn_rudder coeffs
bref = ar_bref refs
cref = ar_cref refs
trans :: V3 (V3 a) -> V3 (V3 a)
trans (V3
(V3 e11 e12 e13)
(V3 e21 e22 e23)
(V3 e31 e32 e33))
=
V3
(V3 e11 e21 e31)
(V3 e12 e22 e32)
(V3 e13 e23 e33)
skew :: Num a => V3 a -> V3 (V3 a)
skew (V3 x y z) =
V3
(V3 0 (-z) y )
(V3 z 0 (-x))
(V3 (-y) x 0 )