Teknisk dokumentation duavn II - The Pigeon. Version 0.2 Dokumentansvarig: Joakim Rylander Datum: 22 maj THE PIGEON

Transkript

1 Teknisk dokumentation duavn II - The Pigeon Version 0.2 Dokumentansvarig: Joakim Rylander Datum: 22 maj THE Status Granskad Godkänd Malin

2 Projektidentitet Grupp Hemsida: Beställare: Kund: Kursansvarig: Projektledare: Handledare: Daniel Axehill, Linköpings universitet Telefon: , Rickard Karlsson, Linköpings universitet Telefon: , Anders Hansson, Linköpings universitet Telefon: , Jonas Månsson Phone: , Jeroen Hol, Linköpings universitet Phone: , Rikard Falkeborn, Linköpings universitet Phone: , Gruppmedlemmar Namn Ansvarsområden Telefon Ledningsgrupp Jonas Månsson Projektledare jonma575 Markus Olsson Gruppledare UAV marol523 UAV-gruppen Markus Olsson Gruppledare marol523 Malin Kjelldal Dokumentansvarig malkj133 Petter Säby Kvalitetsansvarig petsa060 Johan Gunnarsson Testansvarig johgu540 Mattias Nilson Designansvarig mjukvara matni943 Rikard Vinkvist Designansvarig hårdvara vinri370 Johan Kingstedt Integrationsansvarig johki998 Björn Eriksson Flygplansansvarig bjoer625 Kameragruppen Jonas Månsson Gruppledare jonma575 Joakim Rylander Dokumentansvarig joary771 Andreas Karlsson Kvalitetsansvarig andka573 Anders Nordlund Testansvarig andno586 Jonas Josefsson Designansvarig hårdvara jonjo442 & mjukvara Patrik Martinsson Integrationsansvarig patma570

3 Dokumenthistorik Version Datum Utförda ändringar Utförda av Granskad av /5-07 Upprättande av skal Jocke /5-07 Sammanställning Jocke Malin

4 Innehåll 1 Introduktion Parter Mål Användningsområden Bakgrundsinformation Definitioner Systemöversikt Produktbeskrivning Produktkomponenter Beroende till andra system Ingående delsystem Begränsningar Designfilosofi Positioneringssystemet Översikt av positioneringssystemet Interface mot andra system Koordinatsystem ECEF till NED Flygplansfasta koordinatsystemet Extended Kalman Filter Tillståndsmodell Det som finns och fungerar Problem och saker som är kvar att lösa Lösningar på problemen Saker som testats men inte fungerat så bra Styrsystemet Översikt av styrsystemet Modellering Olinjär modell Linjär modell Simulinkmodell Parametrar Identifiering Linjär identifiering Olinjär identifiering Stabilisering Banföljning Bangenerering Kamerasystemet Systemöversikt Hårdvara Mjukvara

5 5.3.1 Översikt Kommunikation Framtagning av IMU-data Beräkning av servoutslag Vad vi har testat som gick fel och vad som kan jobbas vidare på Hårdvara Linux-datorn Gatekeeper Hardware-In-The-Loop Systembeskrivning PWM-inläsning Sensorsimulering Problem och lösningar på problem Mjukvara Actuators Sensors Main Control control pid tracking generate map Lqcontrol Position Matrix Byggmapp Kompilator A Aerodynamiska derivator 25

6 UAV 1 1 Introduktion Syftet med projektet var att utveckla ett positionerings- och styrsystem för ett autonomt modellflygplan, unmanned aerial vehicle (UAV). Positioneringssystemet använder sig av en GPS samt en IMU. IMU:n består av accelerometer, gyro samt en elektromagnetisk kompass, vilka alla mäter i tre dimensioner. Utifrån data från positioneringssystemet styr styrsystemet flygplanets roder. Databehandlingen sker ombord på planet i en Linux-dator. Piloten kan välja om planet ska flyga autonomt eller manuellt. Projektet har till viss del varit en fortsättning på projektet Autonomt flygplan - UAV vilket utfördes 2006 vid Linköpings universitet. Planet är även utrustat med en kamera vilken kommer att kunna styras från marken via ett par VR-glasögon. I detta dokument presenteras våra tekniska lösningar, ni kommer att kunna läsa varför vi valt de lösningar vi har valt och varför vi har valt bort andra tänkbara lösningar. All tillhörande kod bifogas på CD-skivan som följer med dokumentet. 1.1 Parter Kunden är Rikard Karlsson och beställaren är Daniel Axehill, båda från ISY vid Linköpings universitet. Projektet har utförts av 14 personer varav 12 läser fjärde året på Y (Teknisk Fysik och Elektroteknik) och 2 läser fjärde året på D (Datavetenskap), samtliga går kursen Reglerteknisk Projektkurs, TSRT Mål Målet med projektet var att konstruera och bygga ett flygplan som skulle kunna flyga autonomt efter en på förhand given bana med hjälp av GPS och data från en IMU. Planet skulle även vara utrustat med en kamera vilken skulle kunna styras via VR-glasögon. 1.3 Användningsområden Produkten kan användas inom många områden t.ex. att leta efter försvunna personer eller som en demonstrator för Reglerteknik. 1.4 Bakgrundsinformation Projektet Autonomt flygplan - UAV utfört 2006 vid Linköpings universitet utgjorde grundmaterialet i detta projekt. Det projektet hade liknande mål som detta men lyckades inte få flygplanet att flyga autonomt. Det är främst hårdvaran från det projektet som har unyttjats. Det som var helt nytt för detta projekt är kameran på planet samt att grundliga tester av hård- och mjukvara skulle kunna utföras. 1.5 Definitioner UAV Unmanned Aerial Vehicle IMU Inertial Measurement Unit, används för att bestämma UAV:ns position och rörelse. GPS Global Positioning System, används för att bestämma UAV:ns position.

7 UAV 2 2 Systemöversikt Detta kapitel ger en översikt av systemet och dess ingående delar. 2.1 Produktbeskrivning Ett modellflygplan har modifierats så att det autonomt kan följa en given bana i luften. Planet är även utrustat med en kamera vilken styrs via ett par VR-glasögon. 2.2 Produktkomponenter Produkten består av ett modellflygplan utrustad med en kamera, en Linux-dator, VRglasögon, en GPS, två IMU:er bestående av en accelerometer, ett gyro samt en elektromagnetisk kompass. 2.3 Beroende till andra system Mätutrustningen, GPS och IMU, kommer att överföra data till Linux-datorn. Positioneringssystemet kommer att köras på Linux-datorn där också styrsystemet körs samtidigt. Linux-datorn styr flygplanets roder med hjälp av styrservon via en gatekeeper. Se Figur 1 för hur systemet interagerar. Reglersystem Positioneringssystem Linux dator Flygplan IMU GPS Kamera VR glasögon Figur 1: Systemen innanför den prickade linjen består av mjukvara medan systemen innanför det heldragna området är hårdvara på flygplanet. 2.4 Ingående delsystem Systemet kan delas in i följande delsystem:

