Block diagrams 01-02-2012

UAVDevBoard – MatrixPilot

MatrixPilot – Glossary

cCL course leg bearingcSP bearing set pointySP yaw set pointy yawq pitchf rollqSP pitch setpointz altitudezSP altitude setpointTh throttle commandThm manual throttle commandEm manual elevator commandAm manual aileron commandRm manual rudder commandVa airspeedVg ground speedVw wind velocityVIMU speed from IMUSc steering command

de elevator deflection (>0 up)dr rudder deflection (>0 right)da aileron deflection (>0 right)

MatrixPilot – Parameters

Tmax ALT_HOLD_THROTTLE_MAXTmin ALT_HOLD_THROTTLE_MINqmax ALT_HOLD_PITCH_MAXqmin ALT_HOLD_PITCH_MINq0 ALT_HOLD_PITCH_HIGHqRTL RTL_PITCH_DOWNDH HEIGHT_MARGINVSP DESIRED_SPEEDPKP PITCHGAINPKD PITCHKDKRUE RUDDER_ELEV_MIXKROE ROLL_ELEV_MIXKEBO ELEVATOR_BOOSTYKPA YAWKP_AILERONRKPA ROLLKPRKDA ROLLKDYKDA YAWKD_AILERONKABO AILERON_BOOSTYKPR YAWKR_RUDDERYKDR YAWKD_RUDDERRKPR ROLLKP_RUDDERKARU MANUAL_AILERON_RUDDER_MIXKRBO RUDDER_BOOST

MatrixPilot – Coordinate Systems – Industry Standard Convention

q

q

y

y

f

f

xe

ye

ze

xb

yb

zbEarth fixed reference frame: (xe, ye, ze)Body-fixed reference frame: (xb, yb, zb)Euler angles yaw, pitch & roll: ( ,y , )q f

MatrixPilot – Rotation rates – Industry Standard Convention

xb

zb

(p, q, r) are the coordinates of the rotational vector Wexpressed in the body-fixed reference frame (xb, yb, zb)

yb

p

q

r

MatrixPilot – Coordinate Systems – Warning

This presentation uses the industry standard convention for aerospace coordinate systems.

The plane coordinate system coincide with the Earth-fixed reference frame when the plane is located at the origin of the Earth-fixed reference frame, pointing North, with the plane level with respect to both pitch q and roll f.

For historical reasons the plane and the earth coordinate systems used by the UAVDevBoard software differ slightly from the industry standard convention.

Please refer to:http://code.google.com/p/gentlenav/wiki/UDBCoordinateSystems

The relation between the Direct Cosine Matrix and the Euler angles is (standard convention):

𝑅=[c os𝜃 c os𝜓 sin 𝜙 sin 𝜃 cos𝜓− cos𝜙 sin𝜓 cos𝜙 sin𝜃 cos𝜓+sin𝜙 sin𝜓cos𝜃 sin𝜓 sin 𝜙 sin 𝜃 sin𝜓+cos𝜙cos𝜓 cos𝜙 sin 𝜃 sin𝜓− sin𝜙 cos𝜓−sin 𝜃 sin 𝜙cos𝜃 cos𝜙 cos𝜃 ]

MatrixPilot – Altitude Control (full altitude hold or pitch only)

Min()Va

VIMU

VSP

Dz

T

DH

-DH 0

Tmax

Tmin

Dz

q

DH

-DH 0

qmax

qmin

q0

zSP

0 speed control

g2

VV 22SP

z

DzV

s1

1

Dz

Th

qSP

t 70ms

Thm

full altitudehold

MatrixPilot – Normal Pitch Control

PKP

PKD

KRUE

qSP

qRTL

dr

de

0

0

0

KROE

radio off& GPS steering

pitch stabilization

rudder input used& rudder output used& pitch feedback

sinq

cos q sinf

(cos q sinf)2

cosq

dtd

KEBO0