8 UAV 3 Positioneringssystemet Styrsystemet Kamerasystemet Hårdvara I Figur 1 ges hur delsystemen interagerar där kamerasystemet består av kameran och VR-glasögonen. 2.5 Begränsningar Start och landning av planet sker manuellt, alltså det autonoma läget slås bara på när planet är i luften. Manuell styrning kan alltid avbryta det autonoma läget. Planet kan dessutom bara flyga vid bra väderförhållanden och med enkla banor. 2.6 Designfilosofi För att enkelt kunna byta ut och ersätta t.ex. sensorer och algoritmer är systemen uppbyggda i moduler. Koden är skriven på ett generellt sätt så att nya modeller lätt kan testas i regulator och filter. 3 Positioneringssystemet 3.1 Översikt av positioneringssystemet Positioneringssystemet syfte är att förse reglersystemet med nödvändig data för att följa en fördefinierad bana. Med hjälp av en GPS, en IMU och en modell kan flygplanets lägen skattas. Mätdata från IMU har bra precision, men tenderar att driva över tiden. GPS:ns data är brusig, men har inget statiskt fel. Det positioneringssystemet gör är att vikta samman de olika mätningarna tillsammans med information från en modell. Detta görs med ett extended Kalman filter (EKF) och på så vis skattas planets position i luften. 3.2 Interface mot andra system Positioneringssystem Styrsystem IMU GPS Från positioneringssystemet skickas Figur 2: Interface positioneringssystemet Position i NED-systemet (se Kap. Transformationer).

9 UAV 4 Hastighet i flygplansfasta koordinatsystem. Vinkelläge i flygplansfasta koordinatsystem. Vinkelhastighet i flygplansfasta koordinatsystem. 3.3 Koordinatsystem ECEF till NED Först och främst måste man bestämma ett origo i latitud, longitud och höjd, lämpligen där planet startar. Detta görs genom att ändra i C-kod(ekf.c). GPS:ens latitud och longitud kommer att transformeras först till ett jordfixt system, ECEF (Earth Centred, Earth- Fixed Frame), enligt X = a cos(α) cos(ω) 1 a2 b 2 a 2 sin(α) + h 2 Y = a cos(α) sin(ω) 1 a2 b 2 a 2 sin(α) + h 2 Z = a(1 a2 b 2 a ) + h 2 sin(α) 1 a2 b 2 a 2 sin(α). 2 där α är latitud och ω är longitud och a = b = Jordradien vid ekvatorn Jordradien vid polerna Därefter transformeras ECEF-systemet till ett lokalt NED-system (North,East,Down), enligt rotationsmatrisen sin(α) cos(ω) sin(ω) cos(ω) cos(α) RE N = sin(α) sin(ω) cos(ω) sin(ω) cos(α) cos(α) 0 sin(α) Flygplansfasta koordinatsystemet Förutom det jordfasta koordinatsystemet (NED) så finns även ett flygplansfast koordinatsystem (X b,y b,z b ) benämns som B med origo i masscentrum på flygplanet. X b pekar från masscentrum fram genom nosen på flygplanet, Y b pekar ut genom högra vingen och Z b pekar ner mot jordens centrum, se Figur 3. IMU:n har ett inbyggt koordinatsystem som man vill ska sammanfalla med det flygplansfasta. IMU:ns axlar pekar med X IMU genom nosen på flygplanet, Y IMU genom vänster vinge och Z IMU rakt upp. För att få dessa koordinatsystem att sammanfalla används följande rotationsmatris. RIMU B =

10 UAV 5 z_imu y_imu x_imu X_b Y_b Z_b N E D Figur 3: Översikt koordinatsystem 3.4 Extended Kalman Filter Givet ett olinjärt system ẋ(t) = f(x(t),u(t)) + v(t) (1) y(t) = h(x(t)) + e(t) (2) Där processbruset v(t) och mätbruset e(t) är vitt brus. Systemet kan approximeras med ẋ(t) f(ˆx,u) + f x (ˆx,u)(x(t) ˆx) + v(t) (3) där f x (ˆx,u) = x f(x,u) x=ˆxt t. Diskretisering av systemet ges enligt Karlsson (2002) av T x(t + T) = ˆx + e f x(ˆx,u)τ dτ f(ˆx,u) (4) 0 där e At = I + At + A 2 t2 t3 2! + A3 3! +... Algoritmen för kalmanfiltret ges då enligt Karlsson (2002) av: Tidsuppdatering: T ˆx t+t t = ˆx t t + e f x(ˆx t t,u)τ dτ f(ˆx t t,u) (5) 0 P t+t t = e f x(ˆx t t,u)t P t t e f x(ˆx t t,u) T + Q t (6) Där P t t = E((ˆx t t x(t))(ˆx t t x(t)) T ) och Q t = Cov(v(t)) Mätuppdatering: där H t = x h(x) x=ˆxt t och Cov(e(t)) = R t. ˆx t t = ˆx t t T + K t (y t h(ˆx t t T )) (7) S t = H t P t t T H t + R t (8) K t = P t t T H ts t 1 (9) P t t = P t t T K t S t K t (10)

11 UAV Tillståndsmodell Modellen för flygplanet har följande tillstånd: x = [ u v w p q r θ φ ψ x cg y cg z cg a x a y a z ] T med komponenter enligt tabell (1). Modellen för accelerationen ges av en enkel brusmodell. Tillstånd Koordinatsystem Beteckning Eulervinklar Flygplanets (B) φ, θ, ψ Vinkelhastigheter Flygplanets (B) p, q, r Positioner NED x cg, y cg, z cg Hastigheter NED u, v, w Accelerationer NED a x, a y, a z Tabell 1: Komponenter i tillståndsvektorn 3.6 Det som finns och fungerar Positioneringssystemet finns i både Matlab-kod och c-kod. C-koden ligger på Linuxdatorn och fungerar när man kör HITL men med riktiga sensordata blir det inte fullt lika bra. Det samma gäller för Matlab-koden. Själva EKF:en verkar fungera som den ska men vinklarna skattas inte helt korrekt, se kapitel Problem och saker som är kvar att lösa Det största problemet är att EKF:en inte skattar positionen av flygplanet bättre än med enbart GPS-data, se Figure 4(b). Man ser då att skattningen är lite kantig vilket den inte borde vara. Orsaken till detta är förmodligen att vinklarna inte skattas helt rätt. I Figure 4(a) ser vi hur planets positon skattas mha EKF:en fast utan mätuppdateringen från GPS:en samt vad GPS:en säger att vi hade för bana vid flygningen. Man ser då att den skattade positionen är en vridning av vad GPS:en säger. Anledningen till att det ser ut på detta vis är oklar men förmodligen är det fel i någon rotationsmatris eller i skattningen av vinklarna. Att på ett tillförlitligt sätt skatta vinklarna. Ett initeringssystem för positioneringen måste göra. Där man bestämmer origo samt skatta eventuella bias-fel. Att kunna bestämma flygplanets höjd. De mätningar man får från GPS:en kan man inte lita på eftersom att när man har landat planet är det fortfarande uppe och flyger enligt GPS:en. GPS:en ger ifrån sig speed over ground vilken vi loggar. Dock loggas inte course over ground vilken skulle behövas för att tala om åt vilket håll flygplanet flyger. Den approximation som har gjorts är att speed over ground approximerats med u, alltså med flygplanets hastighet rakt frammåt.

12 y y THE UAV skattat GPS data 300 skattat GPS data x x (a) Skattning från EKF:en utan mätuppdatering från GPS:n. (b) Skattning från EKF:en med mätuppdatering från GPS:n. Figur 4: Skattad och uppmätt position vid en riktig flygning. 3.8 Lösningar på problemen Först och främst måste man göra ett test där man vet de exakta vinklarna vid vissa tidpunkter och jämföra dessa med vad IMU:n säger och vad man kan skatta dessa till. Detta gäller för alla vinklar, θ, φ och ψ. Implementera en höjdmätare. Utvärdera hur pass bra approximationen är och fundera på om man ska implementera så att course over ground loggas. 3.9 Saker som testats men inte fungerat så bra. Det gjordes ett försök att generera hela koden för EKF:en med kommandot ccode i Matlab. Detta resulterade att det blev cirka rader kod vilket inte kändes så optimalt. Detta försök gjordes för vi trodde att inversen i EKF:en skulle ta för lång tid. 4 Styrsystemet 4.1 Översikt av styrsystemet Styrsystemets uppgift är att navigera och styra flygplanet i luften efter det att piloten har slagit över till autonomt läge. Planet ska kunna följa en i förväg genererad bana. Styrsystemet utvecklades Matlab/Simulink och implementerades sedan i C-kod för att kunna användas på flygdatorn. Regulatorn består av tre delar; en del för att generera en bana, en del för att följa den banan samt en del för att stabilisera flygplanet i luften. Stabliseringen utgör en inre loop medan banstyrningen utgör en yttre. 4.2 Modellering En modell över flygplanets dynamik är ett måste både för simulering och regulatorframtagning samt för kalmanfiltret för positioneringen. Följande ekvationer står som grund till flygplansmodellen och är tagna ur (Nelson 2002). De beskriver följande:

13 UAV 8 u, v, w - Flygplanets hastighet gentemot ett på marken fixt koordinatsystem, dock uttryckt i flygplanets kroppsfixa x,y,z koordinater. Dvs u är flygplanets hastighet i nosriktningen, v avdriftshastigheten längst ena vingen och z hastigheten nedåt. p, q, r - Flygplanets rotationshastighet i kroppsfixa koordinater. Dvs p är rollvinkelhastighet, q är tippvinkelhastighet och r är girvinkelhastighet. θ,φ,ψ - Flygplanets orientering uttryckt i eulervinklar där θ är tippvinkel, φ är rollvinkel och ψ är girvinkel. Det finns olika konventioner i vilken ordning eulervinklarna skall roteras och det påverkar var en singularitet kommer att uppträda. Observera att på detta sätt {θ,φ,ψ} som är vinklarna kring {y,x,z} i de kroppsfixa koordinaterna finns en singularitet i θ = 90 o. x cg,y cg,z cg - Flygplanets postition gentemot ett på marken fixt koordinatsystem a u,a v,a w - Flygplanets acceleration i de kroppsfixa x,y,z koordinaterna Allting är beskrivet i SI-enheter förutom utsignalen äirspeedfrån simulinkmodellerna som är i knop Olinjär modell En olinjär DAE-modell över flygplansdynamiken ges av: u = qw + rv gsin(θ) + X(u,w,δ T )/m v = ru + pw + gcos(θ)sin(φ) + Y (v)/m ẇ = pv + qu + gcos(θ)cos(φ) + Z(u,w,δ e,δ T )/m 1 ṗ = I xi z I (L(v,p,r,δ xz 2 a )I z + N(v,p,r,δ r )I xz + qr (Iz 2 I z I y + Ixz) 2 + pq I xz (I y I x I z ) q = 1 I y (M(w,w,δ e ) rp(i x I z ) I xz (p 2 r 2 )) 1 ṙ = I xi z I (L(v,p,r,δ xz 2 a )I xz + N(v,p,r,δ r )I x qr I xz (I z I y + I x ) pq (I x I y Ix 2 Ixz)) 2 θ = qcos(φ) sin(φ) φ = p + qsin(φ)tan(θ) + rcos(φ)tan(θ) ψ = (qsin(φ) + rcos(φ)) 1 cos(θ) ẋ cg = cos(θ) cos(ψ) u + (sin(φ) sin(θ) cos(ψ) cos(φ) sin(ψ)) v+ +(cos(φ) sin(θ) cos(ψ) + sin(φ) sin(ψ)) w ẏ cg = cos(θ) sin(ψ) u + (sin(φ) sin(θ) sin(ψ)+ +cos(φ) cos(ψ)) v + (cos(φ) sin(θ) sin(ψ) sin(φ) cos(ψ)) w ż cg = sin(θ) u + sin(φ) cos(θ) v + cos(φ) cos(θ) w ȧ u = 0 ȧ v = 0 ȧ w = 0 Luftkrafterna och momenten X,Y,Z och L,M,N kan ställas upp på olika sätt, en linjäriserad (11)

14 UAV 9 modell ges av Nelson (2002): X/m = g sinθ 0 + X u (u u 0 ) + X w (w w 0 ) + X δt (δ T δ T0 ) Y/m = g cos θ 0 sin φ 0 + Y v (v v 0 ) Z/m = g cos θ 0 cos φ 0 + Z u (u u 0 ) + Z w (w w 0 ) + Z δe (δ e δ e0 ) + Z δt (δ T δ T0 ) L = I x (L v (v v 0 ) + L p (p p 0 ) + L r (r r 0 ) + L δa (δ a δ a0 )) M = I y (M w ((w w 0 ) + Mẇ(ẇ ẇ 0 ) + M q (q q 0 ) + M δe (δ e δ e0 ) + M δt (δ T δ T0 ))) N = I z (N v (v v 0 ) + N p (p p 0 ) + N r (r r 0 )) (12) Denna luftkraftmodell är linjäriserad kring det stationära referenstillståndet {u 0 v 0 w 0 p 0 q 0 r 0 θ 0 φ 0 ψ 0 } som typiskt är ett trimmat flygläge i konstant hastighet framåt. Vid detta trimmade tillstånd när flygplanet ligger i konstant planflykt ges rodrens referenslägen som {δ e0 δ a0 δ T0 δ r0 } Linjär modell Vid identifiering och framtagning av regulatorer finns ett behov av en linjäriserad modell. Om man anser att de inertiella kopplingarna i flygplanet är små och sätter I xz = 0 så avkopplas den longitudinella och den laterala dynamiken vid en linjärisering. Den longitudinella linjäriserade tillståndsekvationen ser ut på följande vis: u ẇ q θ = X u X w 0 g Z u Z w u 0 0 M u + MẇZ u M w + MẇZ w M q + Mẇu u w q θ + 0 Z δe M δe + MẇZ δe 0 δ e Och de laterala ges av: v Y v Y p (u 0 Y r ) g ṗ ṙ = L v L p L r 0 φ N v N p N R v p r φ + 0 Y δr L δa N δa 0 L δr N δr [ δa δ r ] Simulinkmodell Modellen finns implementerad i simulink som en S-funktion (mex funktion) skriven i C. Detta för att den ska gå att köra i realtid i HITL samt att det ger en större översikt än om den vore implementerad som simulinkblock. Samma c-kod har även används till både positionering och identifiering. Modellen finns implementerad i simulink som en S-funktion (mex funktion) skriven i C. Detta för att den ska gå att köra i realtid i HITL samt att det ger en större översikt än om den vore implementerad som simulinkblock. Samma c-kod har även används till både positionering och identifiering. Både den olinjära modellen och den linjära modellen finns även som matlabkod. Koden för den olinjära modellen är skrivet så att den går använda både som matlabkod eller som C-kod. Följande filer finns för den olinjära modellen:../modell/olinjär modell/flygplan_par_input.c - Detta är i huvudsak simulinkskalet för en s-funktion. Denna fil kompileras med matlabs mex-kommando.

15 UAV 10 Delta_e Delta_a states Delta_T Delta_r Flygplan_par_input P Parameters Constant7 Ref Reference state Constant6 Airspeed I Constant5 Inertia Flygplansmodell Figur 5: Simulinkmodell för flygplanet../modell/olinjär modell/flygplan\_par\_input\_wrapper.c - I denna fil finns ekvationerna.../modell/olinjär modell/model_names.h - Headerfil som behövs till modellen../modell/olinjär modell/olinjar.m - m-fil för modellen, kan simuleras med ode45 i matlab. Följande filer finns för den linjära modellen:../modell/linjär modell/flygplan_par_input_linjar.c - Detta är i huvudsak simulinkskalet för en s-funktion. Denna fil kompileras med matlabs mex-kommando.../modell/linjär modell/flygplan_par_input_linjar_wrapper.c - I denna fil finns ekvationerna.../modell/linjär modell/model_names.h - Headerfil som behövs till modellen../modell/linjär modell/linjar.m - m-fil för modellen, ger A,B,C matriser samt ett LTI objekt över modellen Simulinkmodellerna tar rodersignaler som insignaler och ger alla tillstånd samt farten som utsignaler. De tar även en parametervektor med aerodynamiska stabilitetsderivator, en tröghetsmomentvektor samt ett referenstillstånd som insignaler. Referenstillståndet blir även modellens initialtillstånd. Det är viktigt använda rätt referenstillstånd då de aerodynamiska derivatorna gäller endast kring referenshastigheten u 0. Till den linjära modellen finns några ytterligare parametrar benämnda Plinj Parametrar Modellen behöver flygplansspecifika parametrar för att överensstämma med det verkliga systemet. Många av dessa parametrar går att bestämma genom fysisk mätning på flygplanet. Vår tanke var att använda System Identification Toolbox till att identifiera de aerodynamiska derivatorna. Därför användes givna parametervärden, bla de från förra årets projekt samt från ett större flygplan vid simuleringar och som startparametrar vid identifiering. Modellerna behöver vektorer som ligger i workspace med följande struktur:

16 UAV 11 P = [Xu Xw XdT Yv Zu Zw Zw_dot Lv Lp Lr Lda Mw Mw_dot Mq Mde Np Nr Nv Ndr Zde]; Plinj = [Yp Yr Mu ZdT Ldr Nda]; I = [Ix Iy Iz Ixz]; Ref = [ ]; Vid identifieringen av den linjära modellen delades även den laterala och longitudinella dynamiken upp och följande parametervektorer används: Plat = [Yv Yp Yr Lv Lp Lr Lda Ldr Np Nr Nv Ndr Nda]; Plong = [Xu Xw XdT Zu Zw Zw_dot ZdT Zde Mw Mw_dot Mq Mde Mu]; Hur de aerodynamiska derivatorna kan beräknas från mer mätbara konstanter beskrivs i appendix. 4.3 Identifiering För att identifiera de okända parametrarna används System Identification Toolbox i Matlab. För detta finns 4 st strukturerade modeller, s.k greybox modeller. De fyra modellerna är den olinjära ovan, hela den linjära, den laterala och den longitudinella som beskrivs ovan. De filer som är relevanta för identifieringen är följande: För den linjära identifieringen:../identifiering/linjär/linjar_ident.m - Greyboxmodellen för hela den linjära modellen../identifiering/linjär/get_params_linjar.m - Matlabfil där data behandlas och ordnas och en identifiering körs.../identifiering/linjär/lateral_ident.m - Greyboxmodellen för den laterala modellen../identifiering/linjär/get_params_lateral.m - Matlabfil där data behandlas och ordnas och en identifiering körs.../identifiering/linjär/longitudinell_ident.m - Greyboxmodellen för den longitudinella modellen../identifiering/linjär/get_params_longitudinell.m - Matlabfil där data behandlas och ordnas och en identifiering körs. För den olinjära identifieringen:../identifiering/olinjär/olinj.m - Greyboxmodellen som m-fil för den olinjära modellen../identifiering/olinjär/olinjar.c - Greyboxmodellen för den olinjära modellen skriven i c-kod

17 UAV 12../Identifiering/Olinjär/model_names.h - Headerfil som behövs till greyboxmodellen../identifiering/olinjär/get_params_olinjar.m - Matlabfil där data behandlas och ordnas och en identifiering körs. För att konvertera loggad data till samma dataformat som genereras av HITL så finns filen:../identifiering/loggad data/konv_data.m Denna genererar en.mat fil innehållande en struct. Om den benämns Log_Data så är Log_Data.X tidsvektorn, och Log_Data.Y innehåller datat. Exempelvis Log\_Data.Y(1).Data ger den loggade signalen för autonom på/av (eller vid identifieringssflygningar sidorodret). Man får en beskrivning av önskat tillstånd i genom Log_Data.Y(i).Name Linjär identifiering Man kan starta en identifiering och enbart försöka passa in modellen för enstaka utsignaler. Detta görs genom att ge estimeringsfunktionen en greyboxmodell med enbart den önskade utsignalen. Om exempelvis modell är en greyboxmodell och data är en loggad data så kan estimeringen göras enbart på utsignal i från modellen genom att ge modell(:,i) som inargument: pem(data,modell(:,i). På detta sätt valdes de tillstånd som är viktiga för regleringen ut och identifiering gjordes på ett tillstånd i taget i den laterala och longitudinella modellen. Som datamängder finns flygningar där exempelvis enbart skevroder används och andra dataserier där enbart höjdroder används. När sedan parametervärden för dessa fåtts utökas sedan identifieringen till att fyra tillstånd i den linjära modellen identifierats, {p, q, θ, φ} med de nya parametervärdena som start parametrar. Dessa tillstånd har nu bra passning, mellan 20-45% när de jämförs med funktionen compare i System Identification Toolbox. Dessa parametrar som fungerar bra ihop med den linjära modellen finns i:../identifiering/parametrar/params_linjar_ident.mat Olinjär identifiering Den olinjära identifieringen är mycket resurskrävande för datorn. Försök med att starta i de från den linjära identifieringen framtagna parametervärdena misslyckades eftersom problemet blev för stort och minneskrävande. Eftersom det är svårt att avgöra efter den linjära identifieringen vilka parametrar som fått ett vettigt värde så är det svårt att förminska problemet genom att minska antalet fria parametrar, man vet helt enkelt inte vilka man kan låsa. En bättre metod hade varit att mäta upp de parametrar som har en fysikalisk koppling och låsa dessa. Problemet var dock att detta insågs för sent. Det har varit problem att starta i de parametervärden från förra året som sett lovande ut i simulering och detta tros bero på att insignalsrepresentationen skiljer sig drastiskt och en omskalning av enbart de parametrar som är kopplade till insignalerna skulle nog kunna göra att idenfieringen kan rulla med dessa parametrar. För att minimera problemet skulle man kunna koncenterera sig på att endast skatta de parametrar de tog från flygplanet Navion förra året och lita på att de andra parametrarana är schysta då de är tagna från ett modellflygplan.

18 UAV Stabilisering För stabiliseringen av flygplanet används en LQ-regulator som reglerar på rollvinkel (φ) och tippvinkelhastighet (q). LQ-regulatorn togs fram för en linjäriserad modell som består av tillstånden (v,w,p,q,r,φ). Tanken med att använda sig av rollvinkel- och tippvinkelhastighet beror mest på intuitiva antaganden givet tillstånden och de möjligheter som finns att styra flygplanet. De styrsignaler som får användas är skevroder och höjdroder. För att göra en sväng så ska en viss rollvinkel ställas ut, detta görs med skevrodren, samt en tippvinkelhastighet för att inte tappa höjd, detta görs då med höjdrodret. Den linjära modellen på tillståndsform ẋ = Ax + Bu Vinkelhastighet och rollvinkel väljs som reglerstorheter så att [ ] M = Viktmatriserna väljs som kvadratiska [ a11 0 Q 1 = 0 a 22 ] [ b11 0 Q 2 = 0 b 22 ] Q 1 är straffmatris för reglerstorheterna q och φ. Q 2 är straffmatris för styrsignalerna höjd-och skevroder. Konstanter a 11,a 22,b 11,b 22 väljs beroende på hur man vill straffa resp. reglerstorhet och styrsignal Beräkning av den optimala återkoppling u = Lx + L r r görs med lqr-funktionen i MAT- LAB som beräknar L. L r beräknas enligt (Glad & Ljung) där L r = [M(BL A) 1 B] 1 som ger att det slutna systemets statiska förstärkning är lika med identiteten. 4.5 Banföljning Banföljningsmodulen genererar en rollvinkelreferens samt höjdskillnaden mellan det tänkta målet (waypoint) och flygplanets höjd över marken, se block i Figur 6 kallat PN-styrning. Höjdfelet blir insignal till en PID-regulator för att beräkna en styrsignaln till höjdrodren. Rollvinkelreferensen är den rollvinkel som planet ska hålla för att erhålla korrekt kurs mot målet. Rollvinkelreferensen beräknas genom att först beräkna riktningsvektorn till målet (i xyplanet) och sedan vilken kurs/färdriktning flygplanet har. Genom att beräkna vinkeln mellan dessa två vektorer kan sedan kurs/färdriktnings-differensen beräknas. Kryssprodukten av riktningsvektorn till målet och flygplanets riktning ger åt vilket håll flygplanet ska svänga beroende på vilket tecken kryssprodukten får Bangenerering Den tänkta banan som flygplanet ska följa genereras i programmet MyGps. Där man sätter ut ett antal waypoints som flygplanet ska passera, dessa waypoints blir relativt

19 UAV 14 delta_e delta_a v w states To Workspace p q 0 delta_t r delta_t theta K*u Lr_Add 0 delta_r P parameters delta_r Parameters Flygplan_olinjar states phi psi xf Lr Ref ref Ref yf zf I I I Flygplansmodell Referens v w K*u L p q r pos_flyg phi NextWaypoint num(s) 1e 2s 2 +s rollref alt_diff PN_styrning pos_flyg pos_mal Out1 altitude_pid heading_diff vinklar heading_diff PN Figur 6: Reglering i Simulink flygplanet. Man kan även sätta ut egna waypoints i en txt-fil, t.ex. 100,0,-100 (x,y,z) som helt enkelt betyder att waypointen ligger 100 meter rakt framför flygplanet på en höjd av 100 meter ( 100 z-axeln pekar ner mot jorden). Den position som flygplanet har då man går över till autonomt läge blir origo för de utplacerade koordinaterna (waypoints). 5 Kamerasystemet 5.1 Systemöversikt Kamerasystemet, även kallat VR-enheten, har till uppgift att från planet filma med en kamera och i realtid skicka ner videosignalen till marken. Kameran ska sitta på ett rörligt stativ på planet, stativet kan roteras kring två axlar med hjälp av två servon. Servonas utslag bestäms av hur en person på marken roterar en IMU. Huvudtanken är att personen på marken monterar IMU:n på huvudet och ser kamerabilden i ett par VR-glasögon, systemet ska då ge en känsla av att man sitter på planet och kameran vrider sig på samma sätt som huvudet. Vridningen av servona motsvarar vridning kring Y b-axeln, θ, och Z b-axeln, ψ, enligt figur Hårdvara Hårdvaran för VR-systemet är uppdelat i två delar, en på marken och en monterad på flygplanet. Dessa är förbundna med två radiolänkar, en för styrning av servon och en för sändning av bild från kameran. De delar som sitter på flygplanet är till största delen färdiga system som har köpts in. Dessa delar är kamera, radiosändare, radiomottagare, servon samt ack. för dessa. Delarna sammankopplas via ett litet kretskort som delar upp spänningen till de olika delarna och förbinder kameran med radiosändaren. Hårdvaran på marken är inbyggd i ett chassi, eller låda, och dess funktion beskrivs mer

20 UAV 15 i användarhandledningen. I lådan finns en mottagare för mottagning av bild från kameran, ett ack., en standardsändare för RC-servon (utmonterad från en vanlig fjärrkontroll tillhörande ett modellflygplan) samt ett kretskort som är egentillverkat. Detta kort har till uppgift att kommunicera med IMU:n och från den givna datan ställa ut resistanser till RC-sändaren som styr servona på flygplanet. Kortet har utvecklats i Eagle och sedan etsats av Sören Hanson på ISY. För övrigt så förser kortet även andra ingående delar med spänning. På kortet finns: 1st AVR Mega16, där alla beräkningar sker. 1st Kristalloscillator på 14,7456 MHz. 3st Spänningsregulatorer som spänningsdelar ner till rätt nivåer för ingående komponenter. 1st MAX232-krets som sköter spänningsnivåerna mellan AVR och IMU. 1st Digital potentiometer som styrs från AVR:en och ställer ut vissa resistanser. En mängd kondensatorer för att hålla konstanta spänningar och för att buffra för MAX232-kretsen. Till kortet ansluts knappar, diod, fläkt, radiosändare o.s.v. för styrning av AVR:en och övrig utrustning. Fläkten behövs för att kyla spänningsregulatorerna. Läs användarmanualen för mer ingånde beskrivning av funktionen. IMU:ns kabel är specialgjord och har två D-sub kontakter i ena änden, en hane och en hona. Den ena används för spänningsmatning och den andra för kommunikation med AVR:en. Observera att dessa kontakter inte är anslutna på standardvis och får alltså inte anslutas på något annat sätt än till det etsade kortet på angivet sätt i användarmanualen. Värt att nämna är också att servona är mycket känsliga och kan lätt gå sönder. Man bör inte spänningssätta servona utan att den stora grå VR-lådan är påslagen eftersom servona då får brus som styrsignal. Aktsamhet krävs också när man resetar enheten genom att trycka på den lilla röda knappen och får bara göras med ett mycket kort tryck. Om man håller in knappen längre körs servona i sina bottenlägen och kan gå sönder. Som bilaga till detta dokument finns filer från EAGLE, pinkonfigurationer och annat som krävs för att kunna reproducera enheten eller felsöka den. 5.3 Mjukvara Översikt Programmet som styr AVR:en är skriven i C och hittas i filen VR.c. Översiktligt fungerar koden så här: Initiering: Systemet initieras, alla register ställs rätt så att UARTkommunikationen fungerar som den ska och IMU:n initieras för rätt sampelperiod, baudrate och mät-mode. Main: Eulervinklar hämtas in, vinklar omvandlas till potentiometervärden och ställs ut Kommunikation Enhetens AVR läser in eulervinklar från IMU:n och skickar ut antal 256-delar av 50 kω till de digitala potentiometrarna.

21 UAV 16 Kommunikationen med IMU:n sköts genom UART-kommunikation via AVR:ens egna USART-port enligt RS232-protokollet. Det fungerar så pass enkelt att vi bara lägger den data, en byte i taget, i ett register så skjuts bitarna ut en efter en och på samma sätt för hämtning av data. Detta sker i funktionerna Skicka data(), Ta emot data() och Hamta byte(). För att skicka data till digitala potentiometrarna så måste man känna till deras eget kommunikationsprotokoll. Det går till så att man har en 17-bits vektor (stackbit + 8 bit pot + 8 bit pot) som skickas ut en efter en genom att man först höjer en bit för att aktivera skrivning och sen pulsar en klockbit en gång för varje gång man lägger ut en ny bit på porten, sedan deaktiverar man skrivning igen. Detta sker i funktionen Ohm out() Framtagning av IMU-data IMU:n är inställd så att den enbart skickar eulervinklarna, de kommer som tre stycken 32-bitars float, varav de två senare (θ och ψ) ska användas. Då c-kompilatorn vi använde i AVR-studio inte ville typomvandla rätt så lyckades vi inte beräkna våra vinklar rätt, som ett resultat fick vi manuellt typomvandla med hjälp av if-satser, for-loopar och de fyra räknesätten. Ytterligare behövde vi även koda upphöjt till -operatorn med hjälp av for-loopar då kompilatorn inte heller ville kännas vid ˆ-tecknet. Den här koden utgör den första delen av Euler to ohm() Beräkning av servoutslag Servoutslagen är beror på värdena på digitala potentiometrarna som är en direkt skalning av eulervinklarna. Testning har gjorts på hur långt man kan vrida servona, ca ±55 grader. Då potentiometervärdena närmar sig minimum eller maximum så går servona i botten och kan gå sönder, en säkerhet mot detta har kodats in så att servona inte hamnar i förbjudna lägen. 5.4 Vad vi har testat som gick fel och vad som kan jobbas vidare på Självklart kan koden snyggas till ytterligare om man kan få typomvandlingen från fyra bytes till float att funka. Egentligen är koden ganska enkel bara att vi tvingats fulkoda en del för att vi inte varit vän med c-kompilatorn. En utvidgning på det här skulle vara att hitta en bättre plats för kameran på planet och trimma in kameran för det. Man kan också trimma in så att vinkeln man vrider IMU:n i ger exakt samma vinkel på kameran. Sen finns en liten bug som gör att IMU:n inte vill nollställa ψ-vinkeln när man trycker på RESET, det funkar ibland men ibland inte och det är något man absolut kan se över. 6 Hårdvara 6.1 Linux-datorn Linux-datorn består av en TS-7200 embedded arm dator (se för mer info). Processorn är en ARM9 200 MHz RISC-processor utan flyttalsenhet. Det man då gör är att man kompilerar med -msoft-float flaggan vilket medför att flyttalsberäkningarna blir emulerade. Detta betyder att flyttalsberäkningar går mycket långsammare än heltalsberäkningar.

22 UAV 17 Datorn är långsammare än man tror, en invertering av en 6x6 double-matris tar ca 60 ms med matrix ops.c vilket inte är så snabbt med tanke på att man vill köra så ofta man kan i reglerloopen. 6.2 Gatekeeper Gatekeeeperns uppgift är att sköta uställande av signaler till roder oavsett om planet står i autonomt eller manuellt läge. Man ska alltid kunna gå över till autonomt läge, även om något händer med flygdatorn. Gatekeepern är etsad på ett separat chip och är inte spänningssatt tillsammans med flygdatorn. I autonomt läge skickar flygdatorn styrsignaler till gatekeepern som den sedan ska omvandla till PWM-pulser för att sedan skickas till de servon som styr rodren. I manuellt läge, det vill säga när planet ska bete sig som ett vanligt modellflygplan, tar gatekeepern in PWM-pulser från radiomottagaren, samplar dessa och ställer ut identiska pulser till servon. Gatekeepern kan även användas till loggning. Då den står i manuellt läge kan information om styrutslag sändas till flydatorn för vidare loggning. Gatekeepern består av mikroprocessorn Atmega16 från Atmel. Processorn är externt klockad i 8Mhz. Den är spänningssatt tillsammans med servo och mottagare med ett batteri på 7.2V. Processorn får in spänningen 5.1V efter en extern spänningsomvandling i två steg (7.2V- 5.8V - 5.1V). Programmen som körs och även skickas med som bilaga är skrivna i C-kod i programmet AVR Studio. Kommunikationen mellan flydator och gatekeeper sköts via en SPI buss där flydatorn är master. SPI gränssnittet i flygdatorn baseras på +3.3V medans SPI på gatekeeper baseras på +5V. För att lösa detta sitter en IC (MAXIM:s MAX3390E) som omvandlar +3.3V till +5V och vice versa. Datat som skickas mellan flygdator och gatekeeper är alltid en byte. För att gatekeepern ska veta vad datat som skickas från flygdatorn är för något så skickas även ett prefix tillsammans med datat. Styrsignalerna till servon ligger från strax över 128 till strax under 256. Datat behöver alltså alla 8 bitar för att beskriva talet men vi behöver fortfarande skicka med ett prefix. Detta löses genom att innan signalen skickas från flygdatorn skiftas byten åt höger samtidigt som den mest signifikanta biten (som alltid är etta) nollas. Vi får därmed två lediga bitar som kan användas som prefix. Återskapande av datat sker på liknande sätt i gatekeepern. Det enda problemet är att upplösningen blir sämre då vi inte vet den minst signifikanta biten. Detta problem är litet då vi fortfarande har gott och väl den upplösning som behövs för bra roderutslag. Om större upplösning ändå önskas kan koden ändras så att prefix skickas i en separat byte för att sedan åtföljas av databyten. I Appendix kommer C-kod samt kopplingsschema av gatekeeper finnas tillgängligt. Prefix Förklaring 00 Mode 01 Aileron 10 Elevator 11 Throttle Tabell 2: Prefix för SPI kommunikation. 6.3 Hardware-In-The-Loop Systembeskrivning I Hardware-In-The-Loop testbänken finns det möjligheter att köra manuella flygningar och även autonoma flygningar för till exempel testning av reglering och positionering. Se figur 7.

23 UAV 18 Komponent Värde Kvantitet Kristall 8MHz 1 Reset switch 1 Kapacitans 100nF 2 Kapacitans 100uF 1 Kapacitans 22pF 2 Kontactlista för att koppla in SPI, spänning och servon Resistans 4.7kohm 1 Plinth för AVR 1 ATmega16 1 Laminate 7.5x10cm 1 Jumpers 4 Spänningstranformation +3.3,+5V 1 Tabell 3: Komponenter på gatekeepern. Manuell flygning: Vid manuell flygning körs en modell av flygplanet på dspace DSPkort. Vidare genererar piloten styrsignaler till modellen genom en flygradio. En radiomottagare tar emot dessa signaler och genererar PWM-pulser vilka läses in genom dspace I/O-burk. På den PC som är ansluten till dspace DSP-kort körs en simulinkmodell som hämtar upp modelltillstånd från DSP-kortet och skickar dessa vidare på nätverket till en visualiseringsdator där programmet FlightGear körs. På den PC som är ansluten till dspace finns även möjlighet att logga och visualisera styrsignaler och modelltillstånd samt att generera stegstörningar genom programmet ControlDesk. Den loggade datan kan användas dels för att visualisera flygningen offline samt för att utvärdera hur bra flygningen gick. Autonom flygning: Vid autonom flygning körs en modell av flygplanet på dspace DSP-kort. Då mode är i Autonomt läge genererar flygdatorn styrsignalerna Skevrodervinkel samt Höjdrodervinkel som tas emot av GateKeepern. Styrsignalerna Motorpådrag samt Mode genereras av flygradion och tas emot av en radiomottagare. Radiomottagaren och GateKeepern genererar PWM-pulser till respektive roder. Dessa signaler läses in genom dspace I/O-burk till modellen på DSP-kortet. Modellens utsignaler skickas sedan via en serieport på dspace I/O-burk till flygdatorns serieport varvid loopen är sluten. På den PC som är ansluten till dspace DSP-kort körs en simulinkmodell som hämtar upp modelltillstånd från DSPkortet och skickar dessa vidare på nätverket till en visualiseringsdator där programmet FlightGear körs. På den PC som är ansluten till dspace finns även möjlighet att logga och visualisera styrsignaler och modelltillstånd samt att generera stegstörningar genom programmet ControlDesk. Data loggas även i flygdatorn till en textfil som efter körningen kan konverteras till Matlab-format PWM-inläsning I blocket PWM till roderutslag sker en omvandling av de analogt inlästa styrsignalerna till att passa modellens förväntade styrsignaler. Externt i dspace-lådan sker en lågpassfiltrering och sampling av PWM-pulserna. De analoga signalnivåerna represenerar på så sätt olika dutycycle hos PWM-pulserna. För att kunna använda styrsignalerna till modellen krävs ytterligare lågpassfiltrering, genom ett glidande medelvärde över två PWM-perioder. Därefter sker en mappning av dessa signalnivåer till att representera roderutslag (som är modellens insignal) i form av en avvikelse i vinkelutslag.

24 UAV 19 Figur 7: Blockschema för HITL. Figur 8: Simulinkschema för HITL.

25 UAV Sensorsimulering För att få en bra HITL-testbänk så behöver flygplanets sensorer simuleras på DSP:n och skicka data till flygdatorn. I det verkliga flygfallet får flygdatorn information av sensorerna via två serieportar. På grund av hårdvarubegränsningar i dspace I/O-burk skickas den simulerade sensordatan via en serieport till flygdatorn. IMU:n skickar data med en frekvens på ca 100 Hz och GPS:n skickar med ca 3 Hz. I HITL skickas all sensordata med en frekvens på 10 Hz Problem och lösningar på problem Vid implementering av testriggen Hardware-In-The-Loop fanns inga tydliga ramar för hur det skulle göras då denna del inte ingått i tidigare års projekt. Det enda som styrde oss var att vi skulle använda dspace-datorerna i Laboteket. Prestandan hos dessa passade inte riktigt våra behov. Bland annat skulle vi behövt två serieportar samt fyra portar för PWM-inläsning och ett DSP-kort med högre klockfrekvens. Detta har gjort att vi under utvecklingens gång testat många idéer och förkastat en hel del av dem vilket gjort att HITL blev mer tidskrävande än väntat. Eftersom vi endast hade tillgång till en serieprot gjordes det ett försöka att emulera en serieport på en I/O-pinne med hjälp av simulink. Detta fungerade i och för sig, men då koden kompilerades ner på DSP-kortet fick steglängden sättas till något ganska stort för att undvika Task Overrun. Lösningen blev att skicka både IMU- och GPS-data genom dspace serieport vid HITL-körningar. Denna serieport var relativt lätt att använda men vi hade i början svårt att hitta dokumentation om dspace hårdvara vilket försenade arbetet flera veckor. 7 Mjukvara I detta avsnitt beskrivs hur c-koden som körs på Linuxdatorn är uppbyggd. Programmet är helt moduläriserat. I kodmappen finns en mapp för varje modul samt en för h-filerna och en byggmapp. För varje modul finns en struct, eller objekt som vi valt att kalla det. I objektet finns alla globala variabler som finns i den modulen. Objekten är typdefinierade structpekare. Varje funktion i modulerna är tänka att ta modulobjektet som indata. T.ex. initsensors(sensorobjptr sensorobject) Detta för att få en lite objektorienterad design. Nedan följer en beskrivning av alla moduler i koden. 7.1 Actuators I denna modul ligger rutinerna för hur man kommer åt aktuatorerna. Här kan man läsa av och skriva till gatekeepern. Använbara funktioner är gatekeepersetstate(gatekeeperobjptr gkobj) Denna funktion skriver servoutslag till gatekeepern. gatekeepergetstate(gatekeeperobjptr gkobj)

26 UAV 21 Denna funktion läser av de nuvarande styrsignalerna som gatekeepern ställer ut. Kommentar till MySleep funktionen är att koden inte testats utan denna funktion, meningen med den är att datorn ska vara i busy-wait läge en halv mikrosekund för att vänta på gatekeepern. 7.2 Sensors I denna modul ligger rutinerna för hur man pratar med sensorerna. Användbara funktioner är getimuvector(sensorobjptr sensorobject, imuvectorptr imudata) Denna funktion läser av IMU:n. Vid skrivandet av denna tekniska dokumentation var imuinläsningen i det verkliga fallet fördröjd (inte i HITL), vad detta beror på är oklart. getgpsposition(sensorobjptr sensorobject, gpsdataptr pos) Denna funktion läser av vad gpsloggern läst in från gps:en från en delad minnesplats. Alla funktioner finns duplicerade i sensors.c på grund av att man skall kunna simulera dem med HITL. I sensormappen ligger även en gpslogger som är open-source, denna är skiven i C++ och körs separat från duvan2 programmet. HitlLogger är till för HITL körningar, denna fungerar på samma sätt som gpsloggern med den skillnaden att den läser in simulerad sensordata från serieporten. imu.c och mtcomm.c har med imuinläsningen att göra där mtcomm.c är exempelkod från Xsens. gps.c samt gsppub.c innehåller funktioner för att läsa av gpsdata från gpsloggern. hitl.c samt hitlpub.c fungerar på samma sätt som gpsinläsningen men läser också in imudata från delat minnesutrymme. 7.3 Main I denna modul hanteras hur koden exekveras, kommunikationen mellan modulerna samt loggning av data. Denna allokerar först upp allt minne som behövs till modulerna för DUV AN TIMER DELTA att sedan initiera dem. Sedan börjar själva main-loopen som körs 2000 gånger i sekunden. get timer2(tp) returnerar aktuellt tick på 2 khz räknaren vilket kan vara bra då man skall mäta tid, 2 khz räknaren resettas varje gång den räknat upp till DUVAN TIMER DELTA. Innan man satt igång autonomt mode, alltså då gatekeeperobject >mode är mindre än 210 så måste alla manuella styrsignaler skickas in till EKF:en de omvandlas från gatekeeperkommandn till radianer i gk 2 rad funktionen. När autonom mode har satts igång blir firstmode=1 och kartan sprids då ut framför flygplanet. När gatekeeperobject >mode är större än 210 så körst hela reglerloopen som ställer ut styrsignalen till gatekeepern. Styrsignalerna konverteras om från radianer till gatekeeperkommandon genom Lq 2 gk funktionen i controlmodulen. Kommunikationen mellan alla moduler sköts av datapool som innehåller inlästa sensorvärden samt utsignaler från respektive modul. 7.4 Control Controlmodulen kan delas upp i 5 delar som beskrivs nedan.

27 UAV control Detta är controlmodulens interface mot mainmodulen. Här finns funktioner för att uppdatera banregleringen och Lq-regleringen samt generera en bana. Användbara funktioner är updatetrackinglq(controlobjptr controlobject,datapoolptr datapool) Uppdaterar banföljningen och referenserna till Lq-regulatorn. updatelq(controlobjptr controlobject,datapoolptr datapool) Beräknar Lq-regulatorns utsignaler pid Denna undermodul innehåller funktioner för att beräkna pid-utsignaler. Användbara funktioner är calculatepidcontroloutput(pidobjptr pidobj, double refminusactual) Beräknar pid-utsignalen tracking Denna undermodul beräknar rollreferensvinkeln samt pitchreferensvinkelhastigheten som Lq-regulatorn styr på, mer om detta står i avsnittet om styrsystemet. Användbara funktioner är double calculatephiref(trackingobjptr trackingobject,...,double psi) Beräknar vilken rollvinkel planet bör ha för att komma att peka på målet generate map Denna undermodul läser in kartan vid initiering och lägger ut den framför planet vid omslag till autonomt mode. Användbara funktioner är spreadmap(mapobjptr map, double x, double y, double z, double psi) Lägger ut bana.txt framför planet. Inläsningen av banan sker i readmap, vid skrivandet av denna tekniska dokumentation är antalet waypoints hårdkodade till fyra, om fler önskas så skall j <= 3 ändras till j <= x där x är antalet waypoints minus ett Lqcontrol Denna undermodul multiplicerar tillståndsvektorn med -L och referensvektorn med Lr och lägger ihop dessa för att få utsignalen från regulatorn. Användbara funktioner är calculatelqoutput(lqobjptr lqobject) Beräknar utsignalen från Lq-regulatorn.