radio ON & pitch feedback

Em

0

pitch feedback

MatrixPilot – Normal Pitch Control

The Direct Cosine Matrix is used as much as possible:

Pitch

Pitch rate

Roll

θ≃sin 𝜃=−𝑅31

𝑑𝜃𝑑𝑡

=�̇�≃cos𝜃𝑑𝜃𝑑𝑡

=cos𝜃 (𝑞cos𝜙−𝑟 sin 𝜙 )=𝑞⋅𝑅33+𝑟 ⋅ 𝑅32

𝜙≃cos𝜃 sin 𝜙=𝑅32

MatrixPilot – Waypoints Normal Navigation

waypoint (n)

waypoint (n+1)

course leg

finish line

waypoint radius

The bearing set point cSP is equal to the bearing between the plane and the next waypoint

The finish line is perpendicular to the course leg

N cSP

g

MatrixPilot – Waypoints Cross Track Navigation

waypoint (n)

waypoint (n+1)

N cCL

course leg

finish line

waypoint radius

CTMARGIN

If the cross track error is greater than CTMARGIN the plane bearing set point cSP is limited to the course leg bearing cCL plus or minus 45°

N cSP

cross track error

If the cross track error is lower than CTMARGIN the deviation of the plane bearing set point cSP relatively to the course leg bearing cCL is proportional to the cross track error

The finish line is perpendicular to the course leg

g

MatrixPilot – Navigation – Yaw Set Point

𝜓𝑆𝑃=𝜒 𝑆𝑃+arcsin (𝑉𝑤❑⊥

𝑉 𝑎)𝑉𝑤❑

⊥=𝑉𝑤𝑥  sin   𝜒𝑆𝑃−𝑉𝑤𝑦

cos   𝜒 𝑆𝑃

The yaw set point ySP is not strictly equal to the bearing set point cSP in order to take into account the crabbing of the airplane due to the wind

ySP

g

aw

N

cSP

x

y

V w❑⊥

MatrixPilot – Navigation – Yaw Angle Error

ySP

Nx

y

yDy

1

1

sin ySP

sin y

co

s yS

P

co

s y

�⃗�

𝐯cos𝜃 �⃗�×𝑣=[𝑅11

𝑅21]×[c os𝜓𝑆𝑃

sin𝜓𝑆𝑃]

cos𝜃 �⃗�=[𝑅11

𝑅21]=[c os𝜃 c os𝜓cos𝜃 sin𝜓 ]

¿cos𝜃 (cos𝜓 sin𝜓𝑆𝑃−𝑐𝑜𝑠𝜓𝑆𝑃 sin𝜓 )¿cos𝜃 sin (𝜓𝑆𝑃−𝜓 )¿cos𝜃 sin (𝛥𝜓 )

The error between the yaw set point and the actual yaw is computed using the Direct Cosine Matrix

≃𝛥𝜓

MatrixPilot – Navigation – Steering Command

ySP

y

DyScDy

Sc

90°

-90°

0

cosq

180°

-180°

-cosq

cos q sinDy

wind_gain

The steering command is set to a constant value if the yaw error is greater than 90° or lower than -90°

The steering command is homogeneous to a bank angle

MatrixPilot – Normal Roll Control

YKPA

0

aileron navigation& GPS steering

RKPA

0

roll stabilization aileron& pitch feedback

RKDA

0

roll stabilization aileron& pitch feedback

YKDA

0

yaw stabilization aileron& pitch feedback

p

cos q sinf

r

Scda

KABO

0

radio ON& pitch feedback

Am

MatrixPilot – Normal Yaw Control

YKPR

0

rudder navigation& GPS steering

RKPR

0

roll stabilization rudder& pitch feedback

YKDR

0

yaw stabilization rudder& pitch feedback

KARU

0

pitch feedback

cos q sinf

r

Scdr

Am

KRBO

0Rm

radio ON& pitch feedback