Prof. Univ. D r. Ing. Nicu -George B îzdoacă cotutelă Drd. Ing. Dan Andr ițoiu IULIE 2019 CRAIOVA 2 UNIVERSITATEA DIN CRAIOVA FACULTATEA DE… [616195]

1

UNIVERSITATEA DIN CRAIOVA
FACULTATEA DE AUTOMATICĂ, CALCULATOARE ȘI
ELECTRONICĂ

DEPARTAMENTUL DE MECATRONICĂ

PROIECT DE DIPLOMĂ
George -Cătălin Cioacă

COORDONATOR ȘTIINȚIFIC
Prof. Univ. D r. Ing. Nicu -George B îzdoacă
cotutelă
Drd. Ing. Dan Andr ițoiu

IULIE 2019
CRAIOVA

2

UNIVERSITATEA DIN CRAIOVA
FACULTATEA DE AUTOMATICĂ, CALCULATOARE ȘI
ELECTRONICĂ

DEPARTAMENTUL DE MECATRONICĂ

SISTEM ROBOTIC AERIAN AUTONOM
George -Cătălin Cioacă

COORDONATOR ȘTIINȚIFIC
Prof. Univ. D r. Ing. Nicu -George Bzdoacă
cotutelă
Drd. Ing. Dan Andrițoiu

IULIE 2019
CRAIOVA

3
„Oamenii de știință visează să facă lucruri mărețe. Inginerii le fac. ”
James A. Michener
„Omul se află la granița dintre animal și robot.”
Michelle Rosenberg
„Tehnologia este natura om ului modern.”
Octavio Paz

4
DECLARAȚIE DE ORIGINALITATE

Subsemnatul Cătălin -George Cioacă , student: [anonimizat], Calculatoare și Electronică a Universit ății din Craiova, certific prin prez enta că am luat la
cunoșt ință de cele prezentate mai jos și că î mi asum, în acest context, originalita tea proiectului meu de
licență :
 cu titlul SISTEM ROBOTIC AERIAN AUTONOM ,
 coordonată de prof. Dr. Ing. Nicu Bîzdoacâ cotutelă Drd. Ing. Dan Andrițoiu ,
 prezentată în sesiunea iulie 2019 .
La elaborarea proiectului de licență, se consideră plagiat una dintre următoarele acțiuni:
 reproducerea exactă a cuvintelor unui alt autor, dintr -o altă lucrare, în limba română sau prin
traducere dintr -o altă limbă, dacă se omit ghilimele și referința precisă,
 redarea cu alte cuvinte, reformularea prin cuvinte proprii sau rezumarea ideilor din alte
lucrări , dacă nu se indică sursa bibliografică,
 prezentarea uno r date experimentale obținute sau a unor aplicații realizate de alți autori fără
menționarea corectă a acestor surse,
 însușirea totală sau parțială a unei lucrări în care regulile de mai sus sunt respectate, dar care
are alt autor.
Pentru evitarea acest or situații neplăcute se recomandă:
 plasarea într e ghilimele a citatelor directe și indicarea referinței într -o listă corespunzătoare la
sfărșitul lucrării,
 indicarea în text a reformulării unei idei, opinii sau teorii și corespunzător în lista de referin țe
a sursei originale de la care s -a făcut preluarea,
 precizarea sursei de la care s -au preluat date experimentale, descrieri tehnice, figuri, imagini,
statistici, tabele et caetera ,
 precizarea referințelor poate fi omisă dacă se folosesc informații sau teorii arhicunoscute, a
căror paternitate este unanim cunoscută și acceptată.
Data , Semnătura candidat: [anonimizat] ,

5

UNIVERSITATEA DIN CRAIOVA
Facultatea de Automatică, Calculatoare și Electronică

Departamentul de Mecatronică Aprobat la data d e
…………………
Șef de departament,
Prof. dr. ing.
Marius BREZOVAN /
Emil PETRE /
Dorian COJOCARU

PROIECTUL DE DIPLOMĂ

Numele și prenumele student: [anonimizat]/ –
ei:
George -Cătălin Cioacă

Enunțul temei:

SISTEM ROBOTIC AERIAN AUTONOM

Datele de pornire:

Quadcop terele sunt clasificate ca și aeronave cu aripi rotative
Spre deosebire de elicoptere, quadcopter de obicei folosesc
pale cu pas fix și simetric opus pe axele formate de rotoare.
Mișcarea unui quadcopter se realizează prin modificarea
turației rotoare lor modificând astfel tracțiunea și directia.

Conținutul proiectului:

Descriere Hardware
Descriere Software
Descriere GUI (Graphic User Interface)

Material grafic obligatoriu:
Desene de proiectare/asamblare sistem
Scheme electronice si de comand

Consultații:
zilnice
Conducătorul științific
(titlul, nume și prenume,
semnătura): Prof. Dr. Ing. Nicu Bîzdoacâ cotutelă Drd. Ing. Dan
Andrițoiu

Data eliberării temei:
01.12.2019

Termenul estimat de predare a
proiectului: 01.07.2019

6

Data predăr ii proiectului de către
student și semnătura acestuia:

7

UNIVERSITATEA DIN CRAIOVA
Facultatea de Automatică, Calculatoare și Electronică

Departamentul de Mecatronică

REFERATUL CONDUCĂTORULUI ȘTIINȚIFIC

Numele și prenumele candida tului/ -ei: Geor ge-Cătălin Cioacă
Specializarea: Mecatronică
Titlul proiectului : SISTEM ROBOTIC AERIAN AUTONOM
Locația în care s -a realizat practica de
documentare (se bifează una sau mai
multe din opțiunile din dreapta): În facultate □
În producție □
În cercet are □
Altă locație: [se detaliază ]

În urma analizei lucrării candidatului au fost constatate următoarele:
Nivelul documentării Insuficient
□ Satisfăcător
□ Bine
□ Foarte bine
□
Tipul proiectului Cercetare
□ Proiectare
□ Realizare
practică □ Altul
[se detaliază ]
Aparatul matematic utilizat Simplu
□ Mediu
□ Complex
□ Absent
□
Utilitate Contract de
cercetare □ Cercetare
internă □ Utilare
□ Altul
[se detaliază ]
Redactarea lucrării Insuficient
□ Satisfăcător
□ Bine
□ Foarte bine
□
Partea grafică, desene Insuficient ă
□ Satisfăcătoare
□ Bună
□ Foarte bună
□
Realizarea
practică Contribuția autorului Insuficientă
□ Satisfăcătoare
□ Mare
□ Foarte mare
□
Complexitatea
temei Simplă
□ Medie
□ Mare
□ Complexă
□
Analiza cerinț elor Insuficient
□ Satisfăcător
□ Bine
□ Foarte bine
□
Arhitectura Simplă
□ Medie
□ Mare
□ Complexă
□

8
Întocmirea
specificațiilor
funcționale Insuficientă
□ Satisfăcătoare
□ Bună
□ Foarte bună
□
Implementarea Insuficientă
□ Satisfăcătoa re
□ Bună
□ Foarte bună
□
Testarea Insuficientă
□ Satisfăcătoare
□ Bună
□ Foarte bună
□
Funcționarea Da
□ Parțială
□ Nu
□
Rezultate experimentale Experiment propriu
□ Preluare din bibliografie
□
Bibliografie Cărți
Reviste
Articole
Referințe web

Comentarii
și
observații

În concluzie, se propune:

ADMITEREA PROIECTULUI
□ RESPINGEREA PROIECTULUI
□

Data, Semnătura conducătoru lui științific,

9
REZUMATUL PROIECTULUI
Un multicopter este un vehicul aerian mecanic simplu, a cărui mișcare este controlată prin accelerarea
sau încetinirea m ai multor motoare/ propulsoare . Spre deosebire de elicoptere, multicopterele de obicei
folosesc pale cu pas fix și simetric opus pe axele formate de rotoare.
Termenii cheie :
FC = Flight Controller (Controler de zbor)
ESC = Electronic Speed Controller
GND = Ground
GPIO = General Purpose Input Output
GPS = Global Positioning System
LiPo = Lithium Pol ymer
PCB = Printed Circuit Board
PPM = Pulse Position Modulation
PWM = Pulse Width Modulation
Rx = Receiver
Tx = Transmitter
VTOL = Vertical Take Off and Landing
USB = Universal Serial Bus

10
MULȚUMIRI
Mulțumesc în special familiei, profesorilor si prietenilor care au ajutat, într -un fel sau altul, ca această
lucrare să fie dusă cu bine la final!

11
PROLOG

12
Cuprins
1 INTRODUCERE ………………………….. ………………………….. ………………………….. …………………… 18
1.1 SCOPUL ………………………….. ………………………….. ………………………….. ………………………….. ……………… 18
1.2 MOTIVAȚIA ………………………….. ………………………….. ………………………….. ………………………….. …………. 18
1.3 CE ESTE UN QUADCOPTER ? ………………………….. ………………………….. ………………………….. …………………… 26
2 QUADCOPTER – SCURT B REVIAR TEORETIC ………………………….. …………………………. 28
2.1 ELICEA ………………………….. ………………………….. ………………………….. ………………………….. ………………. 28
2.1.1 Principiu de funcționare ………………………….. ………………………….. ………………………….. ……………… 28
2.1.2 Elice de propulsive ………………………….. ………………………….. ………………………….. ……………………… 31
2.1.3 Modelarea matematică ………………………….. ………………………….. ………………………….. ……………… 33
3 DESCRIERE HARDWARE ………………………….. ………………………….. ………………………….. …… 37
3.1 CONFIGURAȚIA DE ZBOR ………………………….. ………………………….. ………………………….. ………………………. 37
3.1.1 Cadrul ………………………….. ………………………….. ………………………….. ………………………….. ………….. 38
3.2 MOTOARELE ………………………….. ………………………….. ………………………….. ………………………….. ……….. 40
3.3 ELECTRONIC SPEED CONTROLLER (ESC) ………………………….. ………………………….. ………………………….. …….. 42
3.4 CONTROLERUL DE ZBOR (FLIGHT CONTROLLER ) ………………………….. ………………………….. ………………………… 43
3.5 MICROCONTROLER PIXHAWK PX4 ………………………….. ………………………….. ………………………….. …………… 44
3.5.1 Caracteristici cheie: ………………………….. ………………………….. ………………………….. ……………………. 45
3.5.2 Senzori ………………………….. ………………………….. ………………………….. ………………………….. …………. 46
3.5.3 Interfețe ………………………….. ………………………….. ………………………….. ………………………….. ………. 46
3.5.4 Specificatii tehnice: ………………………….. ………………………….. ………………………….. ……………………. 46
3.5.5 Senzor: ………………………….. ………………………….. ………………………….. ………………………….. ………… 47
3.5.6 Interfață: ………………………….. ………………………….. ………………………….. ………………………….. ……… 47
3.5.7 Conectivitate ………………………….. ………………………….. ………………………….. ………………………….. … 47
3.5.8 Specificații processor ………………………….. ………………………….. ………………………….. ………………….. 47
3.6 ACCELEROMETRU /GYROSCOP IMU 6000 ………………………….. ………………………….. ………………………….. ….. 48
3.7 MAGNETOMETRU LSM303D ………………………….. ………………………….. ………………………….. ……………….. 49
3.7.1 Aplicații ………………………….. ………………………….. ………………………….. ………………………….. ……….. 49
3.8 MODUL GPS ………………………….. ………………………….. ………………………….. ………………………….. ……….. 50
3.9 MODUL TELEMETRIE 3DR 433M HZ ………………………….. ………………………….. ………………………….. ………… 51
3.10 ACUMULATORUL ………………………….. ………………………….. ………………………….. ………………………….. …… 52
3.11 ÎNCARCATORUL DE ACUMULATOARE ………………………….. ………………………….. ………………………….. . 54
3.12 TRANSMIȚĂTORUL ȘI RECEPTORUL RC ………………………….. ………………………….. ………………………….. ………. 55
3.13 FRSKY X8R 2.4G 16CH ………………………….. ………………………….. ………………………….. ……………………… 56
3.14 FRSKY 2.4GH Z ACCST TARANIS ………………………….. ………………………….. ………………………….. ………….. 57

13
4 ASAMBLAREA ………………………….. ………………………….. ………………………….. …………………….. 58
5 PROGRAMAREA ESC -URIL OR ………………………….. ………………………….. ………………………. 60
5.1 CALIBRAREA ESC-URILOR ………………………….. ………………………….. ………………………….. ………………….. 60
5.2 MONTAREA ESC-URILOR ………………………….. ………………………….. ………………………….. …………………… 61
6 DESCRIERE SOFTWARE ………………………….. ………………………….. ………………………….. ……. 62
6.1 PROGRAMAREA CONTROLER ULUI DE ZBOR (FLIGHT CONT ROLLER ) ………………………….. ………………………….. …… 62
6.2 CALIBRAREA ACCELEROME TRULUI ………………………….. ………………………….. ………………………….. ……………. 65
6.3 CALIBRARE MAGNETOMETR U ………………………….. ………………………….. ………………………….. …………………. 66
6.4 CALIBRAREA RADIOCOMEN ZII ………………………….. ………………………….. ………………………….. …………………. 67
6.5 MODURILE DE ZBOR ………………………….. ………………………….. ………………………….. ………………………….. .. 68
6.6 SETAREA REGULATOARELO R DE TIP PROPORȚIONA L INTEGRATOR DERIVAT IVE ………………………….. …………………… 70
6.7 ARMAREA MOTOARELOR ȘI PREGATIREA DE ZBOR ………………………….. ………………………….. ……………………… 71
7 DOMENIILE DE U TILIZARE ………………………….. ………………………….. ………………………….. . 73
7.1 AGRICULTURĂ ………………………….. ………………………….. ………………………….. ………………………….. ……… 73
7.2 FOTOGRAFIE AERIENĂ ………………………….. ………………………….. ………………………….. …………………………. 74
7.3 COMERCIAL E, INDUSTRIALE ȘI CONS TRUCȚII ………………………….. ………………………….. ………………………….. … 74
7.4 DRONE RACING & SPORTS ………………………….. ………………………….. ………………………….. ……………………. 75
7.5 LIVRAREA ȘI TRANSPORT AERIAN ………………………….. ………………………….. ………………………….. ……………… 75
7.6 COMBATEREA INCENDIILO R ………………………….. ………………………….. ………………………….. …………………… 76
7.7 FORȚELE MILITARE ȘI A RMATE ………………………….. ………………………….. ………………………….. ………………… 77
7.8 MAPARE ………………………….. ………………………….. ………………………….. ………………………….. ……………. 77
7.9 MINERIT ………………………….. ………………………….. ………………………….. ………………………….. …………….. 78
7.10 ALTE DOMENII DE ULILI ZARE A QUADCOPTERELO R: ………………………….. ………………………….. …………………….. 79
7.10.1 In industria medicală. ………………………….. ………………………….. ………………………….. …………….. 79
7.10.2 Apararea legii. ………………………….. ………………………….. ………………………….. ………………………. 79
7.10.3 Recreere, Hobby & Distracție. ………………………….. ………………………….. ………………………….. …. 79
7.10.4 Robotică și aplicații experimentale. ………………………….. ………………………….. ………………………. 79
7.10.5 Supraveghere și monitorizare. ………………………….. ………………………….. ………………………….. …. 79
7.10.6 Operațiuni de căutare și salvare. ………………………….. ………………………….. ………………………….. 79
7.10.7 Monitorizarea vieții sălbatice. ………………………….. ………………………….. ………………………….. …. 79
7.10.8 Divertisment. ………………………….. ………………………….. ………………………….. ………………………… 79
7.10. 9 Gradinărit. ………………………….. ………………………….. ………………………….. ………………………….. .. 79
7.10.10 Jurnalism. ………………………….. ………………………….. ………………………….. ………………………….. …. 79
7.10.11 Transportul de persoane. ………………………….. ………………………….. ………………………….. ………… 79
7.10.12 Topografie. ………………………….. ………………………….. ………………………….. ………………………….. . 79

14
7.10.13 Acoperire aeriana. ………………………….. ………………………….. ………………………….. …………………. 79
7.10.14 Expansiunea telecomunicaților. ………………………….. ………………………….. ………………………….. .. 79
7.10.15 Previziuni și monitorizare meteo. ………………………….. ………………………….. …………………………. 79
8 CONCLUZII ………………………….. ………………………….. ………………………….. …………………………. 80
8.1 CONCLUZII GENERALE ………………………….. ………………………….. ………………………….. ………………………….. 80
8.2 CONTRIBUȚII PERSONALE ………………………….. ………………………….. ………………………….. ……………………… 80
8.3 DEZVOLTĂRI VIITOARE ………………………….. ………………………….. ………………………….. …………………………. 81
9 BIBLIOGRAFIE ………………………….. ………………………….. ………………………….. ……………………. 82
10 REFERINȚE WEB ………………………….. ………………………….. ………………………….. ………………… 84
11 CODUL SURSĂ ………………………….. ………………………….. ………………………….. …………………….. 86
12 CD / DVD ………………………….. ………………………….. ………………………….. ………………………….. … 211
13 INDEX ………………………….. ………………………….. ………………………….. ………………………….. ……. 212

15
Figuri
FIGURA 1-1 PLANOR ………………………….. ………………………….. ………………………….. ………………………….. ……………… 18
FIGURA 1-2 HELICOPTER ORNITOPTER ………………………….. ………………………….. ………………………….. ……………… 19
FIGURA 1-3 PARAȘUTA ………………………….. ………………………….. ………………………….. ………………………….. …………… 19
FIGURA 1-4 CONCEPT ELICE COAXIAL E ………………………….. ………………………….. ………………………….. ……………………… 19
FIGURA 1-5 STUDIUL PASARILOR FAC UT DE OTTO LILENTHAL ………………………….. ………………………….. ……………………….. 20
FIGURA 1-6 ORVILLE ZBOARĂ LA KITTY HAWK 1903 ………………………….. ………………………….. ………………………….. …….. 21
FIGURA 1-7 AVIONUL „VUIA I” MARTIE 1906 ………………………….. ………………………….. ………………………….. ……………. 21
FIGURA 1-8 GYROPLANE ………………………….. ………………………….. ………………………….. ………………………….. …………. 22
FIGURA 1-9 GYROPLANE COAXIAL 1933 ………………………….. ………………………….. ………………………….. …………………… 23
FIGURA 1-10 SYKORSKY R-4 ………………………….. ………………………….. ………………………….. ………………………….. ……. 24
FIGURA 1-11 BELL H-47 ………………………….. ………………………….. ………………………….. ………………………….. ………… 25
FIGURA 1-12 IAR 330 PUMA SOCAT ………………………….. ………………………….. ………………………….. …………………….. 25
FIGURA 2-1 ELICE PERECHE CW-CCW ………………………….. ………………………….. ………………………….. …………………….. 29
FIGURA 2-2 CARACTERISTICILE ELIC EI ………………………….. ………………………….. ………………………….. ……………………….. 29
FIGURA 2-3 FARMAN MF.11 CU ELICE DE PROPULS IVE ………………………….. ………………………….. ………………………….. ….. 31
FIGURA 2-4 MODEL QUADCOPTER ………………………….. ………………………….. ………………………….. …………………………. 33
FIGURA 3-1 CONFIGURAȚII ZBOR ………………………….. ………………………….. ………………………….. ………………………….. .. 37
FIGURA 3-2 CADRUL ………………………….. ………………………….. ………………………….. ………………………….. ……………… 38
FIGURA 3-3 MONTARE ………………………….. ………………………….. ………………………….. ………………………….. …………… 38
FIGURA 3-6 ASAMBLARE PARȚIALĂ ………………………….. ………………………….. ………………………….. …………………………. 39
FIGURA 3-8 MOTOARE DC………………………….. ………………………….. ………………………….. ………………………….. ………. 40
FIGURA 3-9 MOTOR BRUSHLESS ………………………….. ………………………….. ………………………….. ………………………….. .. 41
FIGURA 3-10 ELECTRONIC SPEED CONTROLLER ………………………….. ………………………….. ………………………….. …………… 42
FIGURA 3-11 PIXHAWK PX4 PIX 2.4.8 ………………………….. ………………………….. ………………………….. ……………………. 44
FIGURA 3-12 GYROSCOP IMU 6000 ………………………….. ………………………….. ………………………….. ………………………. 48
FIGURA 3-13 LSM303D ………………………….. ………………………….. ………………………….. ………………………….. ………… 49
FIGURA 3-14 U-BLOX GPS ………………………….. ………………………….. ………………………….. ………………………….. ……… 50
FIGURA 3-15 TELEMETRIE 3DR 433M HZ………………………….. ………………………….. ………………………….. …………………. 51
FIGURA 3-16 ACUMULATOARE FOLOSITE ÎN ACEST PROIECT ………………………….. ………………………….. …………………………. 52
FIGURA 3-17 ISDT Q6 LITE ………………………….. ………………………….. ………………………….. ………………………….. …….. 54
FIGURA 3-18 FRSKY R9M TRANSMITTER ………………………….. ………………………….. ………………………….. ………………….. 55
FIGURA 3-19 FRSKY X8R 2.4G 16CH ………………………….. ………………………….. ………………………….. ……………………. 56
FIGURA 3-20 FRSKY TARANI S ………………………….. ………………………….. ………………………….. ………………………….. … 57
FIGURA 4-1 ANSAMBLU ………………………….. ………………………….. ………………………….. ………………………….. ………….. 58
FIGURA 4-2 ANSAMBLU ESC-BRAȚ CU FIRE ………………………….. ………………………….. ………………………….. ……………….. 58

16
FIGURA 4-3 ATAȘAREA BRAȚELOR IN FUNCTIE DE CULORI ………………………….. ………………………….. ………………………….. … 59
FIGURA 4-4 SURUB M3 INOX CU CAP IMBUS ………………………….. ………………………….. ………………………….. ……………. 59
FIGURA 5-1 ESC ………………………….. ………………………….. ………………………….. ………………………….. ………………….. 60
FIGURA 5-2 MONTAREA ESC-URILOR ………………………….. ………………………….. ………………………….. ………………………. 61
FIGURA 5-3 FIXAREA ESC PE BRAȚ ………………………….. ………………………….. ………………………….. ………………………….. 61
FIGURA 6-1 FEREASTRĂ DE CONFIGURARE FRAME ………………………….. ………………………….. ………………………….. ………… 62
FIGURA 6-2 CONECTORI PIXHAWK 2.4.8 ………………………….. ………………………….. ………………………….. ………………….. 62
FIGURA 6-3 MISSION PLANER ………………………….. ………………………….. ………………………….. ………………………….. …… 63
FIGURA 6-4 FC NECONECTAT ………………………….. ………………………….. ………………………….. ………………………….. ……. 64
FIGURA 6-5 WIZZARD DE CONFIGURAR E ………………………….. ………………………….. ………………………….. …………………… 64
FIGURA 6-6 CALIBRARE ACCELEROMET RU ………………………….. ………………………….. ………………………….. ………………….. 65
FIGURA 6-7 CALIBRARE MAGNETOMETR U ………………………….. ………………………….. ………………………….. …………………. 66
FIGURA 6-8 CALIBRAREA STAȚIEI RA DIO ………………………….. ………………………….. ………………………….. ……………………. 67
FIGUR A 6-9 MODURILE DE ZBOR ………………………….. ………………………….. ………………………….. ………………………….. .. 68
FIGURA 6-10 FEREASTRĂ SETĂRI PID ………………………….. ………………………….. ………………………….. ………………………. 70
FIGURA 6-11 POZIȚIA MANȘELOR PENT RU A ARMA COPTERUL ………………………….. ………………………….. ………………………. 71
FIGURA 7-1 AGRICULTURĂ ………………………….. ………………………….. ………………………….. ………………………….. ………. 73
FIGURA 7-2 FOTOGRAFIE AERIANĂ ………………………….. ………………………….. ………………………….. ………………………….. 74
FIGURA 7-4 DRONE RACING ………………………….. ………………………….. ………………………….. ………………………….. …….. 75
FIGURA 7-5 LIVRAREA ȘI TRANSPORT AERIAN ………………………….. ………………………….. ………………………….. ……………… 75
FIGURA 7-6 COMBATEREA INCENDIILO R ………………………….. ………………………….. ………………………….. ……………………. 76
FIGURA 7-7 FORȚELE MILITARE ȘI A RMATE ………………………….. ………………………….. ………………………….. ………………… 77
FIGURA 7-8 MAPARE ………………………….. ………………………….. ………………………….. ………………………….. …………….. 77
FIGURA 7-9 MINERIT ………………………….. ………………………….. ………………………….. ………………………….. …………….. 78

17

R S A A

SISTEM ROBOTIC AERIAN
AUTONOM

ROBOTIC SYSTEM AIR
AUTONOMY

18
1 INTRODUCERE
1.1 Scopul
Acest proiect a fost realizat cu scopul de a crea un sistem robotic aerian autonom, capabil să
execute auto-decolări, misiunile prestabilite și să auto -aterizeze, toat e acestea cu posibilitatea de
monitorizare și comandă de la o stație terestră.
1.2 Motivația
Una dintre cele mai mari și vechi dorințe ale omenirii a fost să zboare. Acest ideal este
consemnat și in mitologia antică, unde Icar împreună cu tatăl său Daedalus, a u construit labirintul din
Creta, în care regele Cretei, Minor, l -a închis pe Minotaur.
Bănuiți că l -ar fi ajutatpe Teseu să fugă atunci când venise să ucidă Minotaurulul , Dedal si Icar
au fost închiși în labirint de catre Minor.
Pentru a scăpa, au faurit niște aripi de pene și ceară și au zburat din labirint. Fascinat de
frumusețea înălțimilor, Icar s -a apropiat prea mult de soare, în ciuda sfatului tatălui său. Helios,
invidios ca oamenii pot zbura, a topit aripile lui Icar, iar acesta s -a prăbușit în Marea Egee, murind.
Pași mici au fost facuți de -a lungul timpului de multe minți inova toare ale istoriei. Leonardo
daVinci este cel care a fost fascinat cel mai mult de -a lungul întregii sale vieți de zbor. Leonardo a
studiat zborul păsărilor și a scris mai multe studii despre acesta, printre care Codexul asupra
zborului păsărilor din 1505. A conceput și
diverse aparate de zburat, printre care una cu
aripi batante (ornitopter) și o mașină de
zburat cu aripă rotativă (helicopter ).
Proiectele lui Leonardo pentru o parașută au
fost recreate cu mijloacele acelei epoci și
testate cu succes. Multe din proiectele lui
Leonardo s -au dovedit astfel fezabile.

Figura 1 -1 Planor

19

Figura 1-2 Helicopter Ornitopter

În jurul anului 1480, el a proiectat o parasuta de forma unei
piramide cu baza patrata, alaturi de car e a mentionat: "Dacă omul are
un cort de pânză scrobita, larg pe fiecare latura de 12 coți și înalt tot
de 12 coți, el va putea să -și dea drumul de la orice înălțime, fără să i
se întâmple nimic rău".

Figura 1 -3 Parașuta

În 1754 Mihail Lomo nosov realizează primul model cu elice
acționată de arc.

Figura 1 -4 Concept elice coaxiale

20

Au mai fost multe idei de -a lungul vremii de a realiza zborul, dar cel care a pus bazele
zborului modern a fost Otto Lilienthal, care în anul 1889 publică cart ea Der Vogelflug als Grundlage
der Fliegekunst („Zborul păsării ca baza a artei zborului”), care în prezent este cea mai importantă
publicație în tehnica zborului din secolul 19. Atenția publicului larg pentru carte a fost mică.
Companiile aeriene favoriza u principiul „mai ușor decât aerul”, ceeace ducea la dezvoltarea balonului
către dirijabil.
Și totuși Lilienthal a desemnat aceasta ca o aberație și a spus: Die Nachahmung des
Segelflugs muss auch dem Menschen möglich sein, da er nur ein geschicktes Steue rn erfordert, wozu
die Kraft des Menschen völlig ausreicht („Imitația planării, trebuie să fie posibilă și omului deoarece,
necesită numai o manevrare pricepută pentru care puterea lui este suficientă”).
Frații Lilienthal și -au dat seama că o formă curbată al aripilor este mai importanantă fiindcă
se creează o portanță mai mare decât forma plană. Forma caracteristică al aripilor la păsări a fost
observate și de alți „tehnicieni de zbor”, dar frații Lilienthal erau primi care au efectuat măsurători
exacte al acestora.

Figura 1 -5 Studiul pasarilor facut de Otto Lilenthal
Din nefericire, el si -a gasit sfirsitul pe 10 august 1896 in timp ce isi testa propriul lui aparat de
zbor, astfel fiind primul om care a realizat un zbor cu un aparat mai greu de cit aerul.
Dar visul s -a indeplinit de catre fratii Wright la data de 17 decembrie 1903 ca nd este efectuat
primul zbor cu motor al unui aparat mai greu decit aerul.

21

Construirea elicei creează o serie de greutăți, spre exemplu problema unghiului paletelor
elicei, care trebuia să ridice în aer un motor de 12 CP (cai putere) cu patru cilindri și răcire cu apă,
având o greutate de 110 kg.
În cele din urmă, după multe încer cări infructuoase, la 17 decembrie 1903, Orville reușește să
zboare timp de 12 secunde, pe o distanță de 37 de metri, cu o viteză de 10,8 km/h. Wilbur, la rândul
său, reușește să zboare în aceeași zi, de data aceasta timp de 59 de secunde, 260 metri cu o v iteză de
19 km/h.

Avionul lor, Kitty Hawk, avea lățimea
(inclusiv aripile) de 12,3 m, lungimea de 6,4 m și o
greutate de 340 kg, fiind construit exclusiv din lemn,
având aripile din pânză întinsă pe o ramă. În anii
următori frații Wright îmbunătățesc con tinuu tehnica
zborului prin perfecționarea elicei și dotarea avionului
cu un motor mai puternic, produs de Alberto Santos
Dumont în 1906 – 1907.

Figura 1 -6 Orville zboară la Kitty Hawk 1903

În 1905 apar primii pionieri europeni ai
zborului, Traian Vui a, care zboară în Paris, pe o distanță
de 12 m, în ziua de 18 martie 1906 și Jacob Christian
Hansen Ellehammer, care în septembrie 1906 zboară pe
insula Lindh olm pe o distanță de circa 40 m.
Dupa aproape un secol, compatriotului nostru
Traian Vuia, i -au fost recunoscute meritele sale aduse in
domeniul aeronauticii si anume, primul aparat de zbor
prezentat oficial la o expozitie internationala.
Figura 1 -7 Avionul „Vuia I” martie 1906
În ziua de 1 iulie 1902, el sosea la Paris, aducând în bagajele sale pro iectul unui original
„aeroplan -automobil”, conceput în perioada studenției, și macheta aferentă, realizată pe parcursul
ultimelor douăsprezece luni. În iarna lui 1902/1903, Vuia începe construcția aparatului, perfecționând
până în minime detalii planurile originale la care lucrase cu un an înainte la Lugoj. Se lovește din nou

22

de probleme de natură financiară, dar reușește să le depășească, ajutat și de mentorul său Coriolan
Brediceanu. În toamna lui 1904 începe să -și construiască și un motor, tot invenție p ersonală. În 1904
obține un brevet pentru această invenție în Marea Britanie. Întreaga parte mecanică e terminată în
februarie 1905. Aparatul este gata în decembrie, după ce i se montează motorul, și este numit Vuia I,
poreclit Liliacul, din cauza formei s ale. Avea prevăzută o greutate totală de 250 kg, o suprafață de
susținere de 14 m² și un motor de 20 CP. Primele experimente au început în 1905, ca pe un automobil,
cu aripile demontate, pentru a căpăta experiență în manevrarea lui.
Pe 18 martie 1906 la Mo ntesson, lângă Paris, aparatul Vuia I a zburat pentru prima dată.
După o accelerație pe o distanță de 50 de metri, aparatul s -a ridicat la o înălțime de aproape un metru,
pe o distanță de 12 m, după care palele elicei s -au oprit și avionul a aterizat.
Multe ziare din Franța, Statele Unite și Marea Britanie au scris despre primul om care a
zburat cu un aparat mai greu decât aerul, echipat cu sisteme proprii de decolare, propulsie și aterizare.
De atunci a fost scoasă în evidență și propagată ideea că Vuia a reușit cu aparatul său să decoleze de
pe o suprafață plată, folosind numai mijloace proprii, "la bord", fără "ajutor extern" (pantă, cale ferată,
catapultă, etc.). Totuși, au fost și mai există multe contradicții și dezbatere asupra definiției de primul
aeroplan.

Figura 1 -8 Gyroplane
În 1907 francezii Jacques si Louis Breguet au reusit sa creeze Gyroplane 1, un aparat ce s -a
ridicat de la sol și a staționat la 0,6 metri înalțime timp de un minut cu tot cu pilot. Aparatul era însă
foarte instabil așa încâ t a fost nevoie de câte un om la fiecare capăt al său pentru a -l ține în echilibru.
În același an un alt francez, Paul Cornu, a creat un aparat ce l -a ridicat pe realizatorul sau la
0,3 metri pentru 20 de secunde și, chiar daca nu a depășit performantele fratilor Breguet, acesta este

23

considerat primul elicopter și primul zbor al unui elicopter, nefiind ajutat în niciu n fel de martorii
evenimentului
Îmbunătățirile aduse aparatului și experimentele făcute în același an au dus la atingerea unei
înalțimi de ap roximativ 2 metri, Cornu a abandonat însă dezvoltarea aparatului, acesta
nemaiprezentând stabilitate la inaltimi mai mari și punând în pericol viața pilotului.
Însă fratii Jacques si Louis Breguet și -au continuat cercetările și Giroplan Laboratoire,
compan ia inființată de ei, a fost un timpuriu elicopter . Proiectat integral de Louis Breguet , au
experimentat deja cu giravioane în 1909, cu toate acestea, el a ales să se concentreze pe avioane până
la sfârșitul anilor 1920. În 1929 el a anunțat o serie de br evete care au abordat stabilizarea zborului
giravioanelor, și, în 1931, Breguet a creat Syndicat d'Etudes de giroplan ( din franceză pentru
"Syndicatul pentru Studiile giroplanoarelor"), împreună cu Rene Dorand ca director tehnic. Scopul lor
a fost dezvolt area unui elicopter experimental, giroplan Laboratoire.

Figura 1 -9 Gyroplane coaxial 1933

24
Abia din anii ’20 au început să apară modele capabile să asigure stabilitate, să se ridice mai
sus și să petreacă mai mult timp în aer. Datorită lipsei dezvoltării tehnologice necesare controlului
aeronavelor cu aripă mobila, a căror dinamică de zbor este de o complexitate superioara celor cu aripă
fixă, cercetările în acest domeniu au fost destul de lente. Însă, un cercetător de origine rusă, Igor
Sykor sky a reusit in anul 1942 sa pună bazele producției de serie a primului elicopter. Acest model a
fost Sykorsky R -4.

Figura 1 -10 Sykorsky R -4
Notorietatea acestui gen de aparat de zbor a cresct datorită proprietății sale de a decola și
ateriza pe verticală, prima lui utlizare militară a fost în timpul războiului din Koreea cănd a fost
folosit la evacuarea răniților din câmpul de luptă. Acest model a fost Bell H -47.

25

Figura 1 -11 Bell H -47
De asemenea țara noastră se numara pri ntre producătorii de astfel de aeronave militare, care
sunt produse la IAR Gimbav, cunoscută și ca IAR Brașov care este una dintre intreprinderile
aeronatice ale României, care produce elicopterul IAR 330 , elicopter de tip Puma , fabricat sub
licență.

Figura 1 -12 IAR 330 Puma SOCAT

26
1.3 Ce este un quadcopter ?
Un quadcopter, numit, de asemenea , un elicopter quad rotor sau multirotor elicopter , este
ridica t și propulsat de patru rotoare . Quadcopter urile sunt clasificate ca nave rotor , spre deosebire de
aeronave cu aripi fixe, deoarece ridicarea lor este generată de un set de rotoare (elice orientate
vertical ).
Quadcopter urile utilizează, în general, două perechi de elici identice fixe; două în sens orar
(CW) și două în sens invers acelor de ceasornic (CCW) . Acestea utilizează variația independentă a
vitezei fiecărui rotor pentru a obține controlul. Prin schimbarea vitezei fiecărui rotor este posibilă
generarea în mod specific a unei împingeri totale dorite ; pentru a localiza centrul de împingere atât
latera l cât și longitudinal și pentru a crea un cuplu total dorit sau o forță de întoarcere

Quadcopter urile diferă de elicopterele convenționale, acestea utilizează rotoare care sunt
capabile să modifice înclinarea lamelor lor dinamic în timp ce se mișcă în jurul butucului rotorului. În
primele zile ale zborului, quadcopter urile (denumiți apoi ca "quadcopter " sau pur și simplu
"elicoptere") au fost văzuți ca soluții posibile la unele dintre problemele persistente în zborul vertical.
Problemele legate de controlu l inductiv al cuplului (precum și problemele de eficiență provenite de la
rotorul coadă, care nu generează o ridicare utilă) pot fi eliminate prin contra -rotație, iar lamele relativ
scurte sunt mult mai ușor de construit. În anii 1920 și 1930 au apărut numeroase desene și modele.
Aceste vehicule au fost printre primele vehicule de lansare și aterizare verticale (VTOL) mai grele
decât aerul. Cu toate acestea, prototipurile timpurii au suferit din cauza performanței slabe , iar
prototipurile ulterioare au necesitat o sarcină prea mare de lucru pilot, din cauza unei augmentări slabe
a stabilității și a unei autorități de control limitate.
La sfarsitul anilor 2000, avansurile in domeniul electronicii au permis productia de controlăre
de zbor ieftine, accelerometr e (IMU), sisteme globale de pozitionare si camere de luat vederi. Acest
lucru a condus la popularilarea configurației quadcopter pentr u vehiculele aeriene fără pilot. Cu
dimensiunile mici și manevrabilitatea lor, aceste quadcoptere pot fi transportate atât în interi or, cât și
în aer liber.
La o mărime mică, quadcopter urile sunt mai ieftine și mai durabile decât elicopterele
convenționale datori tă simplității lor mecanice. Lamele lor mai mici sunt de asemenea avantajoase,
deoarece posedă mai puțină energie cinetică, reducând capacitatea lor de a provoca daune. Pentru
quadcopter urile la scară redusă, acest lucru face vehiculele mai sigure pentru o interacțiune strânsă.
Este, de asemenea, posibilă montarea dispozitive lor de protecție care închid rotoarele, reducând astfel
potențial ul de deteriorare. Cu toate acestea, pe măsură ce crește mărimea, quadcopter urile fixe cu elice
dezvoltă dezavantaje față de elicopterele convenționale. Mărirea dimensiunii lamelor crește impulsul

27
lor. Acest lucru înseamnă că modific ările vitezei lamelor durează mai mult, ceea ce are un impact
negativ asupra controlului. Elicoptrii nu se confruntă cu această problemă, deoarece mărirea
dimensiunii discului rotorului nu afectează în mod semnificativ capacitatea de a controla pasul
lamel or.
Datorită ușurinței lor de construcție și de control, aeronavele quadcopter sunt frecvent
utilizate ca modele de aeronave.
Am încercat să construiesc un multicopter performant cu costuri minime, de aceea am ales să
folosesc un cadru de quadcopter dator ită stabilității mărite a acestei platforme.
În capitolele următoare o să fie descrisă platforma experimentală realizată de mine, împreună
cu suportul teoretic minima care a condus l a realizarea unui quadcopter original.

28
2 QUADCOPTER – SCURT BREVIAR TEO RETIC
2.1 Elicea
Elicea (din g reacă helix, "spirală") organ mașină având forma unor aripi sau a unor lopeți
fixate pe un ax rotati v care servește la punerea în mi șcare a unui avion, a unei nave.
Descoperitorul sau inventatorul elicei care f uncționa este austriacul Joseph Ressel (1793 –
1857).
Elicea este un agregat care utilizează diferența de presiune dintre intradosul și extradosul
palelor acesteia care apare în mișcarea de rotație a elicei.

2.1.1 Principiu de funcționare

Palele sau aripioare le elicei sunt în așa fel amplasate, încât produc prin rotație unde asimetrice
de aer sau apă, prin aceasta iau naștere forțe de presiune și absorbție care determină la rândul lor
formarea unui curent în mediul respectiv.

Fiecare pală a elicei contribuie la acest efect motric de propulsie.

O elice după principiul de funcționare este inversul turbinei, prin faptul că cedează energie
mediului înconjurător pe când turbina preia energia pote nțială din mediul înconjurător.

Paletele au de obicei un profil (lat . fillum = fir) sau contur o față fiind convexă obligând
fluidul să efectueze o cale mai lungă ca și pe partea opusă, această diferență de viteză între cele două
părți creează efectul de sorb (sugere), intensitatea acestui efect poate fi reglată prin modif icarea
vitezei sau poziției paletelor elicei.

29

Elicele, alături de motoare, sunt cele mai
importante elemente ale unui multicopter. în funcție de
ele se aleg toate celelalte piese. Alegerea lor este un
pas foarte important și dificil deoarece un ca lcul
incorect poate duce la prăbușirea aeronavei chiar de la
primul zbor. Când se aleg elicele trebuie să se țineță
cont de 3 caracteristici: sens de rotație, diametru și pas.
Un quad copter folosește două tipuri de elice: elice ce
se rotesc în sens orar ( CW) și elice ce se rotesc în sens
trigonometric ( CCW ). Sensul rotației este dat de
înclinația palelor.

Figura 2 -1 Elice pereche CW -CCW
Mărimile ce caracterizează o elice sunt Diametrul (length ), Pasul ( pitch ) și constanța
specific Kp (este furnizată de producător).
Astfel, puterea necesară rotirii unei elice la un anumit număr de rotații pe min ut este
calculată :
] [*** ) ( RPMPD Kp wattP

Figura 2 -2 Caracteristici le elicei

30
În general, o elice cu pas mai mare va con suma mai mult curent. Pasul poate fi definit ca
distanța parcursă într -o rotație completă. Pe scurt, un pas mai mare înseamnă o rotație mai lentă, cuplu
mai mare și stabilitate sporită (recomandate pentru quad acoptere mai grele, cu cameră pentru
FPV/POV). Pentru acrobații sunt recomandate elice cu pas mic care dezvoltă accelerații mari. Eu am
optat pentru elice din policarbonat cu diametrul de 12 inch și pas 4.5.
Un motor eficient de 70% produce 70% energie și 30% căldură. Un moto r eficient de 90%
produce 9 0% energie și 10% căldură.
Cu motoare mai puțin eficiente nu numai ca se pierde o mulțime de putere (astfel timp d e
zbor), se obține, de asemenea o forță de tracțiune mai mică pe acceleraț ia completă. Acesta va avea
raspuns mai lent pentru a schimba [rpm] și lucru acest va influența stabilitatea quad copterului.
În general, crește rea pasul ui elicei și lungime va atrage un consum mai mare de curent. De
asemenea pitch poate fi definit ca distanța de deplasare în timpul unei rotații a elicei. Într -un cuvânt,
pas mai mare înseamnă rotație mai lentă, dar va crește viteza vehiculului, care utilizează, de
asemenea, mai multă putere.

31

2.1.2 Elice de propulsiv e

Figura 2 -3 Farman MF.11 cu elice de propulsive
Acest tip de avion p ermite pilotului să fie în fața elicei care în timp ul rotației va împinge
avionul.
Acest tip de elice l -a folosit pilotul francez Farman (1913), principiul fiind folosit de
avioanele militare britanice Airco D.H.1 și Airco D.H.2 cu scopul ca în față să fie posibilă folosirea
mitralierei, ulterior au fost introduse combinații dintre elicele de tracțiune și cele de propulsie
(Dornier Do 335 sau Cessna Skymaster).
Formula prin care se calculează deplasarea:
nDuJ
definește raportul dintre vit eza curentului axial
uA și viteza punctelor exterioare de
pecircumferința elicei
)(nD .
In contrast cu turbinele, la viteze de 700 km/h paletele elicelor de avion au un unghi mult
mare față de axă.
În matematică, elicea este o curbă strâmbă, deci situată într -un spațiu tridimensional, având o
propr ietate esențială care o deosebește de toate celelalte curbe strâmbe: are raportul dintre curbură și
torsiune constant. Cea mai simplă elice posibilă este definită prin următoarele trei ecuații scrise într –
un sistem de coordonate cartezian:

) cos(t x
,
)sin(t y
,

32

tz.
Atunci cand parametrul t crește, iar sistemul cartezian descrie ales este drept , punctul (x, y, z)
descrie o elice cu torsiunea pozitivă în jurul axei z.
În coordonate cilindrice
),,( hr , aceeași elice este descrisă d e ecuațiile:

1r
,
t
,
th
.
Toate celelalte elice se obțin din aceasta printr -o rotație, o translație sau o schimbare de scală.
Lungimea elicei generale definită într -un sistem de coordonate cartezian prin
ecuațiile:
), cos(t ax
),sin(t ay

,btz
cu
T t ,0 ,este data de
2 2b a TL  .
Curbura acestei elice generale este
2 2b aak
 ,iar torsiunea ei este
2 2b ab
 .

33
2.1.3 Modelarea matematică
Structura schematică a quadc opterului este exemplificata in figura de mai jos. Pentru a
descrie miș carea quad copterului sunt necesare doar doua sisteme de referintă: solul, care reprezintă
sistemul fix de referință și șasiul quadcopterului.

Figura 2 -4 Model Quadcopter
Mițcarea aeronavei este intotdeauna planifica tă folosind hărți geografice, de aceea este
folositor sa definim solul ca sistem fix de referinta tangent la suprafața Pamântului. Un astfel de
sistem de referinta este cel care utilizeaza coordonatele Nord, Est, Sud, Vest. Originea acestui sistem
de refer inta este localizat intr -un punct de pe suprafata solului si X, Y, Z sunt axele direcționate pe
directiile Nord, Est si in jos. Acest sistem fix de referinta al solului este vazut ca un cadru inertial in
care coordonatele liniare absolute (x,y,z) ale quad acopterului sunt definite. Cadrul mobil (X b, Y b, Zb)
este situat in centrul de greutate al quad copterului si orientat asa cum arata figura de mai sus. Pozitia
unghiulara a cadrului quad copterului in raport cu cel inerțial, este de obicei definit prin unghiu ri
Euler: roll roll φ, pitch θ and yaw ψ . Simplificind, deducem ca vectorul de pozitie inertial si vectorul
unghiului Euler prin of ξ = [ x y z]T și η = [ φ θ ψ]T . Transformarea de la cadrul quad la cel inertial
se realizeaza folosind matricea de rota tie R:

Care este ortoganală. In consecință transformata matricii de la cadrul inertial la cel al
quad copterului este R-1 = RT . vitezele unghilare de la cadru la cel inertial este

34
Legile de transformare sunt ν = W_η η ̇și η ̇= W_η^(-1) ν, în care vite za unghiulară ν este
definite prin vectorul vector ν= [p q r)]T. este important de observant că
poate fi definit doar
dacă
.
Prin urmare, dacă α este unghiul de rotație în jurul versorul u =
, este posibil să se
definească un cuaternion ca q =
T , cu
,
,
, Spre deosebire de unghiurile Euler, rotații cuaternion nu necesită un set de axe de
rotație predefinite, deoarece acestea se pot schimba continuu. Datorită f aptului că metoda de rotație în
jurul unei direcții arbitrare are o singură axă de rotație, grade le de libertate nu poate fi pierdut e; prin
urmare, blocarea cardanică nu poate avea loc. Transformarea reprezentării vitezele de translație din
cadrul corpului cu cea inerțială poate fi exprimat ă prin ξ = Qξ B, unde Q este matricea următoare:

Ca matricea R, Q este ortogonală; Prin urmare, este Q -1 = QT. Din punct de vedere al
vitezelor unghilare , transformarea implicat ă poate fi scris ă astfel : s:
,unde matricea S de pinde
de componentele cuaternare , după cum urmează:

unde masa m se presupune a fi constantă. Fiecare rotor i are o viteză unghiulară ωi care
generează o forță
, k fiind constanta de ridicare (lift), astfel forța totală (thrust ) TB =
T cu
.
Împingere totală, împreună cu forța gravitațională reprezintă forța totală care acționează
asupra quad copter ului,
.
Ca urmare, componenta de translație a mișcării referitoare la cadrul corpului quadcopterului
este

35

B+
.
În scopul de a obține aceeași ecuație cu privire la cadrul inerțial, să observă că forța centrifugă
este anulat ă deoarece cadrul inerțial nu se rotește și, astfel,

Quad copter -ul are o structură simetrică cu privire la axa XB, Y B și ZB, astfel, matricea
inerțială este o diagonală
. De aici rezulta ca totalul momentelor externe M,
rata de schimbare a momentului unghiular H = Iν este luat în considerare și momentul care acționează
asupra quad copterului este furnizat de relația
.
Mai mult decât atât, viteza unghiulară și accelerația rotorului creează un cuplu

în jurul axei rotorului, unde b este constanta glisare și I M este momentul de inerție al rotorului
i. Din structura geometric ă a quad copter ului și cel al componentele
and
peste cadrul corpului,
este posib il pentru a obține informații despre momentele roll, pitch și yaw clipă, și anume

Unde l este distanța dintre r otor și centrul de greutate al qua dcopter și and
reprezintă
derivata ωi(t) în raport cu timpul, adică .
. Ulterior, ecuația care guvernează din amica de
rotație pot fi rezumată după cum urmează

în care Γ reprezintă forțele giroscopice și τ B cuplul extern. După puțină algebră, rezultă

36

în care
. În caz contrar, ecuația poate fi scris ca

În final, odată ce viteza unghiulară a fost evaluată, accelerația unghiulară în
cadrul inerțială poate fi ușor dedus ca
.

37

3 DESCRIERE HA RDWARE
3.1 Configurația de zbor
Configurația de zbor exemplifică modul în care sunt dispuse motoarele precum și la setul
de piese alese pentru a garanta încadrarea în parametrii de zbor.
In cazul quadcopterelor există două configurații "standard" din punct de vedere al
dispunerii rotoarelor: -configurație de tip “X”
-configurație de tip “+”

Figura 3 -1 Configurații zbor
În capitolele următoare vor fi prezentate succesiv piesele componente selecționate alături de
informații generale și indici de performanță ce trebuie urmăriți atunci când se achizitioneaza
elementele componente ale quad copterului / multicopterului.
În aceasta configur atie am obținut un timp mediu de zbor de 23 minute.

38

3.1.1 Cadrul
Primul pas făcut în proiectarea multicopterului este
alegerea cadrului (frame) .
Cadrul unui quadcopter este structura principală sau
scheletul pe care vor fi atașate restul c omponentelor. După ce sa
decis configuratia (fotografiere aeriene, curse, m icro-freestyle
etc.), se decide ce dimensiune se potrivește cel m ai bine cerințelor .
Materialele cele mai folosite sunt aluminiul, lemnul și fibra de
carbon. Cele mai importante pro prietăți a le frame -ului sunt
robustețea și capacitatea de a absorbi vibrații. Aluminiul este
reziste nt și ușor însă este ineficient împotriva vibrațiilor.

Figura 3 -2 Cadrul
Lemnul prezintă proprietăți foarte bune de absorbție, este
relativ ușor, însă fragilitatea lui îl face eficient doar dacă este
folosit pentru piesa centrală a cadrului. Fibra de carbon este cel
mai bun material, având pro prietăți mecanice excepționale. Se
recomanda utilizarea tuburi lor din fibră de carbon pentru brațele
quad copter ului deoarece țesătura are o dispunere î n X, lucru ce
sporește rezistența dar și capacitatea de amortizare a vibrațiilor
produse de motoare. Singurul dezavantaj al acestui material high –
end îl reprezintă costul ridicat. Din cauza bugetului limitat,
optiune ideala a fost o co mbinație între brațe din profil de
aluminiu de 15mm și pentru partea centrală sticlote xtolit .
Figura 3 -3 Montare
Placile din sticlo -textolit (sticlostratitex) sunt realizate prin laminare si au in compozitie
tesatura de sticla impregnata cu rasina fenolica. Materialul este rezistent la temperaturi ridicate (pana
la 155°C) si are exce lente calitati mecanice si electrice, fiind un bun izolator electric.
Sticlotextolitul poate fi de asemenea prelucrat mai usor ca alte materiale, iar datorita
rezistentei sale ridicate la temperatura, poate înlocui laminatele rigide pe baza de rasna silic onica.
Quadcopterul din proiect es te de tip X , avantaj ul acestuia față de cel de tip + fiind stabilitatea
și accelerația mai mare, precum și o mai bună vizibilitate deoarece ”fața” nu este obstrucționată de
una din elice.

39

Figura 3 -4 Brat initial și asamblat complet Figura 3 -5 Cadru mon tat
Figura 3-6 Asamblare parțială Figura 3 -7 Finisare mecanică

40

3.2 Motoarele
Scopul motoarelor electrice este de a converti energia electrică în energi e mecanică sub forma
mișcării de rotație. Aceste sisteme electromecanice sunt caracterizate de anumiți parametrii care ajută
la prezicerea performanțelor motorului. Viteza în absența sarcinii descrie viteza motorului când nu
este cuplu aplicat, iar viteza de mers în gol descrie momentul de cuplu maxim al motorului când
viteza acestuia este de 0 RPM (revolutions per minute). Alți parametrii de interes sunt cuplul, curentul
și tensiunea de alimentare a motorului. Modul de funcționare al acestor motoare este s implu, cuplul
fiind creat de forța de magnetism apărută între magneți staționari și electromagneți alimentați astfel
încât polaritatea dintre cele două componente să fie mereu inversă. Motoarele brushed (cu perii) , deși
au avantajul unui preț mai mic, au e lectromagneții plasați pe rotor și magneții ficși pe stator, de aceea
sunt predispusi supraîncălzirii, dar și uzurii din cauza comutatoarelor. La motoarele brushless,
electromagneții sunt plasați pe stator și magneții staționari pe rotor. Polaritatea elect romagneților este
schimbată de data aceasta de un circuit de control care îmbunătățește precizia motorului.

Figura 3 -8 Motoare DC
[RPM ]= KV * V
KV = rotații/volt, V= tensiunea de alimentare
Factorul KV este folosit pentru a descrie capacitățile unui motor. După cum se observă, la
aceeași tensiune este proporțional cu numărul de rotații pe minut (RPM). Tipul de motor trebuie
adaptat tipului de quadcopter dorit. O dronă de mare performanță, acrobatică, va avea un motor de
viteză mare. Pe de altă pa rte, un quadcopter folosit în zboruri lungi, trebuie să consume cât mai puțină
energie, de aceea motoarele cu [RPM] mic sunt de dorit în această situație.
Motoarele sunt scurgerea principală a energiei bateriei de pe quad copter , prin urmare,
obținerea unei combinații eficiente de elice și motor este foarte importantă. Vit eza motorului este

41

evaluată în KV, în general un motor K V mai mic v a produce mai mult cuplu și un K V mai mare se va
reflecta in crșterea turației .

Tracțiune totală = Masă totală * 2
Tracți une motor = Tracțiune totală / 4

Datorita raportului pret/calitatea a fost ales motorul A2212 1000KV ( Figura 3 -9) care
are următoarele specificații:

Model: A2212 10T / 13T
Motor KV (RPM / V): 1000
Eficiență maximă: 80%
Capacitate ac tuală: 12A / 60S
Curent fără sarcină (A): 0,5A @ 10V
Max. Eficiență Curent: 4 -10A
Valori maxime (W): 150
Rezistența internă (mΩ): 90
Poli: 14
Diametrul axului (mm): 3.17
Baterii LiPO: 2S -3S
Lungime (mm): 27,5
Lățime (mm): 27
Greutate (gm): 64
Figura 3 -9 Motor Brushless
Motoarele alese nu sunt cele mai performante, au un raport calitate/preț foarte bun. Pentru
randament maxim se pot folosi motoare Brushless cu rulmenți ceramici.

42

3.3 Electronic Speed Controller (E SC)
Electronic Speed Control (E SC) este un circuit ce îndeplinește două funcții principale. Prima
este de a acționa asemenea unui regulator de tensiune, reducând tensiunea furnizată de baterie până la
valoarea necesară funcționării motoarelor și microcontrollerelor. A doua funcție este de a converti
semnalul de la flight controller într-un semnal de control pentru motorul brushless.

Figura 3-10 Electronic Speed Controller
Spre deosebire de speed controllerele bazate pe rezistori, ESC -urile nu funcționează aplicând
motoarelor fracțiuni din tensiunea de alimentare proporționale cu viteza pe care o dorim, ci aplicând
intreaga tensiune, dar oprind -o si pornind -o extrem de rapid. În cazul celor apărute primele, diferența
de tensiune nefolosită pentru mișcarea motorului ar fi fost i rosită, fiind disipată sub formă de căldură.
Variind raportul dintre timpii de „ON” (tensiune maximă) și „OFF” (tensiune de 0V), ESC -ul variază
de fapt tensiunea medie citită de motor. Cu alte cuvinte, la orice moment de timp, tensiunea de control
este ori complet „ON” ori complet „OFF”. Aceasta face ca acest tip de control să fie, cel puțin
teoretic, 100% eficient. În realitate totuși, ESC -urile sunt dispozitive imperfecte, eroarea acestora
datorându -se în principal timpului de comutare al tranzistoarelor folosite în circuit și rezistențelor
interne.
BEC (Battery Eliminator Circuit ) este circuitul ce regulează tensiunea furnizată de baterie.
Este extrem de folositor în aplicații de quadcopter, deoarece bateriile suplimen tare ar crește greutatea
dronei

43
3.4 Cont rolerul de zbor (Flight Controller)

Controle rul de zbor (Flight Controller ) este creierul unui quadcopter, cu ajutorul senzori lor de
la bord poate înțelege cum se deplasează acesta . Folosind datele furnizate de acești senzori, FC
folosește algoritmi pentr u a calcula cât de repede fiecare motor ar trebui să se rotească pentru ca
quadcopterul să se comporte așa cum pilotul comandă prin intermediul intrărilor de tip stick pe TX
(Transmițătorul Radio). Majoritatea cablurilor de pe quadcopter vor fi l ocalizate în jurul FC. Acesta
trebuie să aibă conexiunea RX (receptorul), astfel încât să se poat ă spune ce vrea pilotul să
facă. Odată cu introducerea BetaFlight OSD (afișare pe ecran), chiar fluxul video de la camera FPV
trece prin FC la VTX (transmițător video).
Un quadcopter tipic vine cu un controler cu 4 canale , acest controler central de zbor trimite
această informație la controlerele electronice de turație (ESC) ale fiecărui motor care, la rândul lor,
dirijează motoarele pentru a crește sau a reduce viteza.
Frecvența de comunicație utiliz ată de majoritatea controlerelor este de 2,4 GHz. Aceasta este,
de asemenea, frecventa tipica f olosita pentru conexiunile WiFi.
Controlerul de zbor central, de asemenea, ia informații de la IMU, Gyroscope, module GPS și
senzor i de detectare a obstacolelor dacă este pe quadcopter. Efectuează calcule utilizând parametri de
zbor programați și algoritmi, apoi trimite aceste date controlorilor de viteză electronici (ESC) .
De fapt, majoritatea controlere lor de zbor cuprind IMU, GPS, G yroscope și multe alte
caracteristici pentru a controla zborul și stabilitatea quadcopter. În multe cazuri, acestea au dublu IMU
pentru redundanță și alte caracteristici de siguranță, cum ar fi Return -To-Home.
Un exemplu de controler de zbor cent ral ar fi noul Pixhawk PX4 . Are o multitudine de
caracteristici și poate lucra cu o varietate de motoare.

44

3.5 Microcontrol er Pixhawk PX4
Controlerul Pixhawk (FMUv2) a fost lansat din sistemul original "PX4", care constă din
PX4FMU și diferite plăci de tip piggyb ack, inclusiv PX4IO și PX4IOAR
Figura 3 -11 Pixhawk PX4 PIX 2.4.8
Sistemul de management al zborului PX4FMU include:
 PX4-FMU (unitatea de gestionare a zborului).
 Un microcontroler Cortex M4F puternic și memorie flash pentru control ul zborului și
comunicațiilor.
 Un soclu pentru memorie SD.
 Giroscoap cu 3 axe pentru determinarea orientării.
 Un accelerometru cu 3 axe pentru determinarea influențelor exterioare.
 Un compas (magnetometru).
 Un senzor de presiune barometric pentru determinarea altitudinii.
 Conexiune pentru un UBLOX LEA GPS ce poate fi montat extern pentru a determina poziția
absolută.
 Interconexiuni de placă interconectabile pentru adăugarea diferitelor plăc i periferice.
 Interfețe de comunicații pentru conexiunile USB, JTAG și Serial.
 Conexiuni pentru intrarea radio PPM -SUM RC și ieșirile servo. \

45
3.5.1 Caracteristici cheie :
Principal sistem -pe-chip: STM32F427
CPU: 180 MHz ARM ® Cortex ® M4 cu FPU cu o singură prec izie
RAM: 256 KB SRAM (L1)
Sistemul de protecție împotriva defecțiunilor pe sistem: STM32F100
Procesor: 24 MHz ARM Cortex M3
RAM: 8 KB SRAM
WiFi: ESP8266 extern
GPS: U -Blox ® 7/8 (Hobbyking ® ) / U -Blox 6 (Robotics 3D)
Flux optic: unitatea de flux PX4
Redu ndante surse de alimentare și failover automat
Comutator de siguranță extern
Indicator vizual principal LED multicolor
Indicator audio cu piezo, cu mai multe tensiuni
Card microSD pentru înregistrări rapide pe perioade lungi de timp .

46

3.5.2 Senzori
 ST Micro Mic ro L3GD20H giroscop cu 16 biți
 ST Micro Micro LSM303D Accelerometru / magnetometru cu 14 biți
 Inversense MPU 6000 accelerometru / giroscop cu 3 axe
 MEAS MS5611 barometru
3.5.3 Interfețe
 5x UART (porturi seriale), un capabil de mare putere, 2x cu control al
fluxu lui HW
 2x CAN (unul cu transmițător intern 3.3V, unul cu conector de extensie)
 Spektrum DSM / DSM2 / DSM -X® Intrare compatibilă prin satelit
 Futaba S.BUS® de intrare și ieșire compatibile
 Intrare semnal sumă PPM
 RSSI (PWM sau tensiune) de intrare
 I2C
 SPI
 3.3 și 6.6V intrări ADC
 Port microUSB intern și extensie port microUSB extern
3.5.4 Specificatii tehnice:
 Placa se integrează PX4FMU + PX4IO
 Pixhawk este dotat cu o nouă tehnologie a cipurilor și senzorilor cu 32 de biți
 Procesor: 32 biți 2M memorie flash ST M32F4 27 Cortex M4 cu unitate de
procesare cu puncte în virgulă mobile
 Frecvența principală: 256K, 168MHZ RAM
 32 biți STM32F103 coprocesor de rezervă

47
3.5.5 Senzor:
 L3GD20 Giroscop cu 16 biți digitale pe 16 bi ți
 LSM303D accelerometru / magnetometru pe 14 biți pe 14 biți
 MPU6000 accelerometru / magnetometru cu 6 axe
 MS5611 barometru de înaltă precizie
3.5.6 Interfață:
 5 * UART, 1 * compatibil de înaltă tensiune, 2 * de control al fluxului
hardware
 DSM / DSM2 / DSM -X intrări compatibile cu receptorul prin satelit
 Futaba SBUS intrare și ieșire
 Intrare semnal PPM
 RSSI (PWM sau tensiune) de intrare
 3,3 și 6,6 VADC
 Interfață USB externă MICRO
3.5.7 Conectivitate
 1x I2C
 1x CAN (2x opțional)
 1x ADC
 4x UART (2x cu control al debitului)
 1x Consola
 8x PWM cu suprasarcină manuală
 6x intrare PWM / GPIO / PWM
 Intrare S.BUS / PPM / spectru
 SIBUS de ieșire
3.5.8 Specificații processor
 32bit STM32F427 Miez Cortex -M4F cu FPU
 168 MHz
 256 KB RAM

48

3.6 Accelerometru/gyroscop IMU 6000

Familia MPU -6000/MPU -6050 ™ sunt
primele și singurele dispozitive din lume 6 -axe
MotionTracking concepute pentru putere mici,
low-cost, precum și ce rințele de înaltă performanță
la smar tphone -uri, tablete și senzori ușor de purtat .

Figura 3 -12 Gyro scop IMU 6000
Dispozitivele combină un giroscop pe 3 axe și un accelerometru cu 3 axe împreună cu un
bord digital Motion Procesor ™ (DMP ™ ) capabil de prelucrare complexă a 9 axe.
Dispozitivele sunt oferite în același 4x4x0.9 mm QFN și pinout oferind o cal e simplă de
upgrade și făcându -l ușor pentru a se potrivi pe spațiu l limitat al placi .
Funcțiile suplimentare includ un senzor de temperatură încorporat și un oscilator on -chip cu ±
variație de 1% față de intervalul de temperatură de funcționare.
Interfa ț de mișcare devine rapid o funcție cheie în mai multe dispozitive electronice de larg
consum, inclusiv smartphone -uri, tablete, console de jocuri, și smart -TV, deoarece oferă un mod
intuitiv pentru consumatori de a interacționa cu dispozitivele electronic e de urmărire a mișcari în
spațiu l iber.
Platforma InvenSense MotionApps ™, rezumă complexitatea MPU 6050, offloads
management și oferă un set structurat de API -uri pentru dezvoltarea de aplicații.
MPU -6000/MPU -6050 ™ dispune d e urmărire a mișcări lor rapid e și lente, un giroscop
programabil din gama de ± 250 pe scară largă, ± 500, ± 1000, și ± 2,000 ° / sec (DPS) și un
accelerometru programabil de utilizator pe sc ară largă g ama de ± 2g, 4g ±, ± 8g, și ± 16g.

49

3.7 Magnetometru LSM303D
LSM303D este un si stem tip pachet care include un
senzor de accelerație digital 3D și un senzor magnetic
digital.

LSM303D are scale de accelerare liniară completă
de ± 2g / ± 4g / ± 6g / ± 8g / ± 16g și magnetic ă de ± 2 / ± 4
/ ± 8 / ± 12 gauss.

Figura 3 -13 LSM303D
LSM 303D in clude o magistrală de serie I2C interfață care acceptă modul standard și rapid
(100 kHz și 400 kHz) și standardul serial interfață SPI.
Sistemul poate fi configurat pentru a genera un semnal de întrerupere în cădere liberă,
detectare a mișcării și d etectarea câmpului magnetic. Pragurile generatoar e de întrerupere sunt
programabile de catre utilizator.
Blocurile magnetice și accelerometrul pot fi activat e sau pus e în modul de pornire separat.

LSM303D este disponibil într -o capsula de plastic (LGA) și este garantată funcțion area pe un
interval de temperatură extins de la -40 ° C la +85 ° C.
3.7.1 Aplicații
 Busole compacte cu înclinare
 Orientarea ecran
 Detectarea poziției
 Funcții activate de mișcare
 Detectarea caderii libere
 Pedometre
 Economie inteligentă de energie pentru dispozitive portabile
 Gaming și dispozitive de realitate virtuală
 Identificarea impactului
 Monitorizarea vibrațiilor și compensarea

50

3.8 Modul GPS
Seria NEO -6 este o familie de stand -alone
receptoare GPS. Aceste receptoare flexibile și efic iente
oferă numeroase opțiuni de conectivitate într -o
miniaturală carcasa de 16 x 12,2 x 2,4 mm . Arhitectura
lor compactă și de putere conferă opțiuni de a face din
modulul NEO -6 modulul ideal pentru dispozitive

Figura 3 -14 u-blox GPS
Motorul de poziți onareal U -blox-6 folosește 50 -canale și are un Time -To-First-Fix (TTFF) de
sub o secundă. Are motor dedicat de achiziție, cu 2 milioane de corelări pe secundă, este capabil de
căutări paralele în spațiu -timp-frecvență multiple, permițându -i să găsească sat eliți instantaneu.
Designul și tehnologia inovatoare suprimă sursele de bruiajele și atenuează efectele pe căi multiple,
oferind receptoarelor NEO -6 GPS performanțe excelente de navigare, chiar și în cele mai dificile
medii.

51

3.9 Modul teleme trie 3DR 433Mhz
Acest set de telemetrie radio permite să se lege
de un controlor de zbor, cum ar fi APM sau Pixhawk
prin intermediul unui dispozitiv echipat cu un port
USB, cum ar fi un calculator, laptop sau tabletă dotată
cu o conexiune USB (OTG). Acesta permite nu numai
să se vizualizeze datele în timp real, cum ar fi poziția
GPS în direct suprapusă pe o hartă (în cazul în care
controlorul de zbor are opțiunea GPS) pentru tensiunea
sistemului, poziția, punct de referință navagatie, orizont
artificial, ch iar și mult mai mult, folosind software
open source bazate pe
Figura 3 -15 telemetrie 3DR 433Mhz
MAVlink e ideal pt o stație de la sol (exemplu Pixhawk ). Exista si posibilitatea să se zboare chiar și
aeronave cu un joystick prin intermediul computerului și setările de actualizare live! Sau poate fi
folosit in orice proiecte DIY, care are nevoie de o interfață serială wireless.

Acest kit oferă un preț scăzut mare, interval mai lung (aproximativ o mila cu antene furnizate) și
performanțe superioare. Sistemul utilizează banda de 433Mhz și furnizează un link full -duplex,
folosind modulele HopeRF de la HM -TRP.

Firmware -ul include un bootloader care permite upgrade -uri firmware de radio prin interfața
serială, și configurarea parametrilor sistemului. Upgrade -uri de firmware și de configurare sunt pe
deplin susținute în Planner Mission. Configurarea este, de asemenea, posibilă prin intermediul
configuratorului 3DR Radio și comenzi AT.
Interfață pentru modulul este standard de -5V tolerant TTL serial / FT232 serial USB.

52

3.10 Acumulatorul
Există numeroase tipuri de
acumulatori, însă doar unul singur dintre
ele face față provocărilor impuse de astfel
de drone: LiPo (Lythium Polymer).
Deoarece LiPo au cel mai bun raport W/g
(Watt/gram) și pot debita curent de pâna
la 50 ori mai mare decât cel de incarcare.
Acumulatorii LiPo sunt formați, de
obicei, din celule de 3.7V conectate în
serie producând o tensiune mai mare, însă
curentul debitat este constant.
Figura 3 -16 Acumulatoare folosite în acest proiect
Când se alege un acumulator, trebuie luate în considerare mai multe aspecte: tensiunea
nominală, rata de descărcare, capacitatea acumulatorului. E de dorit să se găsească varianta cu cea
mai redusă masă. Zippy Compact sunt o alegere bună în acest sens: oferă aceleași perfo rmanțe ca și
variantele standard, iar greutatea este mai mică. Tensiunea nominală se calculează în funcție de
numărul de celule legate în serie din acumulator. Această valoare este de multe ori inscripționată și pe
eticheta produsului, în cazul în care sun t menționate doar numărul de celule legate în serie (NS) și în
paralel.
Putem folosi formula de mai jos pentru a obține tensiunea de la bornele acumulatorului:
Uacumulator (V)= NS * 3.7V
NS este inscripționat pe etichetă astfel: 2S, 3S,4S etc. În cazul în care avem de -a face cu o
notație de formă 2S2P înseamnă că celulele sunt legate în serie 2 câte 2, iar grupările astfel formate
sunt dispuse în paralel. Astfel se obține un acumulator ce poate oferi un curent maxim mai mare decât
un acumulator 2S1P cu acel eași specificații.
Exemplu : LiPo 1 cu specificațiile 2S1P 5000mAh 20 C va suporta un curent maxim de 100A.
LiPo 2 cu specificațiile 2S2P 5000mAh 20 C va suporta un curent maxim de 200A.
Să detaliez cum au fost calculate valorile curenților în exemplul prec edent, dar mai întâi
trebuie să menționez ce este rata de descărcare. Apare pe eticheta acumulatorilor ca o valoare
numerică urmată de litera C (pentru acumulatorul din Figura 8 rata de descărcare este 25C). Este unul

53
dintre cei doi indicatori ai valorii c urentului maxim ce poate fi "tras" din acumulator. Celălalt
indicator îl reprezintă capacitatea LiPo -ului. O puteți identifica ușor deoarece este valoarea notată cu
fontul cel mai mare, tocmai pentru a fi ușor lizibilă. Pe unele etichete vine împreună cu u nitatea de
măsură și anume mAh (miliAmperi/oră). Capacitatea mai face referință și la durata de funcționare în
standarde nominale a acumulatorului între două încărcări succesive (dacă avem un LiPo de 5000mAh
și îl folosim să alimentăm un circuit care trage 500mA, atunci ne putem aștepta la aproximativ 10 ore
de funcționare continuă a ansamblului). Curentul maxim pe care îl poate debita un acumulator LiPo
poate fi calculat folosind formula:

Curent Maxim = Capacitate * Rată Descărcare

De exemplu, pentru acu mulatorul din Figura 3 -16 care este de 5000 mAh și 25C curentul
maxim constant este:

lmax= 5800 mAh * 25C = 125 A

Acumulatorul se alege în funcție de necesitățile motoarelor. Astfel, pentru motoarele alese de
mine (care am stabilit că pot "trage" și cure nți de 23A) am avea nevoie de un Lipo care să reziste la

ILIPO = lmotor – peak*Nr Motoare = 23*6 = 138 A

Cum I max este 138A avem certitudinea că acumulatorul va face față chiar și atunci când
motoarele sunt rulate la maxim.

54

3.11 ÎNCARCATORUL DE ACUMULATOA RE
Special conceput pentru sistemul de
încărcare inteligent, acest încărcător mic are o
capacitate dublă de alimentare a majorității
încărcătoarelor litere. Dispunând de un ecran
LCD de 2,4 "cu ecran de auto -luminozitate și
un unghi vizibil de 178 °, ecra nul este încă
vizibil chiar și sub lumina puternică a soarelui.

Figura 3 -17 iSDT Q6 Lite
iSDT a redus mărimea încărcătorului Q6 cu 50% și a introdus un nou design rotund care se
potrivește cu ușurință în buzunar sau geantă. Dispunând de un sistem de răcir e extrem de eficient, o
construcție internă optimizată și o tehnologie digitală de alimentare cu energie, acest încărcător este
perfect pentru toate necesitățile de încărcare.
Dispunând de o acuratețe crescută, o viteză sporită de 20 de ori și un control a l închiderii
metalice cu o singură cheie pentru a crea o experiență de încărcare mai raționalizată.
Caracteristici:
• ARM Cortex -M4
• 20x viteză îmbunătățită
• Afișaj IPS de 2,4 "cu auto -luminozitate
• Ventilatorul cu rulment de mare viteză crește creștere a răcirii cu 300%
• Procesor ARM cu viteză mare de 32 biți
• Actualizați firmware -ul utilizând calculatorul personal.

55

3.12 Transmițătorul și Rece ptorul RC

Pentru acest proiect s -a optat la alegerea
modulului FrSky R9M Transmitter, acesta oferă
o soluție simplă și necostisitoare pentru acele
pliante care doresc să -și păstreze investițiile în
transmițător lor 900 MHz existente, în timp ce se
de beneficii semnificative și avantajele fără
interferențe operare de 2,4 GHz.

Figura 3-18 FrSky R9M Transmitter
Sistemul de control radio R9M este primul sistem cu rază lungă de acțiune FrSky care
funcționează la frecvența de 900MHz. Modulul R9M oferă 4 ieșiri de putere RF comutate astfel încât
să puteți alege puterea corespunzătoare pentru diferite situații de zbor. Î n comparație cu un alt
receptor FrSky de 2,4 GHz, precum receptorul L9R, receptorul R9 are o durată de funcționare mai
mare de până la 10 km și mai mult. Mai mult decât atât, R9 este, de asemenea, un receptor de
redundanță care se poate conecta la un alt r eceptor pentru a asigura securitatea aeronavei
dumneavoastră.
Specificație:
Modul transmițător R9M 2019 900MHz
Tensiune Vin: 6.5V -13V
Alimentare externă: 6.5V ~ 13V
Interfața de telemetrie: port inteligent
Interfață de upgrade: port intelligent
Modulări: PXX sau CPPM (detectată automat)
Frecvența de operare RF: 915MHz
Compatibilitate: Receptoare din seria R9

56

3.13 FrSky X8R 2.4G 16CH
Receptorul X8R comunică cu un modul X8 prin
spectrul 2.4ghz, oferind o performanță fără
preceden t și o fiabilitate fără glitch, care nu a fost
observată niciodată în sistemele bazate pe R / C
FM / AM. .

Figura 3 -19 FrSky X8R 2.4G 16CH
Specificație:
Receptor de telemetrie FrSky X8R
Dimensiune: 46,5 x 27 x 14,4 mm (1,83 "x 1,06" x .56 ")
Greutat e: 16,8 g (.59 oz)
Tensiune de operare: 4V -10.0V
Curent de operare: 100mA @ 5V
Gama de operare:> 1,5km (Full Range)
Ieșire RSSI: ieșire analogică de tensiune (0 ~ 3.3V)
Firmware Upgradeable (Aveți firmware -ul SUA)
Numărul canalelor: 16CH (1 ~ 8c de la ieși rile canalelor convenționale, 1 ~ 16ch de la portul SBUS
sau combinați două X8R pentru a deveni un receptor cu 16 canale)

57

3.14 FrSky 2.4GHz ACCST TARANIS

Cel mai important aspect pentru
orice transmițător este menținerea unei
conexiuni solide cu receptorul. FrSky este
bine cunoscut pentru tehnologia ACCST
care folosește frecvența, profitând de
întreaga bandă de 2,4 GHz.

Figura 3 -20 FrSky TARANIS

FrSky a reușit să introducă un transmițător la un preț de intrare care depășește multe dintre
cele mai bune si steme de marcă de pe piață.
S-ar putea să vă faceți griji căci calitatea a fost redusă pentru a menține prețurile în jos.
Compromisul de calitate din motive de preț nu există pentru modul FrSky.
Specificație:
Taranis X9D Plus
Număr de canale: Până la 16 canale
Tensiune de operare: 6 ~ 15V (2S, 3S Lipos sunt acceptabile)
Curent de funcționare: maxim 260mA (modulul RF și iluminatul din spate sunt activate)
Temperatura de funcționare: -10 ~ 60 ℃
Ecran LCD de iluminare din spate: 212 * 64, 2 combinații de cul ori pentru selectare
Model Memories: 60 (poate fi extins prin card SD)
Compatibilitate: Receptoare seria FrSky X, seria D și seria V8 -II (plus alte receptoare dacă se
utilizează un modul extern)
Versiunea Taranis X9D Plus -EU este compatibilă numai cu versiunea UE a receptorului din
seria X.

58

4 ASAMBLAREA
Asamblarea este partea cea mai distractivă și facilă (sau dificilă) a unui astfel de proiect. Este
bine a se porni de la o schiță bine conturată pentru a evita situațiile ambigue ce pot apărea pe parc urs.
Pentru acest quadcopter se porneste de la o configurație X, deci o dispunere radială a brațelor în jurul
unui punct fix ce va reprezenta centrul de greutate al dronei. În acest sens am pregătit cele 4 brațe
astfel: am dat găuri la unul din capete pent ru fixarea brațelor de piesa centrală din sticlotextolit și
pentru introducerea în interiorul bării de aluminiu a firelor lor de comandă .

Figura 4-1 Ansamblu

Figura 4 -2 Ansamblu ESC -Braț cu fire

59

Figura 4-3 Atașarea br ațelor in functie de culori

Ulterior am montat suportul motor pe brat
folosind șuruburi cu cap imbus. Opțional se poate
monta un tren de aterizare. Anterior au fost
colorate brațele quadcopterului pentru a marca
direcția arbitrară de zbor în negru -fata, rosu -spate

Figura 4-4 Surub M3 inox cu cap IMBUS

60

5 PROGRAMAREA ESC -URIL OR
5.1 CALIBRAREA ESC -urilor
Un ESC cu un nou firmware nu știe care
sunt valorile maxime și minime ale throttle -ului
pentru care să comande motorului turație
maximă, respectiv m inimă. Din această cauză
calibrarea este crucială. Se conecteaza temporar
un motor la ESC -ul dorit să fie calibrat.
Presupunând că au fost introduse emițătorul din
modulul FrSky în telecomandă, se pornește și se
setează manșa de throttle la maxim (FOARTE
IMPORTANT!).
Figura 5 -1 ESC
Se conectează pinul de comandă al ESC -ului pe canalul 3 al receiver -ului FrSky și apoi se
alimentează ESC -ul direct din acumulator. În acest moment se aude un sunet caracteristic format din
3 tonuri, fiecare mai înalt decât prece dentul. Uneori, dacă pulsul semnalului este sub 1030µs, un al
patrulea ton este generat. Se pune throttle la minim, moment în care se aude din nou un sunet
caracteristic, de această dată format din tonuri de aceeași înălțime. În acest moment se poate acțio na
manșa throttle, iar motorul ar trebui să se rotească cu o viteză corespunzătoare.
În cazul în care motorul nu emite primul set de sunete atunci când este alimentat ESC -ul,
înseamnă că nu a fost programat corect sau este conectat greșit la receiver.

61

5.2 MONTAREA ESC -urilor
Această etapă necesită maximum de atenție deoarece ordinea în care sunt conectate fazele
motorului pe ESC îi determină sensul de rotație. Fazele (A, B, C) sunt etichetate pe firele motoarelor.

Figura 5 -2 Montarea ESC -urilor
Dispunerea lor ar trebui să fie una concentrică, astfel: se taie doi conductori de culori diferite
(pentru plus și minus, roșu și negru) de dimensiunea diametrului exterior al plăcii din sticlotextolit. Se
conectează plusul și minusul fiecărui ESC. Dispu nerea firele de -a lungul razei piesei centrale din
sticlotextolit (pe dedesubtul piesei de jos, adică firele să fie expuse în exterior ). La unul din capetele
conductorilor se conectează alimentarea primului ESC. Apoi, se conectează restul driver -elor, pe
rând, astfel: ESC -ul în dreptul brațului corespunzător, se dezizolează conductorii dispuși circular (cei
lungi) în dreptul firelor de alimentare de la ESC (trebuie expus celpuțin 1 cm de fir, deci se taie de jur
împrejur cu gr ijă), se inserează o bucată de 2 cm de tub termocontractibil pe fiecare dintre firele lungi
(plus și minus) pentru a putea izola ulterior lipitura, cu ajutorul unei pensete se face o gaură în lițele
expuse, în care se introduce capătul firului de
alimentare corespunzător al ESC -ului. S e face
lipitura și se acoperă, ulterior, cu tubul
termocontractant sau cu bandă izolatoare. Se
securizez ESC -ul și firele conectate cu colier de
plastic.

Figura 5 -3 Fixarea ESC pe br aț

62

6 DESCRIERE SOFTWARE
6.1 Programarea controlerului de z bor (Flight Controlle r)
Se conectează FC -ul (Flight Controller) la calculator folosind un cablu micro USB – USB.
Se descarcă software -ul Mi ssion Planner .
Mission Planner este un software open -source care, în ultimii ani, a prins o mare
popularitate în rândul amatorilor de multicoptere, oferind o concurență semnificativă software -urilor
consacrate în domeniu. Pentru a urca software -ul pe controler, aveți nevoie de o versiune de un flight
controller Pixhawk 2.4.8 . Se lanseaza Mission Planner si se selectează INITIAL SETUP / Install
Firmware / Arducopter V3.6.9 .

Figura 6 -1 Fereastră de configurare frame
În cazul în care este folosit GPS -ul se conectează la mufa GPS. Modulele adiționale precum
OSD (On Screen Display) și Telemetrie se pot conecta la mufa RF Telemetry.

REAMINTESC: Pentru orice modul
la care comunicația cu FC -ul se realizează
prin interfață serială (Rx/Tx) cum sunt
GPS -uI, OSD -ul și Telemetria, pinii RX și
TX ai modulului se conectează la TX -ul,
respectiv RX -ul FC -ului. VCC și GND sunt
comune pentru toa te modulele.
Figura 6 -2 Conectori Pixhawk 2.4.8
Notă: Valoarea implicită a Pixhawk -ului este la o rată de transfer de 57600 baud pe portul său de
telemetrie.

63

Figura 6 -3 Mission Planer
1- Viteza aerului (Viteza relativa la sol
dacă nu este instalat un senzor de aer)

2- Crosstrack error and turn rate (T)

3- Direcția de zbor

4- Unghiul de inclinare

5- Calitatea conecțiunii telemetriei radio (% pachete pierdute)

6- Timpul GPS

7- Altitudinea ( bara albastră este rata ascensio nală)

8- Viteza aerului

9- Viteza față de sol

10- Statutul bateriei

11- Zona mesaje

12- Orizontul artificial

13- Starea GPS

14- Puncte misiune (curent / total)

15- Modul de zbor

64

Pentru a putea vizualiza în timp real parametrii controlerului și pentru a face reglajele
necesare unui zbor cât mai echilibrat este nevoie de o interfață grafică corespunzătoare. Aceasta se
numește Mission Planner și vine cu un mediu foarte simplist și intuitiv de programare.

Figura 6 -4 FC neconectat
Din meniul " Inițial Setup -> Mandatory Hardware " se fac setările inițiale. Pentru cadru
(frame) se alege configurația "X".
Figura 6 -5 Wizzard de configurare

65

6.2 Calibrarea accelerometrului
Pentru calibrarea accelerometrului este nevoie ca quad copterul să fie așezat în diferite poziții
pe fiecare axă (level, left side, right side, nose down, nose up, back side). Foarte important este ca
pozițiile quad copterului să fie perpendiculare pe toate axele astfel ca valorile citite de accelerometru
să fie cât mai precise.

Figura 6 -6 Calibrare accelerometru

66

6.3 Calibrare magnetometru
Magnetometrul este influențat de liniile de câmp magnetic terestru. Aceste erori pot fi
minimizate prin introducerea unei declin ații care se poate calcula cu ajutorul: magnetic -declination
Valoarea obținută se trece în câmpul "Declination" și apoi se apasă butonul "Live
Calibration". În acest moment trebuie să rotesc continuu quad copterul pe cele 3 axe în ambele sensuri
trigonometric și orar .

Figura 6 -7 Calibrare magnetometru
Giroscopul nu are nevoie de ajustări deoarece el se auto -calibrează la fiecare reset.

67

6.4 Calibrarea radiocomenzii
Calibrarea radiocomenzii decurge rapid dacă totul a fost făcut corect până acum. Toate
versiunile mai recente de 2.9 au un standard prestabilit de comunicare între controler și receptorul
radiocomenzii denumit PPM (toate canalele din telecomandă ies pe același pin). Marea majoritate a
receptoarelor de pe piață, însă, au standard PWM (fiecare ca nal din telecomandă are propriul pin de
ieșire pe PCB). Dacă în meniul "Radio Calibration" nu este afișat niciun semnal înseamnă că
receptorul comunică PWM, iar placa este setată pe PPM. Pentru a rezolva această problemă trebuie să
se modifice codul sursă al programului de pe FC.
Cea mai comună ordine a canalelor este ROLL, PITCH, THROTTLE, YAW, MODE, AUX2,
CAMPITCH, CAM ROLL.
Foarte important este ca și în meniul stației ordinea să fie aceeași (în general A.E.T.R )
(Ailerons. Elevator. Throttle. Rudder)

Figura 6 -8 Calibrarea stației radio

68

6.5 Modurile de zbor

Figura 6 -9 Modurile de zbor
Din meniul "Flight Modes" se pot selecta în timp real și sunt programe prestabilite ce vin în
ajutorul pilotului. În general, modurile de zbor se aleg cu aj utorul canalului 5 al telecomenzii. În
modul "Stabilize" pilotul are controlul total. În general, se folosește la setarea PID -urilor și în
zborurile incipiente pentru acomodarea cu comenzile dronei. Control total înseamnă că dacă pilotul
setează o comandă, drona va interpreta semnalul fără a mai face modificări indiferent de altitudinea de
zbor. Este recomandat a fi folosit acest mod doar dacă pilotul are o experiență solidă în calibrarea și
pilotarea dronelor.
Modul " Alt Hold " permite pilotului controlul t otal pe PITCH , ROLL, YAW și însă
altitudinea rămâne constantă indiferent de poziția manșei THROTTLE . Menținerea altitudinii se face
cu ajutorul barometrului, astfel că dacă în zona în care este pilotată drona presiunea atmosferică
variază brusc, quad copter ul va urca sau va coborî în funcție de aceasta. Se menționează că altitudinea
nu variază "indiferent" de poziția manșei THROTTLE . Această afirmație este adevărată pentru valori
THROTTLE cuprinse între 40% – 60%. Dacă acestea sunt mai mici de 40%, respectiv mai mari de
60%, drona va coborî sau va urca cu o viteză de aproximativ 2.5m/s. Acest mod oferă siguranță în
cazul unei schimbări accidentale a poziției manșei de throttle.
Modul " LOITER " este un mod "ALT HOLD " în care toate comenzile sunt blocate,
quad copterul fiind "lipit" pe cer. Spre deosebire de celelalte moduri, acesta folosește și GPS -ul pentru
a menține o poziție cât mai exactă. Parametrii pot fi modificați de către pilot însă într -o măsură
minimalistă, doar pentru a corecta unele imperfecțiuni de poziție și de altitudine.

69
Modul " RTL : Return to launch" oferă posibilitatea ca quad copterul să se întoarcă la punctul
de lansare independent de comenzile pilotului. Acest mod este dependent de GPS . înainte de a zbura,
este recomandat ca LED -ul de la GPS este aprins. Modul RTL este foarte folositor în cazul în care
pilotul nu își mai poate da seama în ce poziție se află drona sau, din diferite motive, acesta nu mai are
control asupra ei.

Trebuie, însă, foarte prudenți deoarece, în acest mod, drona va urc a la o altitudine prestabilită,
iar apoi va veni în linie dreaptă spre poziția de început. Astfel, în cazul în care în drumul ei se vor afla
obstacole, aceasta nu le va putea ocoli, coliziunile fiind iminente. De aceea este foarte important ca
înainte de a activa acest mod pilotul să se asigure ca intre el și dronă nu se află niciun obiect ce ar
putea obstrucționa traiectoria dronei.

70

6.6 Setarea regulatoarelor de tip proporțional integrator derivative

Figura 6 -10 Fereastră setări PID
Pentru o reglare mai eficientă a PID -urilor se poate selecta din meniul COPTER PIDS
funcția canalului 6 ca fiind cea de setare a PID -urilor. Astfel, în timp ce copterul zboară în mod
"Stabilize", utilizând canalul 6 (care trebuie să fie pus pe unul din cele 3 potențiometre ale stației), se
poate regla în timp real valoarea PID -ului pe una din direcții. După ce se obține un PID optim, se
aterizează și se selectează alt parametru de setat și se zboara din nou în care se reglează PID -ul
aferent. în figura de mai jos sunt ilustrate valorile mele ale PID -urilor.

71

6.7 Armarea motoarelor și pregatirea de zbor
Pentru a evita unele accidente datorate unei valori accidentale ale THROTTLE -ului, softul
este prevăzut cu o secvență de siguranță. Acesta constă în mutarea manșei de YAW (Rudder) la
maxim, iar cea de THROTTLE la minim pentru aproximativ 4 – 5 secunde.

Figura 6 -11 Poziția manșelor pentru a arma copterul
Dacă nu sunt executați întocmai acești pași, FC -ul nu va porni motoarele. Odată ce
controlerul vede această secvență de semnale, va considera că este pregătit de zbor și va arma
motoarele. Problema este că semnalele nu ajung mereu la controler, iar armarea nu se poate face decât
în acest mod.
Principalele motive pentru care co ntrolerul nu armează motoare le:
 Canalele nu sunt puse în ordinea corectă (Ailerons. Elevator. Throttle. Rudder).
 Valoarea maximă a YAW -ului nu este mai mare de 1600 – 1700. Din unele motive e posibil
ca atunci când a fost calibrată stația, valoarea maximă citită pentr u YAW să fie mai mică de
1900 -2000. Această problemă se rezolvă prin recalibrarea stației sau modificarea limitei
superioare pentru canalul de YAW (EPA – End Point Adjusment) din meniul stației la o
valoare cuprinsă în intervalul 1900 – 2000.

 Valoarea THROTTLE este mai m are de 1000 – 1100. Pentru ESC -urile modificate cu soft
SimonK, valoarea minimă de armare este aproximativ 1050. Această problemă se rezolvă prin

72
recalibrarea stației sau modificarea limitei inferioare pentru canalul de THROTTLE (EPA –
End Point Adjusment) din meniul stației.
 ESC -uri necalibrate corect. Punctele de MIN și MAX nu sunt identice la toate cele 6 ESC -uri.
 Accelerometrul nu este calibrat.
 Magnetometrul nu este calibrat.
 Este în alt mod decât "Stabilize", iar GPS -ul nu este fixat.

În cazul în car e, după ce au fost verificate toate punctele de mai sus controlerul nu se armează
motoarele, FC -ul se resetează din terminal, după care se șterge firmware -ul. Pentru aceasta se
conectează placa la PC, se lansează MissionPlanner și se conectează la terminal . Din meniul SETUP
se tastează comanda RESET .

73

7 DOMENIILE DE UTILIZA RE
7.1 Agricultură

Figura 7 -1 Agricultură

Supravegherea aeriană de rutină a terenurilor agricole vă poate oferi o analiză detaliată a
performanțelor culturilor.În plus, d ronele pot efectua această evaluare cu cheltuieli mici fără a afecta
mediul s au domeniile. Astfel are ca rezultat o creștere a productiei , ci și va crește randamentul
acestora culturilor
Dronele sunt utilizați pentru a pulveriza pesticidele, pentru a speri e păsările care mănâncă
culturi și pentru a efectua alte activități. Aproximativ zece zile de muncă obișnuită se efectuează în
aproximativ două zile din cauza acestor quadcoptere. Ideea sa răspândit pe tot globul și a fost folosită
în multe elemente ale ag riculturii.
Un astfel de proiect vine de la business -ul Vine Rangers din California , care testează
folosirea UAV -urilor cu ajutorul camerelor cu infraroșu pentru a găsi ceea ce cu ochiul liber nu poate
vedea în procesul de vinificație. Cu o combinație de software aceștia verifică bolile, apoi analizează
gradul, stresul, respirația frunzelor, randamentul etc., pentru a îmbunătăți vinul și strugurii.

74

7.2 Fotografie aerienă
Fotografia aeriană este în realitate printre
utilizările primare ale dronilor.
Datorită tehnologiei îmbunătățite, mai mult
de 40% din drone sunt echipate pentru a ține camera
foto.
Dronele , în zilele noastre, sunt precise și pot
obține imagini clare cu posibilitatea să gestionați ceea
ce captează camera video și vede a direct pe telefon și
obțineți nerea de înregistrari video de înaltă definiție.
Figura 7 -2 Fotografie aeriană
7.3 Comerciale, industriale și construcții
Unii oameni cred, mai devreme sau mai
târziu, că dronele vor lucra alături de noi pentru a
pune bazele unei clădiri și a efe ctua lucrări
similare în domeniul constru cțiilor civile și
industriale. Dronele și, în special, quad -urile își
găsesc drumul în industria construcțiilor în multe
feluri.
Principalele 5 includ:
 Inspecțiile. Figura 7 -3 Construcții
 Monitorizarea angajatilor.
 Afișați clienților un aspect aerian al progresului construcției.
 Topografie.
 Securitate.

75

Monitorizarea frecventă ar putea aduce progrese semnificative în in frastructura de construcție,
ceea ce ar duce la performanțe îmbunătățite. În plus, în cazul în care aeronavele sunt destul de mici,
acestea se pot apropia si capta imagini care vă pot oferi o idee mai aprofundată a construcției.
Câteva companii folosesc ve hicule aeriene fără pilot pentru a înregistra punctele de vedere ale
șantierului de construcție și pentru a le folosi în modelele tridimensionale efectuate de arhitect. Nu
este doar un excelent instrument de construcție, dar și un instrument util de market ing în timp ce
încearcă să ab ordeze sau să vândă un proiect.
7.4 Drone Racing & Sports
Este o altă utilizare populară a dronelor
care își găsește drumul în viața noastra. Mai
mulți utilizatori încep să intre în această
activitate și să o urmeze ca un hobby.
Este ca o cursă asemanătoare jocurilor
video, cu excepția faptului că întâlniți scenarii
reale la manșa unui quadcopter.
Figura 7 -4 Drone r acing
7.5 Livrarea și transport aerian
Una dintre multele utilizări inovatoare ale
dronelor, care a prins foarte repede interesul
media, a fost alimentarea și livrarea de produse.
Companiile mari, cum ar fi DHL, Amazon,
Dominos și FedEx , testează cu ajutorul
transportul local al articolelorcu ajutorul acestora.
Nu numai că vom obține livrări de
produse super -rapid , dar pu teți obține produs ul
livrat oriunde v -ați afla acum.
Figura 7 -5 Livrarea și transport aerian
Un mare obstacol este limitarea FAA în ceea ce priveș te utilizarea comercială a drone lor.
Acestea nu sunt permise în acest moment fără acordul expres al FAA.

76

Firmele de distribuție și comercianții cu amănuntul se confruntă încă cu numeroase provocări
pentru a avea această tehnologie posibilă , însă este vorba doar de timp pentru rezolvarea acestor
probleme.
În planurile Amazon Prime Air peste câțiva ani acestia intenționează să utilizeze mașinile de
comandă pentru a trimite comenzile clienților, exemplu : un set de adidași ajung cu o jumătate de oră
mai puțin.
Aceste drone sunt construite cu senzori care le permit să evite obstacolele de -a lungul căii, să
aterizeze în apropierea unei case sau a oricărui alt loc și să se întoarcă la postul de comandă.

7.6 Combaterea incendiilor

Figura 7 -6 Combaterea incendiilor
Știm deja că elicopterele sunt folosite pentru lupta împotriva incendiilor, dar în curând
dronele ar put ea schimba acest lucru. Aeronavele se dezvoltă pentru a ajuta la lupta împotriva
incendiilor.
Vestea bună d espre acest lucru este compararea cu elicopterele care s unt periculoase pentru
operator, acestea sunt fără pilot.

77

7.7 Forțele militare și armate

Figura 7 -7 Forțele militare și armate
Dronele au avut numeroase utilizări în lumea război ului militar, tactic și modern.
Aceste sisteme aeriene fără pilot sunt folosite în sc opul loviturilor aeriene. Camerele de
dime nsiuni mici și puternice le fac pot rivite pentru det ectarea bombelor. Astfel, aceste drone sunt
capabili să ne informeze despre bombe neexplodate, salvând astfel vieți.
7.8 Mapare
Dronele au avut, de asemenea, un impact
semnificativ în domeniul cartografierii.
Există părți ale pământului care nu sunt
rapid accesibile pentru oameni. Cu toate acestea,
pentru obiectivul de înțelegere a peisajului și
pregătirea hărților 3D, au fost folosite camere de
zbor. Acest sistem este acum disponibil pentru
toată lumea captuarea de imagini pentru
cartografier ea unor astfel de locații.
Figura 7 -8 Mapare

78

7.9 Minerit

Dronele au frecventă aplicare în
locurile de muncă miniere din întreaga
lume. Se utilizează pentru a face față
lucrărilor care sunt periculoase sau grele
pentru lucrători.

Figura 7 -9 Minerit
Pentru a asigura oamenii , dronele sunt, de asemenea, folosite pentru a scana spații
periculoase, cum ar fi intrările de mină și pereții groapelor, mai ales dacă locul este considerat nesigur
și periculos.
Atât maparea, cât și modelarea sunt esențiale pentru or ganizațiile miniere. Aplicarea dronelor
în acest segment oferă modele și hărți mult mai precise și sunt mult mai puțin costisitoare în
comparație cu metodele populare existente.
Întreprinderile miniere testează echipamentele quadcoptere împreună cu dispozi tivele de
vizionare multi -spectrale. Acesta permite companiilor să descopere minereu prin pilotarea de dron e în
jurul unor zone diferite și în interiorul puțurilor de mine .

79

7.10 Alte domenii de ulilizare a quadcopterelor:
7.10.1 In industria medicală.
7.10.2 Apara rea legii.
7.10.3 Recreere, Hobby & Distracție.
7.10.4 Robotică și aplicații experimentale.
7.10.5 Supraveghere și monitorizare .
7.10.6 Operațiuni de căutare și salvare.
7.10.7 Monitorizarea vieții sălbatice.
7.10.8 Divertisment.
7.10.9 Gradinărit.
7.10.10 Jurnalism.
7.10.11 Transportul de persoane.
7.10.12 Topografie.
7.10.13 Acoperir e aeriana.
7.10.14 Expansiunea telecomunicaților.
7.10.15 Previziuni și monitorizare meteo.

80
8 CONCLUZII
8.1 Concluzii generale
Obiectivele principal e al acestei lucrări a fost dezvoltarea unui sistem de tip quadcopter care
să fie capabil să zboare comandat de la distanță de un operator sau total autonom, dar si integrarea mai
multor dispozitive într -un sistem aerian, permițând transferul informației destinate controlului
acestuia, realizarea unei telecomenzi software și dezvoltarea unui program cu scopul de a face robotul
autonom.
Pentru realizarea întregului proiect a fost nevoie de componente hardware necesare pentru
construirea sistemului fizic, de informații cum funcționează un quadcopter astfel încât să realizăm
diferite simulări, de resurse software pentru generarea s emnalelor de control și de aplicații pentru
configurarea diferitelor piese.
Pentru modelele fara pilot la bord, simplitatea si economia construirii reprezinta avantaje de
necontestat, operatiunile de intretinere fiind extrem de reduse, iar costurile legate de functionare,
nesemnificative.
8.2 Contribuții personale
Consider ca această lucrarea include contribuții personale în realizarea practică a proiectului,
dar necesită și informații teoretice pentru realizarea ei.
Sistemul a fost construit de mine începând cu lipirea diferitelor cabluri la componente și
realizarea tuturor legăturilor electrice necesare. Diferite teste s -au făcut în continuare pentru
verificarea pieselor și realizarea programelor.
Am realizat configurarea ESC -urilor, controlarea motoarelor și diferite proceduri de siguranță
ale sistemului. Am realizat transmisia și recepția comenzilor de la distanță folosind module de
comunicație.

81
8.3 Dezvoltări viitoare

Deși multicopterele și controlul autonom al acestora au fost studiate extensiv de -a lungul
timpului, se pot găsi mereu noi direcții promițătoare de studiu și dezvoltare.
Așa cum a fost menționate de la început, acest sistem de tip quadcopter are o
multifuncționaliate foarte largă. Pentru realizarea unui sistem cât mai stabil și cât mai s igur va exista
în continuare loc pentru îmbunătățire. În viitor se dorește ca acest sistem să fie comand at de la
distanțe mai mari și atașarea unei camere pentru identificarea obiectelor.
Următoarele sarcini ce pot fi îndeplinite prin dezvoltari viitoare :
 Realizarea comunicației între mai mulți roboți. 
 Folosirea unor module cu rază lungă de comunicație pentru realizarea controlului de la
distanță.
 Introducerea comenzilor online de pe o pagină web, astfel încât controlul să fie făcut de
oriunde atât timp c ât microcontrolerul de comandă al quadcopterul este conectat la internet.
 Realizarea comunicației cu alte unități robotice pentru îndeplinirea sarcinilor mai complexe și
urmărirea lor.
Aceste sarcini pot fi îndeplinite și vor ajuta în crearea unui sistem cât mai performant și stabil în
mediul exterior.

82
9 BIBLIOGRAFIE
Dan Andrițoiu , “ Sistem Aerian fără Pilot ”
Z. Zuo, “Trajectory tracking control design with command -filtered compensation for
a quadrotor,” IET Control Theory Appl., vol. 4, no. 11, p g. 2343 –2355, 2010.
P. Martin and E. Sala¨un, “The true role of acceleromter feedback in quadrotor control,”
IEEE International Conference on Robotics and Automation, pg. 1623 –1629, May 2010.
S. Bouabdallah, A. Noth, and R. Siegwart, “PID vs LQ control tec hniques applied to
an indoor micro quadrotor,” IEEE/RSJ International Conference on Intelligent Robots and
Systems, vol. 3, pg. 2451 –2456, 2004.
T. Madani and A. Benallegue, “Backstepping control for a quadrotor helicopter”, IEEE/RSJ
International Conferen ce on Intelligent Robots and Systems, pp. 3255 –3260, 2006.
K. M. Zemalache, L. Beji, and H. Marref, “Control of an under -actuated system: Application
to a four rotors rotorcraft,” IEEE International Conference on Robotic and Biomimetics, pg. 404 –
409, 2005.
H. Huang, G. M. Hoffmann, S. L. Waslander, and C. J. Tomlin, “Aerodynamics and control
of autonomous quadrotor helicopters in aggressive maneuvering,” IEEE International Conference
on Robotics and Automation, pg. 3277 –3282, Mai 2009.
A. Tayebi and S. McGi lvray, “Attitude stabilization of a four -rotor aerial robot,” 43rd IEEE
Conference on Decision and Control, vol. 2, pg. 1216 –1221, 2004.
I. C. Dikmen, A. Arısoy, and H. Temelta¸s, “Attitude control of a quadrotor,” 4th
International Conference on Recent Ad vances in Space Technologies, pg. 722 – 727, 2009.
G. V. Raffo, M. G. Ortega, and F. R. Rubio, “An integral predictive/nonlinear H1
control structure for a quadrotor helicopter,” Automatica, vol. 46, no. 1, pg. 29 –39, 2010.
P. Castillo, R. Lozano, and A. D zul, “Stabilisation of a mini rotorcraft with four
rotors,” IEEE Control Systems Magazine, pg. 45 –55, Dec. 2005.
Escare˜no, C. Salazar -Cruz, and R. Lozano, “Embedded control of a four -rotor UAV,”
American Control Conference, vol. 4, no. 11, pg. 3936 –3941, 2006.
R. He, S. Prentice, and N. Roy, “Planning in information space for a quadrotor helicopter in a
GPS-denied environment,” IEEE International Conference on Robotics and Automation, pg. 1814 –
1820, 2008.

83
G. M. Hoffmann, H. Huang, S. L. Waslander, and C. J . Tomlin, “Quadrotor, helicopter flight
dynamics and control: Theory and experiment,” Proceedings of the AIAA Guidance, Navigation and
Control Conference and Exhibit, August 2007.

84
10 REFERINȚE WEB
www.wikipedia.com
http://lazyzero.de/en/modellbau/kkmulticopterflashtool
http://wiki.openpilot.org/display/Doc/RapidESC+Database
http://en.wikipedia.org/wiki/American_wire_gauge#Tables_of_AWG_wire_sizes
http://ardupilot.com/downloads/?did=82
http://magnetic -declination.com
https://pixhawk.ethz.ch/mavlink/
http://copter.ardupilot.com/wiki/auto -mode/
http://copter.ardupilot.com/wiki/common -planning -a-mission-with-waypoints -and-events/
http://copter.ardupilot.com/wiki/mission -command -list/#Condition -Yaw
http://co pter.ardupilot.com/wiki/loiter -mode/
http://copter.ardupilot.com/wiki/circle -mode/
http://copter.ardupilot.com/wiki/rtl -mode/
http://copter.ardupilot.com/wiki/land -mode/
http://copter.ardupilot.com/wiki/common -cameras -and-gimbals/
https://github.com/MegaPirateNG/ardupilot -mpng/tree/mpng -3.0.1 -r4/ArduCopter
www.ecal c.ch/xcoptercalc.htm?ecalc&lang=en
http://www.ecalc.ch/calcinclude/help/propcalchelp.htm
www.rctimer.com
www.hobbyking.com
www.frsky.com
https://docs.px4.io/en/flight_controller/pixhawk.html
https://www.progressiverc.com/media/X9DPlus.pdf

85
https://hobbyking.com/en_us/isdt -q6-lite-200w -battery -charger.html
http://ro.wikipedia.org/wiki/Elice
https://docs.px4.io/en/flight_controller/pixhawk.html
http://www.invensense.com/mems/gyro/mpu6050.html
https://pdf1.alldatasheet.com/datasheet –
pdf/view/929289/STMICROELECTRONICS/LSM303D.html
http://www.u -blox.com/images/downloads/Product_Docs/NEO -6_DataSheet_%28GPS.G6 –
HW-09005%29.pdf
http://www.flyingtech.co.uk/electronics/apm -3dr-radio -telemetry -kit-433mhz
http://myosuploads3.banggood.com/produc ts/20190520/20190520014107R9M2019Non -EU-
A4.pdf
https://imgaz.staticbg.com/images/upload/2012/chenjianwei/X8R.pdf
https://www.bitcraze.io/2018/11/demystifying -drone -dynamics/

86
11 CODUL SURSĂ
ArduCopter.pde

/// -*- tab-width: 4; Mode: C++; c -basic-offset: 4; indent -tabs-mode: nil -*-
#ifndef THISFIRMWARE
# define THISFIRMWARE "ArduCopter -MPNG V3.0.1 R4"
#endif
/*
* ArduCopter Version 3.0
* Creator: Jason Short
* Lead Developer: Randy Mackay
* Based on code and ideas from the Arducopter team: Pat Hickey, Jose Julio, Jani
Hirvinen, Andrew Tridgell, Justin Beech, Adam Rivera, Jean -Louis Naudin, Roberto Navoni
* Thanks to: Chris Anderson, Mike Smith, Jordi Munoz, Doug Weibel, James Goppert,
Benjamin Pelletier, Robert Lefebvre, Marco Robustini
*
* This firmware is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software F oundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* Special Thanks for Contributors (in alphabetical order by first name):
*
* Adam M Rivera :Auto Compass Declination
* Amilcar Lucas :Camera mount library
* Andrew Tridgell :General development, Mavlink Support
* Angel Fernandez :Alpha testing
* Doug Weibel :Libraries
* Christof Schmid :Alpha testing
* Dani Saez :V Octo Support
* Gregory Fletcher :Camera mount orientation math
* Guntars :Arming safety suggestion
* HappyKillmore :Mavlink GCS
* Hein Hollander :Octo Support
* Igor van Airde :Control Law optimization
* Leonard Hall :Flight Dynamics, Throttle, Loiter and Navigation Controllers
* Jonathan Chall inger :Inertial Navigation
* Jean-Louis Naudin :Auto Landing
* Max Levine :Tri Support, Graphics
* Jack Dunkle :Alpha testing
* James Goppert :Mavlink Support
* Jani Hiriven :Testing feedback
* John Arne Birkeland :PPM Encoder
* Jose Julio :Stabilization Control laws
* Marco Robustini :Lead tester
* Michael Oborne :Mission Planner GCS
* Mike Smith :Libraries, Coding support
* Oliver :Piezo support
* Olivier Adler :PPM Encoder
* Robert Lefebvre :Heli Support & LEDs

87
* Sandro Benigno :Camera support
*
* And much more so PLEASE PM me on DIYDRONES to add your contribution to the List
*
* Requires modified "mrelax" version of Arduino, which can be found here:
* http://code.google.com/p/ardupilot -mega/downloads/list
*
*/

////////////////////////////////////////////////////////////////////////////////
// Header includes
////////////////////////////////////////////////////////////////////////////////

#include <math.h>
#include <stdio.h>
#include <stdarg.h>

// Common dependencies
#include <AP_Common .h>
#include <AP_Progmem .h>
#include <AP_Menu.h>
#include <AP_Param .h>
// AP_HAL
#include <AP_HAL.h>
#include <AP_HAL_AVR .h>
#include <AP_HAL_MPNG .h>
#include <AP_HAL_AVR_SITL .h>
#include <AP_HAL_SMACCM .h>
#include <AP_HAL_PX4 .h>
#include <AP_HAL_Empty .h>

// Application dependencies
#include <GCS_MAVLink .h> // MAVLink GCS definitions
#include <AP_GPS.h> // ArduPilot GPS library
#include <DataFlash .h> // ArduPilot Mega Flash Memor y Library
#include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
#include <AP_ADC_AnalogSource .h>
#include <AP_Baro.h>
#include <AP_Compass .h> // ArduPilot Mega Magnetometer Library
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <AP_Curve .h> // Curve used to linearlise throttle pwm to thrust
#include <AP_InertialSensor .h> // ArduPilot Mega Inertial Sensor (accel & gyro) Library
#include <AP_AHRS.h>
#include <APM_PI.h> // PI library
#include <AC_PID.h> // PID library
#include <RC_Channel .h> // RC Channel Library
#include <AP_Motors .h> // AP Motors library
#include <AP_RangeFinder .h> // Range finder library
#include <AP_OpticalF low.h> // Optical Flow library
#include <Filter.h> // Filter library
#include <AP_Buffer .h> // APM FIFO Buffer
#include <AP_Relay .h> // APM relay

88
#include <AP_Camera .h> // Photo or video camera
#include <AP_Mount .h> // Camera/Antenna mount
#include <AP_Airspeed .h> // needed for AHRS build
#include <AP_InertialNav .h> // ArduPilot Mega inertial navigation library
#include <AC_WPNav .h> // ArduCopter waypoint navigation library
#include <AP_Declination .h> // ArduPilot Mega Declination Helper Library
#include <AC_Fence .h> // Arducopter Fence library
#include <memcheck .h> // memory limit checker
#include <SITL.h> // software in the loop support
#include <AP_Scheduler .h> // main loop scheduler

// AP_HAL to Arduino compatibility layer
#include "compat.h"
// Configuration
#include "defines.h"
#include "config.h"
#include "config_channels.h"

// Local modules
#include "Parameters.h"
#include "GCS.h"

////////////////////////////////////////////////////////////////////////////////
// cliSerial
////////////////////////////////////////////////////////////////////////////////
// cliSerial isn't strictly necessary – it is an alias for hal.console. It may
// be deprecated in favor of hal.console in later releases.
static AP_HAL::BetterStream * cliSerial ;

// N.B. we need to keep a static declaration which isn't guarded by macros
// at the top to cooperate with the prototype mangler.

///////////////////////////// ///////////////////////////////////////////////////
// AP_HAL instance
////////////////////////////////////////////////////////////////////////////////

const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER ;

/////////////////////////////////////////////////////// /////////////////////////
// Parameters
////////////////////////////////////////////////////////////////////////////////
//
// Global parameters are all contained within the 'g' class.
//
static Parameters g ;

// main loop scheduler
static AP_Scheduler sch eduler;

////////////////////////////////////////////////////////////////////////////////
// prototypes

89
////////////////////////////////////////////////////////////////////////////////
static void update_events (void);
static void print_flight_mode (AP_HAL::BetterStream *port, uint8_t mode );

////////////////////////////////////////////////////////////////////////////////
// Dataflash
////////////////////////////////////////////////////////////////////////////////
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
static DataFlash_APM2 DataFlash ;
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM1
static DataFlash_APM1 DataFlash ;
#elif CONFIG_HAL_BOARD == HAL_BOARD_MPNG
static DataFlash_MPNG DataFlash ;
#elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
//static DataFlash_File DataFlash("/ tmp/APMlogs");
static DataFlash_SITL DataFlash ;
#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
static DataFlash_File DataFlash ("/fs/microsd/APM/logs" );
#else
static DataFlash_Empty DataFlash ;
#endif

////////////////////////////////////////////////////////////// //////////////////
// the rate we run the main loop at
////////////////////////////////////////////////////////////////////////////////
static const AP_InertialSensor: :Sample_rate ins_sample_rate =
AP_InertialSensor: :RATE_100HZ ;

////////////////////////// //////////////////////////////////////////////////////
// Sensors
////////////////////////////////////////////////////////////////////////////////
//
// There are three basic options related to flight sensor selection.
//
// – Normal flight mode. Real sens ors are used.
// – HIL Attitude mode. Most sensors are disabled, as the HIL
// protocol supplies attitude information directly.
// – HIL Sensors mode. Synthetic sensors are configured that
// supply data from the simulation.
//

// All GPS access shoul d be through this pointer.
static GPS *g_gps;

// flight modes convenience array
static AP_Int8 *flight_modes = &g.flight_mode1 ;

#if HIL_MODE == HIL_MODE_DISABLED

#if CONFIG_ADC == ENABLED
static AP_ADC_ADS7844 adc ;

90
#endif

#if CONFIG_IMU_TYPE == CONFIG_IMU_MPU6000
static AP_InertialSensor_MPU6000 ins ;
#elif CONFIG_IMU_TYPE == CONFIG_IMU_MPU6000_I2C
static AP_InertialSensor_MPU6000_I2C ins ;
#elif CONFIG_IMU_TYPE == CONFIG_IMU_ITG3200
static AP_InertialSensor_ITG3200 ins(MPNG_BOARD_TYPE );
#elif CONFIG_IMU_TYPE == CONFIG_IMU_OILPAN
static AP_InertialSensor_Oilpan ins(&adc);
#elif CONFIG_IMU_TYPE == CONFIG_IMU_SITL
static AP_InertialSensor_Stub ins ;
#elif CONFIG_IMU_TYPE == CONFIG_IMU_PX4
static AP_InertialSensor_PX4 ins ;
#endif

#if CONFIG_HAL_BO ARD == HAL_BOARD_AVR_SITL
// When building for SITL we use the HIL barometer and compass drivers
static AP_Baro_HIL barometer ;
static AP_Compass_HIL compass ;
static SITL sitl ;
#else
// Otherwise, instantiate a real barometer and compass driver
#if CONFIG_BARO == AP_BARO_BMP085
static AP_Baro_BMP085 barometer ;
#elif CONFIG_BARO == AP_BARO_PX4
static AP_Baro_PX4 barometer ;
#elif CONFIG_BARO == AP_BARO_BMP085_MPNG
static AP_Baro_BMP085_MPNG barometer ;
#elif CONFIG_BARO == AP_BARO_MS5611
#if CONFIG_MS5611_SERIAL == AP_BARO_MS5611_SPI
static AP_Baro_MS5611 barometer (&AP_Baro_MS5611: :spi);
#elif CONFIG_MS5611_SERIAL == AP_BARO_MS5611_I2C
static AP_Baro_MS5611 barometer (&AP_Baro_MS5611: :i2c);
#else
#error Unrecognized CONFIG_MS5611_SERI AL setting .
#endif
#endif

#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
static AP_Compass_PX4 compass ;
#else
static AP_Compass_HMC5843 compass ;
#endif
#endif

// real GPS selection
#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO
AP_GPS_Auto g_gps_driver (&g_gps);

#elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA
AP_GPS_NMEA g_gps_driver ;

91

#elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF
AP_GPS_SIRF g_gps_driver ;

#elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX
AP_GPS_UBLOX g_gps_driver ;

#elif GPS_PROTOCOL == GPS_PROTO COL_MTK
AP_GPS_MTK g_gps_driver ;

#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK19
AP_GPS_MTK19 g_gps_driver ;

#elif GPS_PROTOCOL == GPS_PROTOCOL_NONE
AP_GPS_None g_gps_driver ;

#else
#error Unrecognised GPS_PROTOCOL setting .
#endif // GPS PROTOC OL

#if DMP_ENABLED == ENABLED && CONFIG_HAL_BOARD == HAL_BOARD_APM2
static AP_AHRS_MPU6000 ahrs(&ins, g_gps); // only works with APM2
#else
static AP_AHRS_DCM ahrs(&ins, g_gps);
#endif

// ahrs2 object is the secondary ahrs to allow runn ing DMP in parallel with DCM
#if SECONDARY_DMP_ENABLED == ENABLED && CONFIG_HAL_BOARD == HAL_BOARD_APM2
static AP_AHRS_MPU6000 ahrs2(&ins, g_gps); // only works with APM2
#endif

#elif HIL_MODE == HIL_MODE_SENSORS
// sensor emulators
static AP_ADC_HIL adc ;
static AP_Baro_HIL barometer ;
static AP_Compass_HIL compass ;
static AP_GPS_HIL g_gps_driver ;
static AP_InertialSensor_Stub ins ;
static AP_AHRS_DCM ahrs(&ins, g_gps);

static int32_t g ps_base_alt ;

#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
// When building for SITL we use the HIL barometer and compass drivers
static SITL sitl ;
#endif

#elif HIL_MODE == HIL_MODE_ATTITUDE
static AP_ADC_HIL adc ;
static AP_InertialSensor_Stu b ins;
static AP_AHRS_HIL ahrs(&ins, g_gps);
static AP_GPS_HIL g_gps_driver ;

92
static AP_Compass_HIL compass ; // never used
static AP_Baro_HIL barometer ;

static int32_t gps_base_alt ;

#if CONFIG_HAL_B OARD == HAL_BOARD_AVR_SITL
// When building for SITL we use the HIL barometer and compass drivers
static SITL sitl ;
#endif

#else
#error Unrecognised HIL_MODE setting .
#endif // HIL MODE

////////////////////////////////////////////////////////////////// //////////////
// Optical flow sensor
////////////////////////////////////////////////////////////////////////////////
#if OPTFLOW == ENABLED
static AP_OpticalFlow_ADNS3080 optflow ;
#else
static AP_OpticalFlow optflow ;
#endif

////////////////////////// //////////////////////////////////////////////////////
// GCS selection
////////////////////////////////////////////////////////////////////////////////
static GCS_MAVLINK gcs0 ;
static GCS_MAVLINK gcs3 ;

//////////////////////////////////////////////////// ////////////////////////////
// SONAR selection
////////////////////////////////////////////////////////////////////////////////
//
ModeFilterInt16_Size3 sonar_mode_filter (1);
#if CONFIG_SONAR == ENABLED
static AP_HAL::AnalogSource *sonar_analog_source ;
static AP_RangeFinder_MaxsonarXL *sonar;
#endif

////////////////////////////////////////////////////////////////////////////////
// User variables
////////////////////////////////////////////////////////////////////////////////
#ifdef USERHOOK_VARIABLES
#include USERHOOK_VARIABLES
#endif

////////////////////////////////////////////////////////////////////////////////
// Global variables
////////////////////////////////////////////////////////////////////////////////

/* Radio values
* Channe l assignments

93
* 1 Ailerons (rudder if no ailerons)
* 2 Elevator
* 3 Throttle
* 4 Rudder (if we have ailerons)
* 5 Mode – 3 position switch
* 6 User assignable
* 7 trainer switch – sets throttle nominal (toggle switch), sets
accels to Level (hold > 1 second)
* 8 TBD
* Each Aux channel can be configured to have a ny of the available auxiliary
functions assigned to it.
* See libraries/RC_Channel/RC_Channel_aux.h for more information
*/

//Documentation of GLobals:
static union {
struct {
uint8_t home_is_set : 1; // 0
uint8_t simple_mode : 1; // 1 // This is the state of simple mode
uint8_t manual_attitude : 1; // 2
uint8_t manual_throttle : 1; // 3

uint8_t pre_arm_rc_check : 1; // 5 // true if rc input pre -arm checks have
been completed successfully
uint8_t pre_arm_check : 1; // 6 // true if all pre -arm checks (rc, accel
calibration, gps lock) have been performed
uint8_t auto_armed : 1; // 7 // stops auto missions from beginning
until thro ttle is raised
uint8_t logging_started : 1; // 8 // true if dataflash logging has started

uint8_t low_battery : 1; // 9 // Used to track if the battery is low –
LED output flashes when the batt is low
uint8_t fail safe_radio : 1; // 10 // A status flag for the radio failsafe
uint8_t failsafe_batt : 1; // 11 // A status flag for the battery failsafe
uint8_t failsafe_gps : 1; // 12 // A status flag for the gps failsafe
uint8_t failsafe_gcs : 1; // 13 // A status flag for the ground station
failsafe
uint8_t rc_override_active : 1; // 14 // true if rc control are overwritten by
ground station
uint8_t do_flip : 1; // 15 // Used to en able flip code
uint8_t takeoff_complete : 1; // 16
uint8_t land_complete : 1; // 17
uint8_t compass_status : 1; // 18
uint8_t gps_status : 1; // 19
};
uint32_t value ;
} ap;

static struct AP_S ystem{
uint8_t GPS_light : 1; // 0 // Solid indicates we have full 3D lock and
can navigate, flash = read

94
uint8_t arming_light : 1; // 1 // Solid indicates armed state, flashing is
disarmed, double flashing is disarmed and failing pre -arm checks
uint8_t new_radio_frame : 1; // 2 // Set true if we have new PWM data to act
on from the Radio
uint8_t CH7_flag : 1; // 3 // true if ch7 aux switch is high
uint8_t CH8_flag : 1; // 4 // true if ch8 aux switch is high
uint8_t usb_connected : 1; // 5 // true if APM is powered from USB
connection
uint8_t yaw_stopped : 1; // 6 // Used to manage the Yaw hold capabilities

} ap_system ;

/////////// /////////////////////////////////////////////////////////////////////
// Radio
////////////////////////////////////////////////////////////////////////////////
// This is the state of the flight control system
// There are multiple states defined such as S TABILIZE, ACRO,
static int8_t control_mode = STABILIZE ;
// Used to maintain the state of the previous control switch position
// This is set to -1 when we need to re -read the switch
static uint8_t oldSwitchPosition ;

// receiver RSSI
static uint8_t receive r_rssi;

////////////////////////////////////////////////////////////////////////////////
// Motor Output
////////////////////////////////////////////////////////////////////////////////
#if FRAME_CONFIG == QUAD_FRAME
#define MOTOR_CLASS AP_MotorsQuad
#endif
#if FRAME_CONFIG == TRI_FRAME
#define MOTOR_CLASS AP_MotorsTri
#endif
#if FRAME_CONFIG == QUAD_FRAME
#define MOTOR_CLASS AP_Motors Quad
#endif
#if FRAME_CONFIG == Y6_FRAME
#define MOTOR_CLASS AP_MotorsY6
#endif
#if FRAME_CONFIG == OCTA_FRAME
#define MOTOR_CLASS AP_MotorsOcta
#endif
#if FRAME_CONFIG == OCTA_QUAD_FRAME
#define MOTOR_CLASS AP_MotorsOctaQuad
#endif
#if FRAME_CONFIG == HELI_FRAME
#define MOTOR_CLASS AP_MotorsHeli
#endif

#if FRAME_CONFIG == HELI_FRAME // helicopter constructor require s more arguments

95
static MOTOR_CLASS motors(&g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_8, &g.heli_servo_1 ,
&g.heli_servo_2 , &g.heli_servo_3 , &g.heli_servo_4 );
#elif FRAME_CONFIG == TRI_FRAME // tri constructor requires additional rc_7 argument to
allow tail servo reversing
static MOTOR_CLASS motors(&g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_7);
#else
static MOTOR_CLASS motors(&g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4);
#endif

////////////////////////////////////////////////////////////////////////////////
// PIDs
////////////////////////////////////////////////////////////////////////////////
// This is a convienience accessor for the IMU roll rates. It's currently the raw IMU rates
// and not the adjusted omega rates, but the name is stuck
static Vector3f omega ;
// This is used to hold radio tuning values for in -flight CH6 tuning
float tuning_value ;
// used to limit the rate that the pid controller output is logged so that it doesn't
negatively affect performance
static uint8_t pid_log_counter ;

//////////////////// ////////////////////////////////////////////////////////////
// LED output
////////////////////////////////////////////////////////////////////////////////
// This is current status for the LED lights state machine
// setting this value changes the output of the LEDs
static uint8_t led_mode = NORMAL_LEDS ;
// Blinking indicates GPS status
static uint8_t copter_leds_GPS_blink ;
// Blinking indicates battery status
static uint8_t copter_leds_motor_blink ;
// Navigation confirmation blinks
static int8_t copter_le ds_nav_blink ;

////////////////////////////////////////////////////////////////////////////////
// GPS variables
////////////////////////////////////////////////////////////////////////////////
// This is used to scale GPS values for EEPROM storage
// 10^7 times Decimal GPS means 1 == 1cm
// This approximation makes calculations integer and it's easy to read
static const float t7 = 10000000.0 ;
// We use atan2 and other trig techniques to calaculate angles
// We need to scale the longitude up to make these c alcs work
// to account for decreasing distance between lines of longitude away from the equator
static float scaleLongUp = 1;
// Sometimes we need to remove the scaling for distance calcs
static float scaleLongDown = 1;

////////////////////////////////// //////////////////////////////////////////////
// Location & Navigation
////////////////////////////////////////////////////////////////////////////////
// This is the angle from the copter to the next waypoint in centi -degrees

96
static int32_t wp_bearing ;
// The original bearing to the next waypoint. used to point the nose of the copter at the
next waypoint
static int32_t original_wp_bearing ;
// The location of home in relation to the copter in centi -degrees
static int32_t home_bearing ;
// distance between plane and home in cm
static int32_t home_distance ;
// distance between plane and next waypoint in cm. is not static because AP_Camera uses it
uint32_t wp_distance ;
// navigation mode – options include NAV_NONE, NAV_LOITER, NAV_CIRCLE, NAV_WP
static uint8_t nav_mode ;
// Register containing the index of the current navigation command in the mission script
static int16_t command_nav_index ;
// Register containing the index of the previous navigation command in the mission script
// Used to manage the execution of conditional commands
static uint8_t prev_nav_index ;
// Register containing the index of the current conditional command in the mission script
static uint8_t command_cond_index ;
// Used to track the required WP navigation information
// options include
// NAV_ALTITUDE – have we reached the desired altitude?
// NAV_LOCATION – have we reached the desired location?
// NAV_DELAY – have we waited at the waypoint the desired time?
static float lon_error , lat_error ; // Used to report how many cm we are from the next
waypoint or loiter target position
static int16_t control_roll ;
static int16_t control_pitch ;
static uint8_t rtl_state ; // records state of rtl (initial climb, returning
home, etc)
static uint8_t land_state ; // records state of land (flying to location,
descending)

////////////////////////////////////////////////////////////////////////////////
// Orientation
////////////////////////////////////////////////////////////////////////////////
// Convienience accessors f or commonly used trig functions. These values are generated
// by the DCM through a few simple equations. They are used throughout the code where cos
and sin
// would normally be used.
// The cos values are defaulted to 1 to get a decent initial value for a level state
static float cos_roll_x = 1.0;
static float cos_pitch_x = 1.0;
static float cos_yaw = 1.0;
static float sin_yaw;
static float sin_roll ;
static float sin_pitch ;

/////////////////////////////////////////////////////// /////////////////////////
// SIMPLE Mode
////////////////////////////////////////////////////////////////////////////////

97
// Used to track the orientation of the copter for Simple mode. This value is reset at each
arming
// or in SuperSimple mode when the copter leaves a 20m radius from home.
static int32_t initial_simple_bearing ;

// Stores initial bearing when armed – initial simple bearing is modified in super simple
mode so not suitable
static int32_t initial_armed_bearing ;

/////////////////////////// /////////////////////////////////////////////////////
// Rate contoller targets
////////////////////////////////////////////////////////////////////////////////
static uint8_t rate_targets_frame = EARTH_FRAME ; // indicates whether rate targets
provided in earth or body frame
static int32_t roll_rate_target_ef ;
static int32_t pitch_rate_target_ef ;
static int32_t yaw_rate_target_ef ;
static int32_t roll_rate_target_bf ; // body frame roll rate target
static int32_t pitch_rate_target_bf ; // body frame pitch rate target
static int32_t yaw_rate_target_bf ; // body frame yaw rate target

////////////////////////////////////////////////////////////////////////////////
// Throttle variables
//////////////////////////////////////////////////////////////// ////////////////
static int16_t throttle_accel_target_ef ; // earth frame throttle acceleration target
static bool throttle_accel_controller_active ; // true when accel based throttle
controller is active, false when higher level throttle controllers ar e providing throttle
output directly
static float throttle_avg ; // g.throttle_cruise as a float
static int16_t desired_climb_rate ; // pilot desired climb rate – for logging
purposes only
static float target_alt_for_reporting ; // target altitude in cm for reporting (logs
and ground station)

////////////////////////////////////////////////////////////////////////////////
// ACRO Mode
////////////////////////////////////////////////////////////////////////////////
// Used to co ntrol Axis lock
static int32_t roll_axis ;
static int32_t pitch_axis ;

// Filters
#if FRAME_CONFIG == HELI_FRAME
static LowPassFilterFloat rate_roll_filter ; // Rate Roll filter
static LowPassFilterFloat rate_pitch_filter ; // Rate Pitch filter
// LowPas sFilterFloat rate_yaw_filter; // Rate Yaw filter
#endif // HELI_FRAME

////////////////////////////////////////////////////////////////////////////////
// Circle Mode / Loiter control

98
//////////////////////////////////////////////////////////////////// ////////////
Vector3f circle_center ; // circle position expressed in cm from home location. x =
lat, y = lon
// angle from the circle center to the copter's desired location. Incremented at
circle_rate / second
static float circle_angle ;
// the total angle (in radians) travelled
static float circle_angle_total ;
// deg : how many times to circle as specified by mission command
static uint8_t circle_desired_rotations ;
static float circle_angular_acceleration ; // circle mode's angular acceleration
static float circle_angular_velocity ; // circle mode's angular velocity
static float circle_angular_velocity_max ; // circle mode's max angular velocity
// How long we should stay in Loiter Mode for mission scripting (time in seconds)
static uint16_t loiter_time_max ;
// How long have we been loitering – The start time in millis
static uint32_t loiter_time ;

////////////////////////////////////////////////////////////////////////////////
// CH7 and CH8 save waypoint control
////////////////// //////////////////////////////////////////////////////////////
// This register tracks the current Mission Command index when writing
// a mission using Ch7 or Ch8 aux switches in flight
static int8_t aux_switch_wp_index ;

//////////////////////////////// ////////////////////////////////////////////////
// Battery Sensors
////////////////////////////////////////////////////////////////////////////////
// Battery Voltage of battery, initialized above threshold for filter
static float battery_voltage1 = LOW_VOLTAGE * 1.05f;
// refers to the instant amp draw – based on an Attopilot Current sensor
static float current_amps1 ;
// refers to the total amps drawn – based on an Attopilot Current sensor
static float current_total1 ;

/////////////////////////////////// /////////////////////////////////////////////
// Altitude
////////////////////////////////////////////////////////////////////////////////
// The (throttle) controller desired altitude in cm
static float controller_desired_alt ;
// The cm we are off in alti tude from next_WP.alt – Positive value means we are below the
WP
static int32_t altitude_error ;
// The cm/s we are moving up or down based on filtered data – Positive = UP
static int16_t climb_rate ;
// The altitude as reported by Sonar in cm – Values are 2 0 to 700 generally.
static int16_t sonar_alt ;
static uint8_t sonar_alt_health ; // true if we can trust the altitude from the sonar
// The altitude as reported by Baro in cm – Values can be quite high

99
static int32_t baro_alt ;

static int16_t saved_toy_thr ottle;

////////////////////////////////////////////////////////////////////////////////
// flight modes
////////////////////////////////////////////////////////////////////////////////
// Flight modes are combinations of Roll/Pitch, Yaw and Throttle cont rol modes
// Each Flight mode is a unique combination of these modes
//
// The current desired control scheme for Yaw
static uint8_t yaw_mode ;
// The current desired control scheme for roll and pitch / navigation
static uint8_t roll_pitch_mode ;
// The curr ent desired control scheme for altitude hold
static uint8_t throttle_mode ;

////////////////////////////////////////////////////////////////////////////////
// flight specific
/////////////////////////////////////////////////////////////////////////////// /
// An additional throttle added to keep the copter at the same altitude when banking
static int16_t angle_boost ;
// counter to verify landings
static uint16_t land_detector ;

////////////////////////////////////////////////////////////////////////////// //
// 3D Location vectors
////////////////////////////////////////////////////////////////////////////////
// home location is stored when we have a good GPS lock and arm the copter
// Can be reset each the copter is re -armed
static struct Location home ;
// Current location of the copter
static struct Location current_loc ;
// Holds the current loaded command from the EEPROM for navigation
static struct Location command_nav_queue ;
// Holds the current loaded command from the EEPROM for conditional scri pts
static struct Location command_cond_queue ;

////////////////////////////////////////////////////////////////////////////////
// Navigation Roll/Pitch functions
////////////////////////////////////////////////////////////////////////////////
// all a ngles are deg * 100 : target yaw angle
// The Commanded ROll from the autopilot.
static int32_t nav_roll ;
// The Commanded pitch from the autopilot. negative Pitch means go forward.
static int32_t nav_pitch ;

// The Commanded ROll from the autopilot based on optical flow sensor.

100
static int32_t of_roll ;
// The Commanded pitch from the autopilot based on optical flow sensor. negative Pitch
means go forward.
static int32_t of_pitch ;

//////////////////////////////////////////////////////////////////////////// ////
// Navigation Throttle control
////////////////////////////////////////////////////////////////////////////////
// The Commanded Throttle from the autopilot.
static int16_t nav_throttle ; // 0-1000 for throttle control
// This is a simple counter to track the amount of throttle used during flight
// This could be useful later in determining and debuging current usage and predicting
battery life
static uint32_t throttle_integrator ;

//////////////////////////////////////////////////////////////////// ////////////
// Navigation Yaw control
////////////////////////////////////////////////////////////////////////////////
// The Commanded Yaw from the autopilot.
static int32_t nav_yaw ;
static uint8_t yaw_timer ;
// Yaw will point at this location if yaw_mod e is set to YAW_LOOK_AT_LOCATION
static Vector3f yaw_look_at_WP ;
// bearing from current location to the yaw_look_at_WP
static int32_t yaw_look_at_WP_bearing ;
// yaw used for YAW_LOOK_AT_HEADING yaw_mode
static int32_t yaw_look_at_heading ;
// Deg/s we shou ld turn
static int16_t yaw_look_at_heading_slew ;

////////////////////////////////////////////////////////////////////////////////
// Repeat Mission Scripting Command
////////////////////////////////////////////////////////////////////////////////
// The type of repeating event – Toggle a servo channel, Toggle the APM1 relay, etc
static uint8_t event_id ;
// Used to manage the timimng of repeating events
static uint32_t event_timer ;
// How long to delay the next firing of event in millis
static uint16_t ev ent_delay ;
// how many times to fire : 0 = forever, 1 = do once, 2 = do twice
static int16_t event_repeat ;
// per command value, such as PWM for servos
static int16_t event_value ;
// the stored value used to undo commands – such as original PWM command
static int16_t event_undo_value ;

////////////////////////////////////////////////////////////////////////////////
// Delay Mission Scripting Command

101
////////////////////////////////////////////////////////////////////////////////
static int32_t condition_val ue; // used in condition commands (eg delay, change alt, etc.)
static uint32_t condition_start ;

////////////////////////////////////////////////////////////////////////////////
// IMU variables
/////////////////////////////////////////////////////////// /////////////////////
// Integration time for the gyros (DCM algorithm)
// Updated with the fast loop
static float G_Dt = 0.02;

////////////////////////////////////////////////////////////////////////////////
// Inertial Navigation
/////////////////////// /////////////////////////////////////////////////////////
static AP_InertialNav inertial_nav (&ahrs, &ins, &barometer , &g_gps);

////////////////////////////////////////////////////////////////////////////////
// Waypoint navigation object
// To-Do: move in ertial nav up or other navigation variables down here
////////////////////////////////////////////////////////////////////////////////
static AC_WPNav wp_nav(&inertial_nav , &ahrs, &g.pi_loiter_lat , &g.pi_loiter_lon ,
&g.pid_loiter_rate_lat , &g.pid_loiter_ra te_lon);

////////////////////////////////////////////////////////////////////////////////
// Performance monitoring
////////////////////////////////////////////////////////////////////////////////
// The number of GPS fixes we have had
static uint8_t gps_ fix_count ;
static int16_t pmTest1 ;

// System Timers
// –––––
// Time in microseconds of main control loop
static uint32_t fast_loopTimer ;
// Counters for branching from 10 hz control loop
static uint8_t medium_loopCounter ;
// Counters for branchi ng from 3 1/3hz control loop
static uint8_t slow_loopCounter ;
// Counter of main loop executions. Used for performance monitoring and failsafe
processing
static uint16_t mainLoop_count ;
// Delta Time in milliseconds for navigation computations, updated wi th every good GPS read
static float dTnav;
// Counters for branching from 4 minute control loop used to save Compass offsets
static int16_t superslow_loopCounter ;
// Loiter timer – Records how long we have been in loiter
static uint32_t rtl_loiter_start_ti me;
// prevents duplicate GPS messages from entering system
static uint32_t last_gps_time ;
// the time when the last HEARTBEAT message arrived from a GCS – used for triggering gcs
failsafe

102
static uint32_t last_heartbeat_ms ;

// Used to exit the roll and pi tch auto trim function
static uint8_t auto_trim_counter ;

// Reference to the relay object (APM1 -> PORTL 2) (APM2 -> PORTB 7)
static AP_Relay relay ;

//Reference to the camera object (it uses the relay object inside it)
#if CAMERA == ENABLED
static AP_Camera camera(&relay);
#endif

// a pin for reading the receiver RSSI voltage.
static AP_HAL::AnalogSource * rssi_analog_source ;

// Input sources for battery voltage, battery current, board vcc
static AP_HAL::AnalogSource * batt_volt_analog_source ;
static AP_HAL::AnalogSource * batt_curr_analog_source ;
static AP_HAL::AnalogSource * board_vcc_analog_source ;

#if CLI_ENABLED == ENABLED
static int8_t setup_show (uint8_t argc , const Menu::arg *argv);
#endif

// Camera/Antenna mount tracking and stabilisatio n stuff
// –––––––––––––
#if MOUNT == ENABLED
// current_loc uses the baro/gps soloution for altitude rather than gps only.
// mabe one could use current_loc for lat/lon too and eliminate g_gps alltogether?
static AP_Mount camera_m ount(&current_loc , g_gps, &ahrs, 0);
#endif

#if MOUNT2 == ENABLED
// current_loc uses the baro/gps soloution for altitude rather than gps only.
// mabe one could use current_loc for lat/lon too and eliminate g_gps alltogether?
static AP_Mount camera_mount 2(&current_loc , g_gps, &ahrs, 1);
#endif

////////////////////////////////////////////////////////////////////////////////
// AC_Fence library to reduce fly -aways
////////////////////////////////////////////////////////////////////////////////
#if AC_FENCE == ENABLED
AC_Fence fence(&inertial_nav );
#endif

////////////////////////////////////////////////////////////////////////////////
// function definitions to keep compiler from complaining about undeclared functions
//////////////////////////////////// ////////////////////////////////////////////

103
void get_throttle_althold (int32_t target_alt , int16_t min_climb_rate , int16_t
max_climb_rate );
static void pre_arm_checks (bool display_failure );

///////////////////////////////////////////////////////////////// ///////////////
// Top-level logic
////////////////////////////////////////////////////////////////////////////////

// setup the var_info table
AP_Param param_loader (var_info , WP_START_BYTE );

/*
scheduler table – all regular tasks apart from the fast_l oop()
should be listed here, along with how often they should be called
(in 10ms units) and the maximum time they are expected to take (in
microseconds)
*/
static const AP_Scheduler: :Task scheduler_tasks [] PROGMEM = {
{ update_GPS , 2, 1150 }, // 0
{ update_navigation , 10, 500 }, // 1
{ medium_loop , 2, 700 }, // 2
{ update_altitude , 10, 1000 }, // 3
{ fifty_hz_loop , 2, 1300 }, // 4
{ run_nav_updates , 10, 1150 }, // 5
{ slow_loop , 10, 550 }, // 6
{ gcs_check_input , 2, 1300 }, // 7
{ gcs_send_heartbeat , 100, 700 }, // 8
{ gcs_data_stream_send , 2, 1500 }, // 9
{ gcs_send_deferred , 2, 1200 }, // 10
{ compass_a ccumulate , 2, 1200 }, // 11
{ barometer_accumulate , 2, 800 }, // 12
{ super_slow_loop , 100, 1100 }, // 13
{ perf_update , 1000, 550 } // 14
};

void setup() {
// this needs to be the first call, as it fills mem ory with
// sentinel values
memcheck_init ();
cliSerial = hal.console;

// Load the default values of variables listed in var_info[]s
AP_Param: :setup_sketch_defaults ();

#if CONFIG_SONAR == ENABLED
#if CONFIG_SONAR_SOURCE == SONAR_SOURC E_ADC
sonar_analog_source = new AP_ADC_AnalogSource (
&adc, CONFIG_SONAR_SOURCE_ADC_CHANNEL , 0.25);
#elif CONFIG_SONAR_SOURCE == SONAR_SOURCE_ANALOG_PIN
sonar_analog_source = hal.analogin ->channel(
CONFIG_SONAR_SOURCE_ANALOG _PIN);
#else

104
#warning "Invalid CONFIG_SONAR_SOURCE"
#endif
sonar = new AP_RangeFinder_MaxsonarXL (sonar_analog_source ,
&sonar_mode_filter );
#endif

rssi_analog_source = hal.analogin ->channel(g.rssi_pin );
batt_volt_analog_sou rce = hal.analogin ->channel(g.battery_volt_pin );
batt_curr_analog_source = hal.analogin ->channel(g.battery_curr_pin );
board_vcc_analog_source = hal.analogin ->channel(ANALOG_INPUT_BOARD_VCC );

init_ardupilot ();

// initialise the main loop s cheduler
scheduler .init(&scheduler_tasks [0],
sizeof(scheduler_tasks )/sizeof(scheduler_tasks [0]));
}

/*
if the compass is enabled then try to accumulate a reading
*/
static void compass_accumulate (void)
{
if (g.compass_enabled ) {
compass.accumulate ();
}
}

/*
try to accumulate a baro reading
*/
static void barometer_accumulate (void)
{
barometer .accumulate ();
}

static void perf_update (void)
{
if (g.log_bitmask & MASK_LOG_PM )
Log_Write_Performance ();
if (scheduler.debug()) {
cliSerial ->printf_P (PSTR("PERF: %u/%u %lu \n"),
(unsigned )perf_info_get_num_long_running (),
(unsigned )perf_info_get_num_loops (),
(unsigned long)perf_info_get_max_time ());
}
perf_info_reset ();
gps_fix_count = 0;
pmTest1 = 0;
}

void loop()

105
{
uint32_t timer = micros();

// We want this to execute fast
// –––––––––-
if (ins.num_samples_available () >= 1) {

// check loop time
perf_info_check_loop_time (timer – fast_loopTimer );

G_Dt = (float)(timer – fast_loopTimer ) / 1000000.f;
// used by PI Loops
fast_loopTimer = timer;

// for mainloop failure monitoring
mainLoop_count ++;

// Execute the fast loop
// –––––––
fast_loop ();

// tell the scheduler one tick has passed
scheduler .tick();

// run all th e tasks that are due to run. Note that we only
// have to call this once per loop, as the tasks are scheduled
// in multiples of the main loop tick. So if they don't run on
// the first call to the scheduler they won't run on a late r
// call until scheduler.tick() is called again
uint32_t time_available = (timer + 10000) – micros();
scheduler .run(time_available );
}
}

// Main loop – 100hz
static void fast_loop ()
{
// IMU DCM Algorithm
// ––– ––––
read_AHRS (); // ~3300us

// reads all of the necessary trig functions for cameras, throttle, etc.
// –––––––––––––––––––––––
update_trig (); // 430us

// Acrobatic control
if (ap.do_flip) {
if(abs(g.rc_1.control_in ) < 4000) {
// calling roll_flip will override the desired roll rate and throttle output
roll_flip ();
}else{

106
// force an exit from the loop if we are not hands off sti cks.
ap.do_flip = false;
Log_Write_Event (DATA_EXIT_FLIP );
}
}

// run low level rate controllers that only require IMU data
run_rate_controllers (); // 536us / 680us

// write out the servo PWM values
// ––––––––––
set_servos_4 ();

// Inertial Nav
// –––––––
read_inertia ();

// optical flow
// –––––––
#if OPTFLOW == ENABLED
if(g.optflow_enabled ) {
update_optical_flow ();
}
#endif // OPTFLOW == ENABLED

// Read radio and 3 -position switch on radio
// ––––––––––––––
read_radio ();
read_control_switch ();

// custom code/exceptions for flight modes
// ––––––––- –––––
update_yaw_mode ();
update_roll_pitch_mode ();

// update targets to rate controllers
update_rate_contoller_targets ();

// agmatthews – USERHOOKS
#ifdef USERHOOK_FASTLOOP
USERHOOK_FASTLOOP
#endif
}

// stuff that happen s at 50 hz
// –––––––––
static void fifty_hz_loop ()
{
// get altitude and climb rate from inertial lib
read_inertial_altitude ();

// Update the throttle ouput

107
// ––––––––-
update_throttle_mode ();

#if TOY_EDF == ENABLED
edf_toy();
#endif

// check auto_armed status
update_auto_armed ();

#ifdef USERHOOK_50HZLOOP
USERHOOK_50HZLOOP
#endif

#if HIL_MODE != HIL_MODE_DISABLED && FRAME_CONFIG != HELI_FRAME
// HIL for a copter needs very fas t update of the servo values
gcs_send_message (MSG_RADIO_OUT );
#endif

#if MOUNT == ENABLED
// update camera mount's position
camera_mount .update_mount_position ();
#endif

#if MOUNT2 == ENABLED
// update camera mount's position
camera_mo unt2.update_mount_position ();
#endif

#if CAMERA == ENABLED
camera.trigger_pic_cleanup ();
#endif

# if HIL_MODE == HIL_MODE_DISABLED
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST && motors.armed()) {
Log_Write_Attitude ();
#if SECONDARY_DMP_ENA BLED == ENABLED
Log_Write_DMP ();
#endif
}

if (g.log_bitmask & MASK_LOG_IMU && motors.armed())
DataFlash .Log_Write_IMU (&ins);
#endif

}

// medium_loop – runs at 10hz
static void medium_loop ()
{
// This is the start of the mediu m (10 Hz) loop pieces

108
// ––––––––––––––
switch(medium_loopCounter ) {

// This case reads from the battery and Compass
//–––––––––––––––
case 0:
medium_loopCounter ++;

// read battery before compass because it may be used for motor interference
compensation
if (g.battery_monitoring != 0) {
read_battery ();
}

#if HIL_MODE != HIL_MODE_ATTITUDE
// don't execute in HIL mode
if(g.compass_enabled ) {
if (compass.read()) {
compass.null_offsets ();
}
// log compass information
if (motors.armed() && (g.log_bitmask & MASK_LOG_COMPASS )) {
Log_Write_Compass ();
}
}
#endif

// record throttle output
// ––––––––––
throttle_integrator += g.rc_3.servo_out ;
break;

// This case reads rssi information and performs auto trim
//–––––––––––––––––––
case 1:
medium_loopCounter ++;

// read receiver rssi information
read_receiver_rssi ();

// auto_trim – stores roll and pit ch radio inputs to ahrs
auto_trim ();
break;

// This case deals with aux switches and toy mode's throttle
//–––––––––––––––––––-
case 2:
medium_loopCounter ++;

// check ch7 a nd ch8 aux switches
read_aux_switches ();

109
if(control_mode == TOY_A) {
update_toy_throttle ();

if(throttle_mode == THROTTLE_AUTO ) {
update_toy_altitude ();
}
}
break;

// This case deals with logging attitude and motor information to dataflash
// ––––––––––––––––––––––––
case 3:
medium_loopCounter ++;

if(motors.armed()) {
if (g.log_bitmask & MASK_LOG_ATTITUDE_MED ) {
Log_Write_Attitude ();
#if SECONDARY_DMP_ENABLED == ENABLED
Log_Write_DMP ();
#endif
}
if (g.log_bitmask & MASK_LOG_MOTORS )
Log_Write_Motors ();
}
break;

// This case deals with arming checks, copter LEDs and 10hz user hooks
// ––––––––––––––––––––––––––-
case 4:
medium_loopCounter = 0;

// Check for motor arming or d isarming
arm_motors_check ();

// agmatthews – USERHOOKS
#ifdef USERHOOK_MEDIUMLOOP
USERHOOK_MEDIUMLOOP
#endif

// update board leds
update_board_leds ();
#if COPTER_LEDS == ENABLED
update_copter_leds ();
#endif
break;

default:
// this is just a catch all
// ––––––––
medium_loopCounter = 0;
break;
}

110
}

// slow_loop – 3.3hz loop
static void slow_loop ()
{
// This is the slow (3 1/3 Hz) loop pieces
//–––––––––––––-
switch (slow_loopCounter ) {
case 0:
slow_loopCounter ++;
superslow_loopCounter ++;

// check if we've lost contact with the ground station
failsafe_gcs_check ();

// record if the compass is healthy
set_compass_healthy (compass.healthy);

if(superslow_loopCounter > 1200) {
#if HIL_MODE != HIL_MODE_ATTITUDE
if(g.rc_3.control_in == 0 && control_mode == STABILIZE && g.compass_enabled ) {
compass.save_offsets ();
superslow_loopCounter = 0;
}
#endif
}

if(!motors.armed()) {
// check the user hasn't updated the frame orientation
motors.set_frame_orientation (g.frame_orientation );
}

#if AC_FENCE == ENABLED
// check if we have breached a fence
fence_check ();
#endif // AC_FENCE_ENABLED

break;

case 1:
slow_loopCounter ++;

#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
update_a ux_servo_function (&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8, &g.rc_9, &g.rc_10,
&g.rc_11, &g.rc_12);
#elif MOUNT == ENABLED
update_aux_servo_function (&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8, &g.rc_10, &g.rc_11);
#endif
enable_aux_servos ();

#if MOUNT == ENABLED
camera_mount .update_mount_type ();

111
#endif

#if MOUNT2 == ENABLED
camera_mount2 .update_mount_type ();
#endif

// agmatthews – USERHOOKS
#ifdef USERHOOK_SLOWLOOP
USERHOOK_SLOWLOOP
#endif

break;

case 2:
slow_loopCounter = 0;
update_events ();

if(g.radio_tuning > 0)
tuning();

#if USB_MUX_PIN > 0
check_usb_mux ();
#endif
break;

default:
slow_loopCounter = 0;
break;
}
}

// super_slow _loop – runs at 1Hz
static void super_slow_loop ()
{
if (g.log_bitmask != 0) {
Log_Write_Data (DATA_AP_STATE , ap.value);
}

// pass latest alt hold kP value to navigation controller
wp_nav.set_althold_kP (g.pi_alt_hold .kP());

// log battery info to the dataflash
if ((g.log_bitmask & MASK_LOG_CURRENT ) && motors.armed())
Log_Write_Current ();

// perform pre -arm checks
pre_arm_checks (false);

// auto disarm checks
auto_disarm_check ();

// agmatthews – USERHOOKS

112
#ifdef USERHOOK_SUPERSLOWLOOP
USERHOOK_SUPERSLOWLOOP
#endif
}

// called at 100hz but data from sensor only arrives at 20 Hz
#if OPTFLOW == ENABLED
static void update_optical_flow (void)
{
static uint32_t last_of_update = 0;
static uint8_t of_log_counter = 0;

// if new data has arrived, process it
if( optflow.last_update != last_of_update ) {
last_of_update = optflow.last_update ;
optflow.update_position (ahrs.roll, ahrs.pitch, sin_yaw, cos_yaw, current_loc .alt);
// updates internal lon and lat with estimation based on optical flow

// write to log at 5hz
of_log_counter ++;
if( of_log_counter >= 4 ) {
of_log_counter = 0;
if (g.log_bitmask & MASK_LOG_OPTFLOW ) {
Log_Write_Optflow ();
}
}
}
}
#endif // OPTFLOW == ENABLED

// called at 50hz
static void update_GPS (void)
{
// A counter that is used to grab at least 10 reads before commiting the Home location
static uint8_t g round_start_count = 10;

g_gps->update();
update_GPS_light ();

set_gps_healthy (g_gps->status() >= GPS::GPS_OK_FIX_3D );

if (g_gps->new_data && last_gps_time != g_gps->time && g_gps->status() >=
GPS::GPS_OK_FIX_2D ) {
// clear new da ta flag
g_gps->new_data = false;

// save GPS time so we don't get duplicate reads
last_gps_time = g_gps->time;

// log location if we have at least a 2D fix
if (g.log_bitmask & MASK_LOG_GPS && motors.armed()) {
DataFlash .Log_Write_GPS (g_gps, current_loc .alt);

113
}

// for performance monitoring
gps_fix_count ++;

// check if we can initialise home yet
if (!ap.home_is_set ) {
// if we have a 3d lock and valid location
if(g_gps->status() >= GPS::GPS_OK_FIX_3D && g_gps->latitude != 0) {
if( ground_start_count > 0 ) {
ground_start_count –;
}else{
// after 10 successful reads store home location
// ap.home_is_set will be true so this will only happen once
ground_start_count = 0;
init_home ();
if (g.compass_enabled ) {
// Set compass declination automatically
compass.set_initial_location (g_gps->latitude , g_gps->longitude );
}
}
}else{
// start again if we lose 3d lock
ground_start_cou nt = 10;
}
}
}

// check for loss of gps
failsafe_gps_check ();
}

// set_yaw_mode – update yaw mode and initialise any variables required
bool set_yaw_mode (uint8_t new_yaw_mode )
{
// boolean to ensure proper initialisati on of throttle modes
bool yaw_initialised = false;

// return immediately if no change
if( new_yaw_mode == yaw_mode ) {
return true;
}

switch( new_yaw_mode ) {
case YAW_HOLD:
case YAW_ACRO:
yaw_initi alised = true;
break;
case YAW_LOOK_AT_NEXT_WP:
if( ap.home_is_set ) {
yaw_initialised = true;
}

114
break;
case YAW_LOOK_AT_LOCATION:
if( ap.home_is_set ) {
// update bearing – assumes yaw_look_at_WP has been intialised before
set_yaw_mode was called
yaw_look_at_WP_bearing = pv_get_bearing_cd (inertial_nav .get_position (),
yaw_look_at_WP );
yaw_initialised = true;
}
break;
case YAW_CIRCLE:
if( ap.home_is_set ) {
// set yaw to point to center of circle
yaw_look_at_WP = circle_center ;
// initialise bearing to current heading
yaw_look_at_WP_bearing = ahrs.yaw_sensor ;
yaw_initialised = true;
}
break;
case YAW_LOOK_AT_HEADING:
yaw_initialised = true;
break;
case YAW_LOOK_AT_HOME:
if( ap.home_is_set ) {
yaw_initialised = true;
}
break;
case YAW_LOOK_AHEAD:
if( ap.home_is_set ) {
yaw_initialised = true;
}
break;
case YAW_TOY:
yaw_initialised = true;
break;
case YAW_RESETTOARMEDYAW:
nav_yaw = ahrs.yaw_sensor ; // store current yaw so we can start rotating back
to correct one
yaw_initialised = true;
break;
}

// if initialisation has been successful update the yaw mode
if( yaw_initialised ) {
yaw_mode = new_yaw_mode ;
}

// return success or failure
return yaw_initialised ;
}

// update_yaw_mode – run high level yaw controllers

115
// 100hz updat e rate
void update_yaw_mode (void)
{
switch(yaw_mode ) {

case YAW_HOLD:
// heading hold at heading held in nav_yaw but allow input from pilot
get_yaw_rate_stabilized_ef (g.rc_4.control_in );
break;

case YAW_ACRO:
// pilot controlled yaw using rate controller
if(g.axis_enabled ) {
get_yaw_rate_stabilized_ef (g.rc_4.control_in );
}else{
get_acro_yaw (g.rc_4.control_in );
}
break;

case YAW_LOOK_AT_NEXT_WP:
// point towards next waypoint (no pilot input accepted)
// we don't use wp_bearing because we don't want the copter to turn too much during
flight
nav_yaw = get_yaw_slew (nav_yaw, original_wp_bearing , AUTO_YAW_SLEW_RATE );
get_stabi lize_yaw (nav_yaw);

// if there is any pilot input, switch to YAW_HOLD mode for the next iteration
if( g.rc_4.control_in != 0 ) {
set_yaw_mode (YAW_HOLD );
}
break;

case YAW_LOOK_AT_LOCATION:
// point towards a location held in yaw_look_at_WP
get_look_at_yaw ();

// if there is any pilot input, switch to YAW_HOLD mode for the next iteration
if( g.rc_4.control_in != 0 ) {
set_yaw_mode (YAW_HOLD );
}
break;

case YAW_CIRCLE:
// points toward the center of the circle or does a panorama
get_circle_yaw ();

// if there is any pilot input, switch to YAW_HOLD mode for the next iteration
if( g.rc_4.control_in != 0 ) {
set_yaw_mode (YAW_HOLD );
}
break;

116
case YAW_LOOK_AT_HOME:
// keep heading always pointing at home with no pilot input allowed
nav_yaw = get_yaw_slew (nav_yaw, home_bearing , AUTO_YAW_SLEW_RATE );
get_stabilize_yaw (nav_yaw);

// if there is any pilot input, switch to YAW_HOLD mode for the next iteration
if( g.rc_4.control_in != 0 ) {
set_yaw_mode (YAW_HOLD );
}
break;

case YAW_LOOK_AT_HEADING:
// keep heading poin ting in the direction held in yaw_look_at_heading with no pilot
input allowed
nav_yaw = get_yaw_slew (nav_yaw, yaw_look_at_heading , yaw_look_at_heading_slew );
get_stabilize_yaw (nav_yaw);
break;

case YAW_LOOK_AHEAD:
// Commanded Y aw to automatically look ahead.
get_look_ahead_yaw (g.rc_4.control_in );
break;

#if TOY_LOOKUP == TOY_EXTERNAL_MIXER
case YAW_TOY:
// update to allow external roll/yaw mixing
// keep heading always pointing at home with n o pilot input allowed
nav_yaw = get_yaw_slew (nav_yaw, home_bearing , AUTO_YAW_SLEW_RATE );
get_stabilize_yaw (nav_yaw);
break;
#endif

case YAW_RESETTOARMEDYAW:
// changes yaw to be same as when quad was armed
nav_yaw = get_yaw_slew (nav_yaw, initial_armed_bearing , AUTO_YAW_SLEW_RATE );
get_stabilize_yaw (nav_yaw);

// if there is any pilot input, switch to YAW_HOLD mode for the next iteration
if( g.rc_4.control_in != 0 ) {
set_yaw_mo de(YAW_HOLD );
}

break;
}
}

// get yaw mode based on WP_YAW_BEHAVIOR parameter
// set rtl parameter to true if this is during an RTL
uint8_t get_wp_yaw_mode (bool rtl )
{
switch (g.wp_yaw_behavior ) {
case WP_YAW_BEHAVIOR_LOOK _AT_NEXT_WP:

117
return YAW_LOOK_AT_NEXT_WP ;
break;

case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL:
if( rtl ) {
return YAW_HOLD ;
}else{
return YAW_LOOK_AT_NEXT_WP ;
}
break;

case WP_YAW_BEHAVIOR_LOOK_AHEAD:
return YAW_LOOK_AHEAD ;
break;

default:
return YAW_HOLD ;
break;
}
}

// set_roll_pitch_mode – update roll/pitch mode and initi alise any variables as required
bool set_roll_pitch_mode (uint8_t new_roll_pitch_mode )
{
// boolean to ensure proper initialisation of throttle modes
bool roll_pitch_initialised = false;

// return immediately if no change
if( new_roll_pitch _mode == roll_pitch_mode ) {
return true;
}

switch( new_roll_pitch_mode ) {
case ROLL_PITCH_STABLE:
case ROLL_PITCH_ACRO:
case ROLL_PITCH_AUTO:
case ROLL_PITCH_STABLE_OF:
case ROLL_PITCH_TOY:
roll_pitch_initialised = true;
break;

case ROLL_PITCH_LOITER:
// require gps lock
if( ap.home_is_set ) {
roll_pitch_initialised = true;
}
break;
}

// if init ialisation has been successful update the yaw mode
if( roll_pitch_initialised ) {
roll_pitch_mode = new_roll_pitch_mode ;

118
}

// return success or failure
return roll_pitch_initialised ;
}

// update_roll_pitch_mode – run high level ro ll and pitch controllers
// 100hz update rate
void update_roll_pitch_mode (void)
{
switch(roll_pitch_mode ) {
case ROLL_PITCH_ACRO:
// copy user input for reporting purposes
control_roll = g.rc_1.control_in ;
control_pitch = g.rc_2.control_in ;

#if FRAME_CONFIG == HELI_FRAME
if(g.axis_enabled ) {
get_roll_rate_stabilized_ef (g.rc_1.control_in );
get_pitch_rate_stabilized_ef (g.rc_2.control_in );
}else{
// ACRO does no t get SIMPLE mode ability
if (motors.flybar_mode == 1) {
g.rc_1.servo_out = g.rc_1.control_in ;
g.rc_2.servo_out = g.rc_2.control_in ;
} else {
get_acro_roll (g.rc_1.control_in );
get_acro_pitch (g.rc_2.control_in );
}
}
#else // !HELI_FRAME
if(g.axis_enabled ) {
get_roll_rate_stabilized_ef (g.rc_1.control_in );
get_pitch_rate_stabilized_ef (g.rc_2.control_in );
}else{
// ACRO does not get SIMPLE mode ability
get_acro_roll (g.rc_1.control_in );
get_acro_pitch (g.rc_2.control_in );
}
#endif // HELI_FRAME
break;

case ROLL_PITCH_STABLE:
// apply SIMPLE mode transform
if(ap.simple_mode && ap_system .new_radio_frame ) {
update_simple_mode ();
}

control_roll = g.rc_1.control_in ;
control_pitch = g.rc_2.control_in ;

get_stabilize_roll (control_roll );

119
get_stabili ze_pitch (control_pitch );

break;

case ROLL_PITCH_AUTO:
// copy latest output from nav controller to stabilize controller
nav_roll = wp_nav.get_desired_roll ();
nav_pitch = wp_nav.get_desired_pitch ();
get_stabiliz e_roll(nav_roll );
get_stabilize_pitch (nav_pitch );

// copy control_roll and pitch for reporting purposes
control_roll = nav_roll ;
control_pitch = nav_pitch ;
break;

case ROLL_PITCH_STABLE_OF:
// apply SIM PLE mode transform
if(ap.simple_mode && ap_system .new_radio_frame ) {
update_simple_mode ();
}

control_roll = g.rc_1.control_in ;
control_pitch = g.rc_2.control_in ;

// mix in user cont rol with optical flow
get_stabilize_roll (get_of_roll (control_roll ));
get_stabilize_pitch (get_of_pitch (control_pitch ));
break;

// THOR
// a call out to the main toy logic
case ROLL_PITCH_TOY:
roll_pitch_toy ();
break;

case ROLL_PITCH_LOITER:
// apply SIMPLE mode transform
if(ap.simple_mode && ap_system .new_radio_frame ) {
update_simple_mode ();
}
// copy user input for logging purposes
control_roll = g.rc_1.control_in ;
control_pitch = g.rc_2.control_in ;

// update loiter target from user controls – max velocity is 5.0 m/s
wp_nav.move_loiter_target (control_roll , control_pitch ,0.01f);

// copy latest out put from nav controller to stabilize controller
nav_roll = wp_nav.get_desired_roll ();
nav_pitch = wp_nav.get_desired_pitch ();
get_stabilize_roll (nav_roll );

120
get_stabilize_pitch (nav_pitch );
break;
}

#if FRAME_CON FIG != HELI_FRAME
if(g.rc_3.control_in == 0 && control_mode <= ACRO) {
reset_rate_I ();
reset_stability_I ();
}
#endif //HELI_FRAME

if(ap_system .new_radio_frame ) {
// clear new radio frame info
ap_system .new_radi o_frame = false;
}
}

// new radio frame is used to make sure we only call this at 50hz
void update_simple_mode (void)
{
static uint8_t simple_counter = 0; // State machine counter for Simple Mode
static float simple_sin_y =0, simple_cos_x=0;

// used to manage state machine
// which improves speed of function
simple_counter ++;

int16_t delta = wrap_360_cd (ahrs.yaw_sensor – initial_simple_bearing )/100;

if (simple_counter == 1) {
// roll
simple_cos_x = sinf(radians(90 – delta));

}else if (simple_counter > 2) {
// pitch
simple_sin_y = cosf(radians(90 – delta));
simple_counter = 0;
}

// Rotate input by the initial bearing
int16_t _roll = g.rc_1.control_in * simple_cos_x + g.rc_2.control_in * simple_sin_y ;
int16_t _pitch = -(g.rc_1.control_in * simple_sin_y – g.rc_2.control_in *
simple_cos_x );

g.rc_1.control_in = _roll;
g.rc_2.control_in = _pitch;
}

// update_super_simple_bearing – adjusts simple bearing based on location
// should be called after home_bearing has been updated
void update_super_simple_bearing ()
{

121
// are we in SIMPLE mode?
if(ap.simple_mode && g.super_simple ) {
// get distance to home
if(home_distance > SUPER_SIMPLE_RADIUS ) { // 10m from home
// we reset the angular offset to be a vector from home to the quad
initial_simple_bearing = wrap_360_cd (home_bearing +18000);
}
}
}

// set_throttle_mode – sets the throttle mode and initialises any variables as required
bool set_throttle_mode ( uint8_t new_throttle_mode )
{
// boolean to ensure proper initialisation of throttle modes
bool throttle_initialised = false;

// return immediately if no change
if( new_throttle_mode == throttle_mode ) {
return true;
}

// initialise any variables required for the new throttle mode
switch(new_throttle_mode ) {
case THROTTLE_MANUAL:
case THROTTLE_MANUAL_TILT_COMPENSATED:
throttle _accel_deactivate (); // this controller does not use
accel based throttle controller
altitude_error = 0; // clear altitude error reported to
GCS
throttle_initialised = true;
break;

case THROTTLE_HOLD:
case THROTTLE_AUTO:
controller_desired_alt = get_initial_alt_hold (current_loc .alt, climb_rate );
// reset controller desired altitude to current altitude
wp_nav.set_desired_alt (controller_desi red_alt);
// same as above but for loiter controller
if ( throttle_mode <= THROTTLE_MANUAL_TILT_COMPENSATED ) { // reset the
alt hold I terms if previous throttle mode was manual
reset_thrott le_I();
set_accel_throttle_I_from_pilot_throttle (get_pilot_desired_throttle (g.rc_3.
control_in ));
}
throttle_initialised = true;
break;

case THROTTLE_LAND:
set_land_complete (false); // mark landing as incomplete
land_detector = 0; // A counter that goes up if our climb rate stalls
out.

122
controller_desired_alt = get_initial_alt_hold (current_loc .alt, climb_rate );
// reset controller desired altitude to current altitude
throttle_initialised = true;
break;

default:
// To-Do: log an error message to the dataflash or tlogs instead of printing to
the serial port
cliSerial ->printf_P (PSTR("Unsupported thr ottle mode: %d!!" ),new_throttle_mode );
break;
}

// update the throttle mode
if( throttle_initialised ) {
throttle_mode = new_throttle_mode ;

// reset some variables used for logging
desired_climb_rate = 0;
nav_throttle = 0;
}

// return success or failure
return throttle_initialised ;
}

// update_throttle_mode – run high level throttle controllers
// 50 hz update rate
void update_throttle_mode (void)
{
int16_t pilot_climb_rate ;
int16_t pilot_throttle_scaled ;

if(ap.do_flip) // this is pretty bad but needed to flip in AP modes.
return;

// do not run throttle controllers if motors disarmed
if( !motors.armed() ) {
set_throttle_out (0, false);
throttle_accel_deactivate (); // do not allow the accel based throttle to
override our command
set_target_alt_for_reporting (0);
return;
}

#if FRAME_CONFIG == HELI_FRAME
if (control_mode == STABILIZE ){
motors.stab_throttle = true;
} else {
motors.stab_throttle = false;
}
#endif // HELI_FRAME

123
switch(throttle_mode ) {

case THROTTLE_MANUAL:
// completely manual throttle
if(g.rc_3.control_in <= 0){
set_throttle_out (0, false);
}else{
// send pilot's output directly to motors
pilot_throttle_scaled = get_pilot_desired_throttle (g.rc_3.control_in );
set_throttle_out (pilot_throttle_scaled , false);

// update estimate of throttle cruise
#if FRAME_CONF IG == HELI_FRAME
update_throttle_cruise (motors.coll_out );
#else
update_throttle_cruise (pilot_throttle_scaled );
#endif //HELI_FRAME

// check if we've taken off yet
if (!ap.takeoff_complete && motors.armed()) {
if (pilot_throttle_scaled > g.throttle_cruise ) {
// we must be in the air by now
set_takeoff_complete (true);
}
}
}
set_target_alt_for_reporting (0);
break;

case THROTTLE_MANUAL_TILT_COMPENSATED:
// manual throttle but with angle boost
if (g.rc_3.control_in <= 0) {
set_throttle_out (0, false); // no need for angle boost with zero throttle
}else{
pilot_throttle_scaled = get_pilot_desired_throttle (g.rc_3.control_in );
set_throttle_out (pilot_throttle_scaled , true);

// update estimate of throttle cruise
#if FRAME_CONFIG == HELI_FRAME
update_throttle_cruise (motors.coll_out );
#else
update_throttle_cruise (pilot_throttle_scaled );
#endif //HELI_FRAME

if (!ap.takeoff_complete && motors.armed()) {
if (pilot_throttle_scaled > g.throttle_cruise ) {
// we must be in the air by now
set_takeoff_complete (true);
}
}
}

124
set_target_alt_for_reporting (0);
break;

case THROTTLE_HOLD:
if(ap.auto_armed ) {
// alt hold plus pilot inp ut of climb rate
pilot_climb_rate = get_pilot_desired_climb_rate (g.rc_3.control_in );
if( sonar_alt_health >= SONAR_ALT_HEALTH_MAX ) {
// if sonar is ok, use surface tracking
get_throttle_surface_track ing(pilot_climb_rate ); // this function calls
set_target_alt_for_reporting for us
}else{
// if no sonar fall back stabilize rate controller
get_throttle_rate_stabilized (pilot_climb_rate ); // this function calls
set_target_alt_for_reporting for us
}
}else{
// pilot's throttle must be at zero so keep motors off
set_throttle_out (0, false);
set_target_alt_for_reporting (0);
}
break;

case THROTTLE_AUTO:
// auto pilot altitude controller with target altitude held in
wp_nav.get_desired_alt()
if(ap.auto_armed ) {
get_throttle_althold_with_slew (wp_nav.get_desired_alt (), –
wp_nav.get_descent_velocity (), wp_nav.get_climb_velocity ());
set_target_alt_for_reporting (wp_nav.get_desired_alt ()); // To-Do: return
get_destination_alt if we are flying to a waypoint
}else{
// pilot's throttle must be at zero so keep motors off
set_thrott le_out(0, false);
set_target_alt_for_reporting (0);
}
break;

case THROTTLE_LAND:
// landing throttle controller
get_throttle_land ();
set_target_alt_for_reporting (0);
break;
}
}

// set_tar get_alt_for_reporting – set target altitude in cm for reporting purposes (logs
and gcs)
static void set_target_alt_for_reporting (float alt_cm)
{
target_alt_for_reporting = alt_cm;
}

125

// get_target_alt_for_reporting – returns target altitude in cm for r eporting purposes
(logs and gcs)
static float get_target_alt_for_reporting ()
{
return target_alt_for_reporting ;
}

static void read_AHRS (void)
{
// Perform IMU calculations and get attitude info
//––––––––––––––––
#if HIL_MODE != HIL_MODE_DISABLED
// update hil before ahrs update
gcs_check_input ();
#endif

ahrs.update();
omega = ins.get_gyro ();

#if SECONDARY_DMP_ENABLED == ENABLED
ahrs2.update();
#endif
}

static void update_trig (void){
Vector2f yawvector ;
const Matrix3f &temp = ahrs.get_dcm_matrix ();

yawvector .x = temp.a.x; // sin
yawvector .y = temp.b.x; // cos
yawvector .normalize ();

cos_pitch_x = safe_sqrt (1 – (temp.c.x * temp.c.x)); // level = 1
cos_roll_x = temp.c.z / cos_pitch_x ; // level = 1

cos_pitch_x = constrain_float (cos_pitch_x , 0, 1.0);
// this relies on constrain_float() of infinity doing the right thing,
// which it does do in avr -libc
cos_roll_x = constrain_float (cos_roll_x , -1.0, 1.0);

sin_yaw = constrain_float (yawvector .y, -1.0, 1.0);
cos_yaw = constrain_float (yawvector .x, -1.0, 1.0);

// added to convert earth frame to body frame for rate c ontrollers
sin_pitch = -temp.c.x;
sin_roll = temp.c.y / cos_pitch_x ;

// update wp_nav controller with trig values
wp_nav.set_cos_sin_yaw (cos_yaw, sin_yaw, cos_pitch_x );

//flat:

126
// 0 ° = cos_yaw: 1.00, sin_yaw: 0.00,
// 90° = cos_yaw: 0.00, sin_yaw: 1.00,
// 180 = cos_yaw: -1.00, sin_yaw: 0.00,
// 270 = cos_yaw: 0.00, sin_yaw: -1.00,
}

// read baro and sonar altitude at 20hz
static void update_altitude ()
{
#if HIL_MODE == HIL_MODE_ATTITUDE
// we are in the SIM, fake out the baro and Sonar
baro_alt = g_gps->altitude – gps_base_alt ;

if(g.sonar_enabled ) {
sonar_alt = baro_alt ;
}
#else
// read in baro altitude
baro_alt = read_barometer ();

// read in sonar altitude
sonar_alt = read_sonar ();
#endif // HIL_MODE == HIL_MODE_ATTITUDE

// write altitude info to dataflash logs
if ((g.log_bitmask & MASK_LOG_CTUN ) && motors.armed()) {
Log_Write_Control_Tuning ();
}
}

static void tuning(){
tuning_value = (float)g.rc_6.control_in / 1000.0f;
g.rc_6.set_range (g.radio_tuning_low ,g.radio_tuning_high ); // 0 to 1

switch(g.radio_tuning ) {

case CH6_RATE_KD:
g.pid_rate_roll .kD(tuning_value );
g.pid_rate_pitch .kD(tuning_value );
break;

case CH6_STABILIZE_KP:
g.pi_stabilize_roll .kP(tuning_value );
g.pi_stabilize_pitch .kP(tuning_value );
break;

case CH6_STABILIZE_KI:
g.pi_stabil ize_roll .kI(tuning_value );
g.pi_stabilize_pitch .kI(tuning_value );
break;

case CH6_ACRO_KP:

127
g.acro_p = tuning_value ;
break;

case CH6_RATE_KP:
g.pid_rate_roll .kP(tuning_value );
g.pid_rate_pitch .kP(tuning_value );
break;

case CH6_RATE_KI:
g.pid_rate_roll .kI(tuning_value );
g.pid_rate_pitch .kI(tuning_value );
break;

case CH6_YAW_KP:
g.pi_stabilize_yaw .kP(tuning_value );
break;

case CH6_YAW_KI:
g.pi_stabilize_yaw .kI(tuning_value );
break;

case CH6_YAW_RATE_KP:
g.pid_rate_yaw .kP(tuning_value );
break;

case CH6_YAW_RATE_KD:
g.pid_rate_yaw .kD(tuning_value );
break;

case CH6_THROTTLE_KP:
g.pid_throttle .kP(tuning_value );
break;

case CH6_THROTTLE_KI:
g.pid_throttle .kI(tuning_value );
break;

case CH6_THROTTLE_KD:
g.pid_throttle .kD(tuning_value );
break;

case CH6_RELAY:
if (g.rc_6.control_in > 525) relay.on();
if (g.rc_6.control_in < 475) relay.off();
break;

case CH6_WP_SPEED:
// set waypoint navigation horizontal speed to 0 ~ 1000 cm/s
wp_nav.set_horizontal_velocity (g.rc_6.control_in );
break;

case CH6_LOITER_KP:

128
g.pi_loiter_lat .kP(tuning_value );
g.pi_loiter_lon .kP(tuning_value );
break;

case CH6_LOITER_KI:
g.pi_loiter_lat .kI(tuning_value );
g.pi_loiter_lon .kI(tuning_value );
break;

case CH6_LOITER_RATE_KP:
g.pid_loiter_rate_lon .kP(tuning_value );
g.pid_loiter_rate_lat .kP(tuning_value );
break;

case CH6_LOITER_RATE_KI:
g.pid_loiter_rate_lon .kI(tuning_value );
g.pid_loiter_rate_lat .kI(tuning_value);
break;

case CH6_LOITER_RATE_KD:
g.pid_loiter_rate_lon .kD(tuning_value );
g.pid_loiter_rate_lat .kD(tuning_value );
break;

#if FRAME_CONFIG == HELI_FRAME
case CH6_HELI_EXTERNAL_GYRO:
motors.ext_gyro_g ain = tuning_value ;
break;
#endif

case CH6_THR_HOLD_KP:
g.pi_alt_hold .kP(tuning_value );
break;

case CH6_OPTFLOW_KP:
g.pid_optflow_roll .kP(tuning_value );
g.pid_optflow_pitch .kP(tuning_value );
break;

case CH6_OPTFLOW_KI:
g.pid_optflow_roll .kI(tuning_value );
g.pid_optflow_pitch .kI(tuning_value );
break;

case CH6_OPTFLOW_KD:
g.pid_optflow_roll .kD(tuning_value );
g.pid_optflow_pitch .kD(tuning_value );
break;

#if HIL_MODE != HIL_MODE_ATTITUDE // do not allow
modifying _kp or _kp_yaw gains in HIL mode
case CH6_AHRS_YAW_KP:

129
ahrs._kp_yaw.set(tuning_value );
break;

case CH6_AHRS_KP:
ahrs._kp.set(tuning_value );
break;
#endif

case CH6_INAV_TC:
// To-Do: allowing tuning TC for xy and z separately
inertial_nav .set_time_constant_xy (tuning_value );
inertial_nav .set_time_constant_z (tuning_value );
break;

case CH6_THR_ACCEL_KP:
g.pid_throttle_accel .kP(tuning_value );
break;

case CH6_THR_ACCEL_KI:
g.pid_throttle_accel .kI(tuning_value );
break;

case CH6_THR_ACCEL_KD:
g.pid_throttle_accel .kD(tuning_value );
break;

case CH6_DECLINATION:
// set declination to + -20degrees
compass.set_declination (ToRad((2.0f * g.rc_6.control_in –
g.radio_tuning_high )/100.0f), false); // 2nd parameter is false because we do not want
to save to eep rom because this would have a performance impact
break;

case CH6_CIRCLE_RATE:
// set circle rate
g.circle_rate .set(g.rc_6.control_in /25-20); // allow approximately 45 degree
turn rate in either direction
//cliSerial ->printf_P(PSTR(" \nRate:%4.2f"),(float)g.circle_rate);
break;
}
}

AP_HAL_MAIN ();

130

APM_Config.h

// -*- tab-width: 4; Mode: C++; c -basic-offset: 4; indent -tabs-mode: nil -*-

// User specific config file. Any items listed in config.h can be o verridden here.

// Select Megapirate board type:
//#define MPNG_BOARD_TYPE CRIUS_V1
/*
RCTIMER_CRIUS_V2 – (DEFAULT!!!) Use ONLY for RCTimer CRIUS V2 board
CRIUS_V1 – RCTimer CRIUS V1(1.1) board and all HobbyKing AIOP boards
HK_RED_MULTIWII_PRO – HobbyKing MultiWii Pro RED board with ITG3205 and BMA180, BMP085
sensors
BLACK_VORTEX
MULTIWII_PRO_EZ3_BLACK – ReadyToFlyQuads – MultiWii PRO Ez3.0 Blacked MAG Editon Flight
Controller w/ GPS Option (NO COMPASS)
PARIS_V5_OSD – PARIS v5 Mega iOSD –
http://www.multiwiicopter.com/products/multiwii -paris-v5-mega-iosd-gps-autopilot
*/

// Currently not supported
#define CONFIG_SONAR DISABLED

//#define COPTER_LEDS DISABLED

// GPS port speed (Serial2) 38400 by default
//#define SERIAL2_BAUD 38400

// GPS driver selection
//#define GPS_PROTOCOL GPS_PROTOCOL_NONE
/*
GPS_PROTOCOL_AUTO (Default)
GPS_PROTOCOL_NONE
GPS_PROTOCOL_NMEA
GPS_PROTOCOL_SIRF
GPS_PROTOCOL_UBLOX
GPS_PROTOCOL_IMU
GPS_PROTOCOL_MTK
GPS_PROTOCOL_HIL
GPS_PROTOCOL_MTK19
*/

// QuadCopter selected by default
//#define FRAME_CONFIG QUAD_FRAME
/*
* options:
* QUAD_FRAME
* TRI_FRAME
* QUAD_FRAME
* Y6_FRAME
* OCTA_FRAME

131
* OCTA_QUAD_FRAME
* HELI_FRAME
*/

//#define HIL_MODE HIL_MO DE_SENSORS // build for hardware -in-the-loop
simulation
//#define HIL_MODE HIL_MODE_ATTITUDE // build for hardware -in-the-loop
simulation

// If Arduino IDE hang while uploading firmware to your board, try to change string below,
just enter some random characters
#define BOOTLOADER_BUGFIX "234fs34567"

// User Hooks : For User Developed code that you wish to run
// Put your variable definitions into the UserVariables.h file (or another file name and
then change the #define below).
//#define USERHOOK_VARIABLES "UserVariables.h"
// Put your custom code into the UserCode.pde with function names matching those listed
below and ensure the appropriate #define below is uncommented below
//#define USERHOOK_INIT userhook_init(); // for code to be run once at
startup
//#define USERHOOK_FASTLOOP userhook_FastLoop(); // for code to be run at 100hz
//#define USERHOOK_50HZLOOP userhook_50Hz(); // for code to be run at 50hz
//#define USERHOOK_MEDIUMLOOP userhoo k_MediumLoop(); // for code to be run at 10hz
//#define USERHOOK_SLOWLOOP userhook_SlowLoop(); // for code to be run at 3.3hz
//#define USERHOOK_SUPERSLOWLOOP userhook_SuperSlowLoop(); // for code to be run at 1hz

132

APM_Config_mavlink_h il.h

B. // -*- tab-width: 4; Mode: C++; c -basic-offset: 4; indent -tabs-mode: nil -*-
C.
D. // HIL_MODE SELECTION
E. //
F. // Mavlink supports
G. // 1. HIL_MODE_ATTITUDE : simulated position, airspeed, and attitude
H. // 2. HIL_MODE_SENSORS: full sensor simulation
I. #define HI L_MODE HIL_MODE_ATTITUDE
J.
K. // HIL_PORT SELCTION
L. //
M. // PORT 1
N. // If you would like to run telemetry communications for a groundstation
O. // while you are running hardware in the loop it is necessary to set
P. // HIL_PORT to 1. This uses the port that w ould have been used for the gps
Q. // as the hardware in the loop port. You will have to solder
R. // headers onto the gps port connection on the apm
S. // and connect via an ftdi cable.
T. //
U. // The baud rate is set to 115200 in this mode.
V. //
W. // PORT 3
X. // If you don' t require telemetry communication with a gcs while running
Y. // hardware in the loop you may use the telemetry port as the hardware in
Z. // the loop port. Alternatively, use a telemetry/HIL shim like FGShim
AA. // https://ardupilot -mega.googlecode.com/svn/Tools/t runk/FlightGear
BB. //
CC. // The buad rate is controlled by SERIAL3_BAUD in this mode.
DD.
EE. #define HIL_PORT 3
FF.
GG. // You can set your gps protocol here for your actual
HH. // hardware and leave it without affecting the hardware
II. // in the loop simulation
JJ. #define GPS_PROTOCOL GPS_PROTOCOL_MTK
KK.
LL. // Sensors
MM. // All sensors are supported in all modes.
NN. // The magnetometer is not used in
OO. // HIL_MODE_ATTITUDE but you may leave it
PP. // enabled if you wish.
QQ. #define MAGNETOMETER ENABLED
// -*- tab-width: 4; Mo de: C++; c -basic-offset: 4; indent -tabs-mode: nil -*-

void set_home_is_set (bool b)
{
// if no change, exit immediately

133
if( ap.home_is_set == b )
return;

ap.home_is_set = b;
if(b) {
Log_Write_Event (DATA_SET_HOME );
}
}

// –––––––––––––––
void set_auto_armed (bool b)
{
// if no change, exit immediately
if( ap.auto_armed == b )
return;

ap.auto_armed = b;
if(b){
Log_Write_Event (DATA_AUTO_ARMED );
}
}

// –––––––––––––––
void set_simple_mode (bool b)
{
if(ap.simple_mode != b){
if(b){
Log_Write_Event (DATA_SET_SIMPLE_ON );
}else{
Log_Write_Event (DATA_SET_SIMPLE_OFF );
}
ap.simple_mode = b;
}
}

// –––––––––––––––
static void set_failsafe_radio (bool mode)
{
// only act on changes
// ––––––-
if(ap.failsafe_radio != mode) {

// store the value so we don't trip the g ate twice
// ––––––––––––––––
ap.failsafe_radio = mode;

if (ap.failsafe_radio == false) {
// We've regained radio contact
// –––––––––-
failsafe_r adio_off_event ();
}else{
// We've lost radio contact

134
// ––––––––
failsafe_radio_on_event ();
}
}
}

// –––––––––––––––
void set_low_battery (bool b)
{
ap.low_battery = b;
}

// –––––––––––––––
static void set_failsafe_gps (bool mode)
{
ap.failsafe_gps = mode;
}

// –––––––––––––––
static void set_failsafe_gcs (bool mode)
{
ap.failsafe_gcs = mode;
}

// –––––––––––––––
void set_takeoff_complete (bool b)
{
// if no change, exit immediately
if( ap.takeoff_complete == b )
return;

if(b){
Log_Write_Event (DATA_TAKEOFF );
}
ap.takeoff_complete = b;
}

// –––––––––––––––
void set_land_complete (bool b)
{
// if no change, exit immediately
if( ap.land_complete == b )
return;

if(b){
Log_Write_Event (DATA_LAND_COMPLETE );
}
ap.land_complete = b;
}

135
// –––––––––––––––

void set_compass_healthy (bool b)
{
if(ap.compass_status != b) {
if(b) {
// compass has just recovered so log to the dataflash
Log_Write_Error (ERROR_SUBSYSTEM_COMPASS ,ERROR_CODE_ERROR_RESOLVED );
}else{
// compass has just failed so log an error to the dataflash
Log_Write_Error (ERROR_SUBSYSTEM_COMPASS ,ERROR_CODE_COMPASS_FAILED_TO_READ );
}
}
ap.compass_sta tus = b;
}

void set_gps_healthy (bool b)
{
if(ap.gps_status != b){
if(false == b){
Log_Write_Event (DATA_LOST_GPS );
}
}
ap.gps_status = b;
}

136

/// -*- tab-width: 4; Mode: C++; c -basic-offset: 4; indent -tabs-mode: nil -*-

static void
get_stabilize_roll (int32_t target_angle )
{
// angle error
target_angle = wrap_180_cd (target_angle – ahrs.roll_sensor );

// limit the error we're feeding to the PID
target_angle = constrain_int32 (target_angle, -4500, 4500);

// convert to desired rate
int32_t target_rate = g.pi_stabilize_roll .get_pi(target_angle , G_Dt);

// set targets for rate controller
set_roll_rate_target (target_rate , EARTH_FRAME );
}

static void
get_stabilize_pitch (int32_t target_angle )
{
// angle error
target_angle = wrap_180_cd (target_angle – ahrs.pitch_sensor );

// limit the error we're feeding to the PID
target_angle = constrain_int32 (target_angle , -4500, 4500);

// conver t to desired rate
int32_t target_rate = g.pi_stabilize_pitch .get_pi(target_angle , G_Dt);

// set targets for rate controller
set_pitch_rate_target (target_rate , EARTH_FRAME );
}

static void
get_stabilize_yaw (int32_t target_angle )
{
int32_t t arget_rate ,i_term;
int32_t angle_error ;
int32_t output = 0;

// angle error
angle_error = wrap_180_cd (target_angle – ahrs.yaw_sensor );

// limit the error we're feeding to the PID

angle_error = constrain_int 32(angle_error , -4500, 4500);

// convert angle error to desired Rate:

137
target_rate = g.pi_stabilize_yaw .get_p(angle_error );
i_term = g.pi_stabilize_yaw .get_i(angle_error , G_Dt);

// do not use rate controllers for helicotpers with external gyros
#if FRAME_CONFIG == HELI_FRAME
if(motors.ext_gyro_enabled ) {
g.rc_4.servo_out = constrain_int32 ((target_rate + i_term), -4500, 4500);
}
#endif

#if LOGGING_ENABLED == ENABLED
// log output if PID logging is on and we are tuning th e yaw
if( g.log_bitmask & MASK_LOG_PID && g.radio_tuning == CH6_YAW_KP ) {
pid_log_counter ++;
if( pid_log_counter >= 10 ) { // (update rate / desired output rate)
= (100hz / 10hz) = 10
pid_log_counter = 0;
Log_Write_PID (CH6_YAW_KP , angle_error , target_rate , i_term, 0, output,
tuning_value );
}
}
#endif

// set targets for rate controller
set_yaw_rate_target (target_rate +i_term, EARTH_FRAME );
}

static void
get_acro_roll (int32_t targ et_rate)
{
target_rate = target_rate * g.acro_p;

// set targets for rate controller
set_roll_rate_target (target_rate , BODY_FRAME );
}

static void
get_acro_pitch (int32_t target_rate )
{
target_rate = target_rate * g.acro_p;

// set targe ts for rate controller
set_pitch_rate_target (target_rate , BODY_FRAME );
}

static void
get_acro_yaw (int32_t target_rate )
{
target_rate = target_rate * g.acro_p;

// set targets for rate controller
set_yaw_rate_target (target_rate , BODY_FRAME );

138
}

// Roll with rate input and stabilized in the earth frame
static void
get_roll_rate_stabilized_ef (int32_t stick_angle )
{
int32_t angle_error = 0;

// convert the input to the desired roll rate
int32_t target_rate = stick_angle * g.acro_p – (roll_axis * g.acro_balance_roll )/100;

// convert the input to the desired roll rate
roll_axis += target_rate * G_Dt;
roll_axis = wrap_180_cd (roll_axis );

// ensure that we don't reach gimbal lock
if (labs(roll_axis ) > 4500 && g.acro_trainer_enabled ) {
roll_axis = constrain_int32 (roll_axis , -4500, 4500);
angle_error = wrap_180_cd (roll_axis – ahrs.roll_sensor );
} else {
// angle error with maximum of + – max_angle_overshoot
angle_error = wrap_180_cd (roll_axis – ahrs.roll_sensor );
angle_error = constrain_int32 (angle_error , -MAX_ROLL_OVERSHOOT ,
MAX_ROLL_OVERSHOOT );
}

// reset target angle to current angle if motors not spinning
if (!motors.armed() || g.rc_3.servo_out == 0) {
angle_error = 0;
}

// update roll_axis to be within max_angle_overshoot of our current heading
roll_axis = wrap_180_cd (angle_error + ahrs.roll_sensor );

// set earth frame targets for rate controller
set_roll_rate_target (g.pi_stabilize_rol l.get_p(angle_error ) + target_rate ,
EARTH_FRAME );
}

// Pitch with rate input and stabilized in the earth frame
static void
get_pitch_rate_stabilized_ef (int32_t stick_angle )
{
int32_t angle_error = 0;

// convert the input to the desired pitch rate
int32_t target_rate = stick_angle * g.acro_p – (pitch_axis * g.acro_balance_pitch )/100;

// convert the input to the desired pitch rate
pitch_axis += target_rate * G_Dt;
pitch_axis = wrap_180_cd (pitch_axis );

139
// ensure that we don't re ach gimbal lock
if (labs(pitch_axis ) > 4500) {
pitch_axis = constrain_int32 (pitch_axis , -4500, 4500);
angle_error = wrap_180_cd (pitch_axis – ahrs.pitch_sensor );
} else {
// angle error with maximum of + – max_angle_overshoot
angle_error = wrap_180_cd (pitch_axis – ahrs.pitch_sensor );
angle_error = constrain_int32 (angle_error , -MAX_PITCH_OVERSHOOT ,
MAX_PITCH_OVERSHOOT );
}

// reset target angle to current angle if motors not spinning
if (!motors.armed() || g.rc_3.servo_out == 0) {
angle_error = 0;
}

// update pitch_axis to be within max_angle_overshoot of our current heading
pitch_axis = wrap_180_cd (angle_error + ahrs.pitch_sensor );

// set earth frame targets for rate controll er
set_pitch_rate_target (g.pi_stabilize_pitch .get_p(angle_error ) + target_rate ,
EARTH_FRAME );
}

// Yaw with rate input and stabilized in the earth frame
static void
get_yaw_rate_stabilized_ef (int32_t stick_angle )
{

int32_t angle_error = 0;

// convert the input to the desired yaw rate
int32_t target_rate = stick_angle * g.acro_p;

// convert the input to the desired yaw rate
nav_yaw += target_rate * G_Dt;
nav_yaw = wrap_360_cd (nav_yaw);

// calculate difference between desired heading and current heading
angle_error = wrap_180_cd (nav_yaw – ahrs.yaw_sensor );

// limit the maximum overshoot
angle_error = constrain_int32 (angle_error , -MAX_YAW_OVERSHOOT , MAX_YAW_OVERSHOOT );

// reset target angle to current heading if motors not spinning
if (!motors.armed() || g.rc_3.servo_out == 0) {
angle_error = 0;
}

// update nav_yaw to be within max_angle_overshoot of our current heading
nav_yaw = wrap_360_cd (angle_error + ahrs.yaw_sensor );

140
// set eart h frame targets for rate controller
set_yaw_rate_target (g.pi_stabilize_yaw .get_p(angle_error )+target_rate ,
EARTH_FRAME );
}

// set_roll_rate_target – to be called by upper controllers to set roll rate targets in the
earth frame
void set_roll_rate_target ( int32_t desired_rate , uint8_t earth_or_body_frame ) {
rate_targets_frame = earth_or_body_frame ;
if( earth_or_body_frame == BODY_FRAME ) {
roll_rate_target_bf = desired_rate ;
}else{
roll_rate_target_ef = desired_rate ;
}
}

// set_pitch_rate_target – to be called by upper controllers to set pitch rate targets in
the earth frame
void set_pitch_rate_target ( int32_t desired_rate , uint8_t earth_or_body_frame ) {
rate_targets_frame = earth_or_body_frame ;
if( earth_or_body_fr ame == BODY_FRAME ) {
pitch_rate_target_bf = desired_rate ;
}else{
pitch_rate_target_ef = desired_rate ;
}
}

// set_yaw_rate_target – to be called by upper controllers to set yaw rate targets in the
earth frame
void set_yaw_rate_targ et( int32_t desired_rate , uint8_t earth_or_body_frame ) {
rate_targets_frame = earth_or_body_frame ;
if( earth_or_body_frame == BODY_FRAME ) {
yaw_rate_target_bf = desired_rate ;
}else{
yaw_rate_target_ef = desired_rate ;
}
}

// update_rate_contoller_targets – converts earth frame rates to body frame rates for rate
controllers
void
update_rate_contoller_targets ()
{
if( rate_targets_frame == EARTH_FRAME ) {
// convert earth frame rates to body frame rates
roll_rate_target_bf = roll_rate_target_ef – sin_pitch *
yaw_rate_target_ef ;
pitch_rate_target_bf = cos_roll_x * pitch_rate_target_ef + sin_roll *
cos_pitch_x * yaw_rate_target_ef ;
yaw_rate_target_bf = cos_pitch_x * cos_roll_x * yaw_rate_t arget_ef –
sin_roll * pitch_rate_target_ef ;
}

141
}

// run roll, pitch and yaw rate controllers and send output to motors
// targets for these controllers comes from stabilize controllers
void
run_rate_controllers ()
{
#if FRAME_CONFIG == HELI_FRAME // helicopters only use rate controllers for yaw
and only when not using an external gyro
if(!motors.ext_gyro_enabled ) {
g.rc_1.servo_out = get_heli_rate_roll (roll_rate_target_bf );
g.rc_2.servo_out = get_heli_rate_pitch (pitch_rate_target_bf );
g.rc_4.servo_out = get_heli_rate_yaw (yaw_rate_target_bf );
}
#else
// call rate controllers
g.rc_1.servo_out = get_rate_roll (roll_rate_target_bf );
g.rc_2.servo_out = get_rate_pitch (pitch_rate_target_bf );
g.rc_4.servo_out = get_rate _yaw(yaw_rate_target_bf );
#endif

// run throttle controller if accel based throttle controller is enabled and active
(active means it has been given a target)
if( g.throttle_accel_enabled && throttle_accel_controller_active ) {
set_throttl e_out(get_throttle_accel (throttle_accel_target_ef ), true);
}
}

#if FRAME_CONFIG == HELI_FRAME
// init_rate_controllers – set-up filters for rate controller inputs
void init_rate_controllers ()
{
// initalise low pass filters on rate controller input s
// 1st parameter is time_step, 2nd parameter is time_constant
rate_roll_filter .set_cutoff_frequency (0.01f, 2.0f);
rate_pitch_filter .set_cutoff_frequency (0.01f, 2.0f);
// rate_yaw_filter.set_cutoff_frequency(0.01f, 2.0f);
// other option fo r initialisation is
rate_roll_filter.set_cutoff_frequency(<time_step>,<cutoff_freq>);
}

static int16_t
get_heli_rate_roll (int32_t target_rate )
{
int32_t p ,i,d,ff; // used to capture pid values for logging
int32_t current_rate ; // this iteration's rate
int32_t rate_error ; // simply target_rate – current_rate
int32_t output ; // output from pid controller

// get current rate
current_rate = (omega.x * DEGX100);

142
// filter input
current_rat e = rate_roll_filter .apply(current_rate );

// call pid controller
rate_error = target_rate – current_rate ;
p = g.pid_rate_roll .get_p(rate_error );

if (motors.flybar_mode == 1) { // Mechanical Flybars get regular integral for r ate auto trim
if (target_rate > -50 && target_rate < 50){ // Frozen at high rates
i = g.pid_rate_roll .get_i(rate_error , G_Dt);
} else {
i = g.pid_rate_roll .get_integrator ();
}
} else {
i = g.pid_rate_roll .get_leaky_i (rate_error ,
G_Dt, RATE_INTEGRATOR_LEAK_RATE ); // Flybarless Helis get huge I -terms. I -term
controls much of the rate
}

d = g.pid_rate_roll .get_d(rate_error , G_Dt);

ff = g.heli_roll_ff * target_rate ;

output = p + i + d + ff;

// constrain output
output = constrain_int32 (output, -4500, 4500);

#if LOGGING_ENABLED == ENABLED
// log output if PID logging is on and we are tuning the rate P, I or D gains
if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_RATE_KP || g.radio_tuning ==
CH6_RATE _KI || g.radio_tuning == CH6_RATE_KD ) ) {
pid_log_counter ++;
if( pid_log_counter >= 10 ) { // (update rate / desired output rate)
= (100hz / 10hz) = 10
pid_log_counter = 0;
Log_Write_PID (CH6_RATE_KP , rate_error , p, i, d, output, tuning_value );
}
}
#endif

// output control
return output;
}

static int16_t
get_heli_rate_pitch (int32_t target_rate )
{
int32_t p ,i,d,ff; //
used to capture pid values for logging
int32_t current_rate ; // this iteration's rate

143
int32_t rate_error ; //
simply target_rate – current_rate
int32_t output ; //
output from pid controller

// get current rate
current_rate = (omega.y * DEGX100);

// filter input
current_rate = rate_pitch_filter .apply(current_rate );

// call pid controller
rate_error = target_rate – current_rate ;
p = g.pid_rate_pitch .get_p(rate_error ); // Helicopters get huge feed -forward

if (motors.flybar_mode == 1) { // Mechanical Flybars get regular integral for rate auto trim
if (target_rate > -50 && target_rate < 50){ // Frozen at high rates
i = g.pid_rate_pitch .get_i(rate_error , G_Dt);
} else {
i = g.pid_rate_pitch .get_integrator ();
}
} else {
i = g.pid_rate_pitch .get_leaky_i (rate_error ,
G_Dt, RATE_INTEGRATOR_LEAK_RATE ); // Flybarless Helis get huge I -terms. I -term controls
much of the rate
}

d = g.pid_rate_pitch .get_d(rate_error , G_Dt);

ff = g.heli_pitch_ff *target_rate ;

output = p + i + d + ff;

// constrain output
output = constrain_int32 (output, -4500, 4500);

#if LOGGING_ENABLED == ENABLED
// log output if PID logging is on and we are tuning the rate P, I or D gains
if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_RATE_KP || g.radio_tuning ==
CH6_RATE_KI || g.radio_tuning == CH6_RATE_KD ) ) {
if( pid_log_counter == 0 ) { // relies on get_heli_rate_roll to
update pid_log_counter
Log_Write_PID (CH6_RATE_KP +100, rate_error , p, i, 0, output, tuning_value);
}
}
#endif

// output control
return output;
}

static int16_t

144
get_heli_rate_yaw (int32_t target_rate )
{
int32_t p ,i,d,ff; //
used to capture pid values for logging
int32_t current_rate ;
// this iteration's rate
int32_t rate_error ;
int32_t output ;

// get current rate
current_rate = (omega.z * DEGX100);

// filter input
// current _rate = rate_yaw_filter.apply(current_rate);

// rate control
rate_error = target_rate – current_rate ;

// separately calculate p, i, d values for logging
p = g.pid_rate_yaw .get_p(rate_error );

i = g.pid_rate_yaw .get_i(rate_error , G_Dt);

d = g.pid_rate_yaw .get_d(rate_error , G_Dt);

ff = g.heli_yaw_ff *target_rate ;

output = p + i + d + ff;
output = constrain_float (output, -4500, 4500);

#if LOGGING_ENABLED == ENABLED
// log output if PID loggins is on a nd we are tuning the yaw
if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_YAW_RATE_KP ||
g.radio_tuning == CH6_YAW_RATE_KD ) ) {
pid_log_counter ++;
if( pid_log_counter >= 10 ) { // (update rate / desired output ra te)
= (100hz / 10hz) = 10
pid_log_counter = 0;
Log_Write_PID (CH6_YAW_RATE_KP , rate_error , p, i, d, output, tuning_value );
}
}

#endif

// output control
return output;
}
#endif // HELI_FRAME

#if FRAME_CONFIG != HELI_FRAME
static int16_t
get_rate_roll (int32_t target_rate )

145
{
int32_t p ,i,d; // used to capture pid values for logging
int32_t current_rate ; // this iteration's rate
int32_t rate_error ; // simply target_rate – current_rate
int32_t output ; // output from pid controller

// get current rate
current_rate = (omega.x * DEGX100);

// call pid controller
rate_error = target_rate – current_rate ;
p = g.pid_rate_roll .get_p(rate_error );

// get i term
i = g.pid_rate_roll .get_integrator ();

// update i term as long as we haven't breached the limits or the I term will certainly
reduce
if (!motors.reached_limit (AP_MOTOR_ROLLPITCH_LIMIT ) ||
((i>0&&rate_error <0)||(i<0&&rate_error >0))) {
i = g.pid_rate_roll .get_i(rate_error , G_Dt);
}

d = g.pid_rate_roll .get_d(rate_error , G_Dt);
output = p + i + d;

// constrain output
output = constrain_int32 (output, -5000, 5000);

#if LOGGING_ENABLE D == ENABLED
// log output if PID logging is on and we are tuning the rate P, I or D gains
if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_RATE_KP || g.radio_tuning ==
CH6_RATE_KI || g.radio_tuning == CH6_RATE_KD ) ) {
pid_log_cou nter++; // Note: get_rate_pitch pid logging
relies on this function to update pid_log_counter so if you change the line above you must
change the equivalent line in get_rate_pitch
if( pid_log_counter >= 10 ) { // (update rate / desired output rate)
= (100hz / 10hz) = 10
pid_log_counter = 0;
Log_Write_PID (CH6_RATE_KP , rate_error , p, i, d, output, tuning_value );
}
}
#endif

// output control
return output;
}

static int16_t
get_rate_pitch (int32_t target_rate )
{

146
int32_t p ,i,d; //
used to capture pid values for logging
int32_t current_rate ; // this
iteration's rate
int32_t rate_error ; //
simply target_rate – current_rate
int32_t output ; //
output from pid co ntroller

// get current rate
current_rate = (omega.y * DEGX100);

// call pid controller
rate_error = target_rate – current_rate ;
p = g.pid_rate_pitch .get_p(rate_error );

// get i term
i = g.pid_rate_pitch .get_integrator ();

// update i term as long as we haven't breached the limits or the I term will certainly
reduce
if (!motors.reached_limit (AP_MOTOR_ROLLPITCH_LIMIT ) ||
((i>0&&rate_error <0)||(i<0&&rate_error >0))) {
i = g.pid_rate_pitch .get_i(rate_error , G_Dt);
}

d = g.pid_rate_pitch .get_d(rate_error , G_Dt);
output = p + i + d;

// constrain output
output = constrain_int32 (output, -5000, 5000);

#if LOGGING_ENABLED == ENABLED
// log output if PID logging is on and we are tuning the rate P, I or D gains
if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_RATE_KP || g.radio_tuning ==
CH6_RATE_KI || g.radio_tuning == CH6_RATE_KD ) ) {
if( pid_log_counter == 0 ) { // relies on get_rate_roll having
updated pid_log_counter
Log_Write_PID (CH6_RATE_KP +100, rate_error , p, i, d, output, tuning_value );
}
}
#endif

// output control
return output;
}

static int16_t
get_rate_yaw (int32_t target_rate )
{
int32_t p ,i,d; //
used to capture pid values for logging

147
int32_t rate_error ;
int32_t output ;

// rate control
rate_error = target_rate – (omega.z * DEGX100);

// separa tely calculate p, i, d values for logging
p = g.pid_rate_yaw .get_p(rate_error );

// get i term
i = g.pid_rate_yaw .get_integrator ();

// update i term as long as we haven't breached the limits or the I term will certainly
reduce
if (!motors.reached_limit (AP_MOTOR_YAW_LIMIT ) ||
((i>0&&rate_error <0)||(i<0&&rate_error >0))) {
i = g.pid_rate_yaw .get_i(rate_error , G_Dt);
}

// get d value
d = g.pid_rate_yaw .get_d(rate_error , G_Dt);

output = p+i+d;
output = constrai n_int32(output, -4500, 4500);

#if LOGGING_ENABLED == ENABLED
// log output if PID loggins is on and we are tuning the yaw
if( g.log_bitmask & MASK_LOG_PID && g.radio_tuning == CH6_YAW_RATE_KP ) {
pid_log_counter ++;
if( pid_log_coun ter >= 10 ) { // (update rate / desired output rate)
= (100hz / 10hz) = 10
pid_log_counter = 0;
Log_Write_PID (CH6_YAW_RATE_KP , rate_error , p, i, d, output, tuning_value );
}
}
#endif

#if FRAME_CONFIG == TRI_FRAME
// constrain output
return output;
#else // !TRI_FRAME
// output control:
int16_t yaw_limit = 2200 + abs(g.rc_4.control_in );

// smoother Yaw control:
return constrain_int32 (output, -yaw_limit , yaw_limit );
#endif // TRI_FRAME
}
#endif // !HELI_FRAME

// calculate modified roll/pitch depending upon optical flow calculated position
static int32_t

148
get_of_roll (int32_t input_roll )
{
#if OPTFLOW == ENABLED
static float tot_x_cm = 0; // total distance from target
static uint32_t last_of_roll_update = 0;
int32_t new_roll = 0;
int32_t p ,i,d;

// check if new optflow data available
if( optflow.last_update != last_of_roll_update ) {
last_of_roll_update = optflow.last_update ;

// add new distance moved
tot_x_cm += optflow.x_cm;

// only stop roll if caller isn't modifying roll
if( input_roll == 0 && current_loc .alt < 1500) {
p = g.pid_optflow_roll .get_p(-tot_x_cm );
i = g.pid_optflow_roll .get_i(-tot_x_cm,1.0f); // we could use the
last update time to calculate the time change
d = g.pid_optflow_roll .get_d(-tot_x_cm ,1.0f);
new_roll = p+i+d;
}else{
g.pid_optflow_roll .reset_I();
tot_x_cm = 0;
p = 0; // for logging
i = 0;
d = 0;
}
// limit amount of change and maximum angle
of_roll = constrain_int32 (new_roll , (of_roll-20), (of_roll+20));

#if LOGGING_ENABLED == ENABLED
// log output if PID logging is on and we are tuning the rate P, I or D gains
if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_OPTFLOW_KP ||
g.radio_tuning == CH6_OPTFLOW_KI || g.radio_tuning == CH6_OPTFLOW_KD ) ) {
pid_log_counter ++; // Note: get_of_pitch pid logging relies on
this function updating pid_log_counter so if you change the line above you must change the
equivalent line in get_of_pitch
if( pid_log_counter >= 5 ) { // (update rat e / desired output rate) = (100hz
/ 10hz) = 10
pid_log_counter = 0;
Log_Write_PID (CH6_OPTFLOW_KP , tot_x_cm , p, i, d, of_roll, tuning_value );
}
}
#endif // LOGGING_ENABLED == ENABLED
}

// limit m ax angle
of_roll = constrain_int32 (of_roll, -1000, 1000);

return input_roll +of_roll;

149
#else
return input_roll ;
#endif
}

static int32_t
get_of_pitch (int32_t input_pitch )
{
#if OPTFLOW == ENABLED
static float tot_y_cm = 0; // total distance from target
static uint32_t last_of_pitch_update = 0;
int32_t new_pitch = 0;
int32_t p ,i,d;

// check if new optflow data available
if( optflow.last_update != last_of_pitch_update ) {
last_of_pitch_update = optflow.last_update ;

// add new distance moved
tot_y_cm += optflow.y_cm;

// only stop roll if caller isn't modifying pitch
if( input_pitch == 0 && current_loc .alt < 1500 ) {
p = g.pid_optflow_pitch .get_p(tot_y_cm );
i = g.pid_optflow_pitch .get_i(tot_y_cm , 1.0f); // we could use the
last update time to calculate the time change
d = g.pid_optflow_pitch .get_d(tot_y_cm , 1.0f);
new_pitch = p + i + d;
}else{
tot_y_cm = 0;
g.pid_optflow_pitch .reset_I();
p = 0; // for logging
i = 0;
d = 0;
}

// limit amount of change
of_pitch = constrain_int32 (new_pitch , (of_pitch -20), (of_pitch +20));

#if LOGGING_ENABLED == ENABLED
// log output if PID logging is on and we are tuning the rate P, I or D gains
if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_OPTFLOW_KP ||
g.radio_tuning == CH6_OPTFLOW_KI || g.radio_tuning == CH6_OPTFLOW_KD ) ) {
if( pid_log_counter == 0 ) { // relies on get_of_roll having updated the
pid_log_counter
Log_Write_PID (CH6_OPTFLOW_KP +100, tot_y_cm , p, i, d, of_pitch ,
tuning_value );
}
}
#endif // LOGGI NG_ENABLED == ENABLED
}

150
// limit max angle
of_pitch = constrain_int32 (of_pitch , -1000, 1000);

return input_pitch +of_pitch ;
#else
return input_pitch ;
#endif
}

/*************************************************************
* yaw contr ollers
*************************************************************/

// get_look_at_yaw – updates bearing to look at center of circle or do a panorama
// should be called at 100hz
static void get_circle_yaw ()
{
static uint8_t look_at_yaw_counter = 0; // used to reduce update rate to 10hz

// if circle radius is zero do panorama
if( g.circle_radius == 0 ) {
// slew yaw towards circle angle
nav_yaw = get_yaw_slew (nav_yaw, ToDeg(circle_angle )*100, AUTO_YAW_SLEW_RATE );
}else{
look_at_yaw_counter ++;
if( look_at_yaw_counter >= 10 ) {
look_at_yaw_counter = 0;
yaw_look_at_WP_bearing = pv_get_bearing_cd (inertial_nav .get_position (),
yaw_look_at_WP );
}
// slew yaw
nav_yaw = get_yaw_slew (nav_yaw, yaw_look_at_WP_bearing , AUTO_YAW_SLEW_RATE );
}

// call stabilize yaw controller
get_stabilize_yaw (nav_yaw);
}

// get_look_at_yaw – updates bearing to location held in look_at_yaw_WP and calls stabilize
yaw contro ller
// should be called at 100hz
static void get_look_at_yaw ()
{
static uint8_t look_at_yaw_counter = 0; // used to reduce update rate to 10hz

look_at_yaw_counter ++;
if( look_at_yaw_counter >= 10 ) {
look_at_yaw_counter = 0;
yaw_look_at_WP_bearing = pv_get_bearing_cd (inertial_nav .get_position (),
yaw_look_at_WP );
}

151
// slew yaw and call stabilize controller
nav_yaw = get_yaw_slew (nav_yaw, yaw_look_at_WP_bearing , AUTO_YAW_SLEW_RATE );
get_stabilize_yaw (nav_yaw);
}

static void get_look_ahead_yaw (int16_t pilot_yaw )
{
// Commanded Yaw to automatically look ahead.
if (g_gps->fix && g_gps->ground_course > YAW_LOOK_AHEAD_MIN_SPEED ) {
nav_yaw = get_yaw_slew (nav_yaw, g_gps->ground_course , AUTO_YAW_SLEW_ RATE);
get_stabilize_yaw (wrap_360_cd (nav_yaw + pilot_yaw )); // Allow pilot to "skid"
around corners up to 45 degrees
}else{
nav_yaw += pilot_yaw * g.acro_p * G_Dt;
nav_yaw = wrap_360_cd (nav_yaw);
get_stabilize_yaw (nav_yaw);
}
}

/*************************************************************
* throttle control
****************************************************************/

// update_throttle_cruise – update throttle cruise if necessary
static void update_thrott le_cruise (int16_t throttle )
{
// ensure throttle_avg has been initialised
if( throttle_avg == 0 ) {
throttle_avg = g.throttle_cruise ;
}
// calc average throttle if we are in a level hover
if (throttle > g.throttle_min && abs(climb_rate) < 60 && labs(ahrs.roll_sensor ) < 500
&& labs(ahrs.pitch_sensor ) < 500) {
throttle_avg = throttle_avg * 0.99f + (float)throttle * 0.01f;
g.throttle_cruise = throttle_avg ;
}
}

#if FRAME_CONFIG == HELI_FRAME
// get_angle_boost – returns a throttle including compensation for roll/pitch angle
// throttle value should be 0 ~ 1000
// for traditional helicopters
static int16_t get_angle_boost (int16_t throttle )
{
float angle_boost_factor = cos_pitch_x * cos_roll_x ;
angle_boost_f actor = 1.0f – constrain_float (angle_boost_factor , .5f, 1.0f);
int16_t throttle_above_mid = max(throttle – motors.throttle_mid ,0);

// to allow logging of angle boost
angle_boost = throttle_above_mid *angle_boost_factor ;

return throttle + angle_boost ;

152
}
#else // all multicopters
// get_angle_boost – returns a throttle including compensation for roll/pitch angle
// throttle value should be 0 ~ 1000
static int16_t get_angle_boost (int16_t throttle )
{
float temp = cos_pitch_x * cos_roll_x ;
int16_t throttle_out ;

temp = constrain_float (temp, 0.5f, 1.0f);

// reduce throttle if we go inverted
temp = constrain_float (9000-max(labs(ahrs.roll_sensor ),labs(ahrs.pitch_sensor )), 0,
3000) / (3000 * temp);

// apply scale and const rain throttle
throttle_out = constrain_float ((float)(throttle -g.throttle_min ) * temp +
g.throttle_min , g.throttle_min , 1000);

// to allow logging of angle boost
angle_boost = throttle_out – throttle ;

return throttle_out ;
}
#endif // FRAME _CONFIG == HELI_FRAME

// set_throttle_out – to be called by upper throttle controllers when they wish to provide
throttle output directly to motors
// provide 0 to cut motors
void set_throttle_out ( int16_t throttle_out , bool apply_angle_boost )
{
if( apply_angle_boost ) {
g.rc_3.servo_out = get_angle_boost (throttle_out );
}else{
g.rc_3.servo_out = throttle_out ;
// clear angle_boost for logging purposes
angle_boost = 0;
}

// update compass with throttle valu e
compass.set_throttle ((float)g.rc_3.servo_out /1000.0f);
}

// set_throttle_accel_target – to be called by upper throttle controllers to set desired
vertical acceleration in earth frame
void set_throttle_accel_target ( int16_t desired_acceleration )
{
if( g.throttle_accel_enabled ) {
throttle_accel_target_ef = desired_acceleration ;
throttle_accel_controller_active = true;
}else{
// To-Do log dataflash or tlog error

153
cliSerial ->print_P(PSTR("Err: target sent to inact ive acc thr controller! \n"));
}
}

// disable_throttle_accel – disables the accel based throttle controller
// it will be re -enasbled on the next set_throttle_accel_target
// required when we wish to set motors to zero when pilot inputs zero throttle
void throttle_accel_deactivate ()
{
throttle_accel_controller_active = false;
}

// get_throttle_accel – accelerometer based throttle controller
// returns an actual throttle output (0 ~ 1000) to be sent to the motors
static int16_t
get_throttle_accel (int16_t z_target_accel )
{
static float z_accel_error = 0; // The acceleration error in cm.
static uint32_t last_call_ms = 0; // the last time this controller was called
int32_t p ,i,d; // used to capture pid values for l ogging
int16_t output ;
float z_accel_meas ;
uint32_t now = millis();

// Calculate Earth Frame Z acceleration
z_accel_meas = -(ahrs.get_accel_ef ().z + GRAVITY_MSS ) * 100;

// reset target altitude if this controller has just been eng aged
if( now – last_call_ms > 100 ) {
// Reset Filter
z_accel_error = 0;
} else {
// calculate accel error and Filter with fc = 2 Hz
z_accel_error = z_accel_error + 0.11164f * (constrain_float (z_target_accel –
z_accel_meas, -32000, 32000) – z_accel_error );
}
last_call_ms = now;

// separately calculate p, i, d values for logging
p = g.pid_throttle_accel .get_p(z_accel_error );
// freeze I term if we've breached throttle limits
if( motors.reached_ limit(AP_MOTOR_THROTTLE_LIMIT ) ) {
i = g.pid_throttle_accel .get_integrator ();
}else{
i = g.pid_throttle_accel .get_i(z_accel_error , .01f);
}
d = g.pid_throttle_accel .get_d(z_accel_error , .01f);

//
// limit the rate
output = constrain_float (p+i+d+g.throttle_cruise , g.throttle_min , g.throttle_max );

154
#if LOGGING_ENABLED == ENABLED
// log output if PID loggins is on and we are tuning the yaw
if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_THR_ACCEL_KP ||
g.radio_tuning == CH6_THR_ACCEL_KI || g.radio_tuning == CH6_THR_ACCEL_KD ) ) {
pid_log_counter ++;
if( pid_log_counter >= 10 ) { // (update rate / desired output rate)
= (50hz / 10hz) = 5hz
pid_log_counter = 0;
Log_Write_PID (CH6_THR_ACCEL_KP , z_accel_error , p, i, d, output, tuning_value );
}
}
#endif

return output;
}

// get_pilot_desired_throttle – transform pilot's throttle input to make cruise throttle
mid stick
// used only for manua l throttle modes
// returns throttle output 0 to 1000
#define THROTTLE_IN_MIDDLE 500 // the throttle mid point
static int16_t get_pilot_desired_throttle (int16_t throttle_control )
{
int16_t throttle_out ;

// exit immediately in the simple c ases
if( throttle_control == 0 || g.throttle_mid == 500) {
return throttle_control ;
}

// ensure reasonable throttle values
throttle_control = constrain_int16 (throttle_control ,0,1000);
g.throttle_mid = constrain_int16 (g.throttle _mid,300,700);

// check throttle is above, below or in the deadband
if (throttle_control < THROTTLE_IN_MIDDLE ) {
// below the deadband
throttle_out = g.throttle_min + ((float)(throttle_control –
g.throttle_min ))*((float)(g.throttle_m id – g.throttle_min ))/((float)(500-g.throttle_min ));
}else if(throttle_control > THROTTLE_IN_MIDDLE ) {
// above the deadband
throttle_out = g.throttle_mid + ((float)(throttle_control -500))*(float)(1000-
g.throttle_mid )/500.0f;
}else{
// must be in the deadband
throttle_out = g.throttle_mid ;
}

return throttle_out ;
}

// get_pilot_desired_climb_rate – transform pilot's throttle input to

155
// climb rate in cm/s. we use radio_in instead of control_in to get the ful l range
// without any deadzone at the bottom
#define THROTTLE_IN_DEADBAND 100 // the throttle input channel's deadband in PWM
#define THROTTLE_IN_DEADBAND_TOP (THROTTLE_IN_MIDDLE +THROTTLE_IN_DEADBAND ) // top of the
deadband
#define THROTTLE_IN_DEA DBAND_BOTTOM (THROTTLE_IN_MIDDLE -THROTTLE_IN_DEADBAND ) // bottom of
the deadband
static int16_t get_pilot_desired_climb_rate (int16_t throttle_control )
{
int16_t desired_rate = 0;

// throttle failsafe check
if( ap.failsafe_radio ) {
return 0;
}

// ensure a reasonable throttle value
throttle_control = constrain_int16 (throttle_control ,0,1000);

// check throttle is above, below or in the deadband
if (throttle_control < THROTTLE_IN_DEADBAND_BOTTOM ) {
// below the deadband
desired_rate = (int32_t)g.pilot_velocity_z_max * (throttle_control –
THROTTLE_IN_DEADBAND_BOTTOM ) / (THROTTLE_IN_MIDDLE – THROTTLE_IN_DEADBAND );
}else if (throttle_control > THROTTLE_IN_DEADBAND_TOP ) {
// above the deadband
desired_rate = (int32_t)g.pilot_velocity_z_max * (throttle_control –
THROTTLE_IN_DEADBAND_TOP ) / (THROTTLE_IN_MIDDLE – THROTTLE_IN_DEADBAND );
}else{
// must be in the deadband
desired_rate = 0;
}

// desired climb rate for logging
desired_climb_rate = desired_rate ;

return desired_rate ;
}

// get_initial_alt_hold – get new target altitude based on current altitude and climb rate
static int32_t
get_initial_alt_hold ( int32_t alt_cm , int16_t climb_rate_cms )
{
int32_t target_alt ;
int32_t linear_distance ; // half the distace we swap between linear and sqrt and
the distace we offset sqrt.
int32_t linear_velocity ; // the velocity we swap between linear and sqrt.

linear_velocity = ALT_HOLD_ACCEL_MA X/g.pi_alt_hold .kP();

if (abs(climb_rate_cms ) < linear_velocity ) {
target_alt = alt_cm + climb_rate_cms /g.pi_alt_hold .kP();

156
} else {
linear_distance = ALT_HOLD_ACCEL_MAX /(2*g.pi_alt_hold .kP()*g.pi_alt_hold .kP());
if (climb_rate_cms > 0){
target_alt = alt_cm + linear_distance +
(int32_t)climb_rate_cms *(int32_t)climb_rate_cms /(2*ALT_HOLD_ACCEL_MAX );
} else {
target_alt = alt_cm – ( linear_distance +
(int32_t)climb_rate_cms *(int32_t)climb_rate_cm s/(2*ALT_HOLD_ACCEL_MAX ) );
}
}
return constrain_int32 (target_alt , alt_cm – ALT_HOLD_INIT_MAX_OVERSHOOT , alt_cm +
ALT_HOLD_INIT_MAX_OVERSHOOT );
}

// get_throttle_rate – calculates desired accel required to achieve desired z_target_speed
// sets accel based throttle controller target
static void
get_throttle_rate (float z_target_speed )
{
static uint32_t last_call_ms = 0;
static float z_rate_error = 0; // The velocity error in cm.
static float z_target_speed_filt = 0; // The fi ltered requested speed
float z_target_speed_delta ; // The change in requested speed
int32_t p ,i,d; // used to capture pid values for logging
int32_t output ; // the target acceleration if the accel based throttle is enabled,
otherwise the output to be sent to the motors
uint32_t now = millis();

// reset target altitude if this controller has just been engaged
if( now – last_call_ms > 100 ) {
// Reset Filter
z_rate_error = 0;
z_target_speed_filt = z_target_speed ;
output = 0;
} else {
// calculate rate error and filter with cut off frequency of 2 Hz
z_rate_error = z_rate_error + 0.20085f * ((z_target_speed – climb_rate ) –
z_rate_error );
// feed forward acceleratio n based on change in the filtered desired speed.
z_target_speed_delta = 0.20085f * (z_target_speed – z_target_speed_filt );
z_target_speed_filt = z_target_speed_filt + z_target_speed_delta ;
output = z_target_speed_delta * 50.0f; // To-Do: replace 50 with dt
}
last_call_ms = now;

// separately calculate p, i, d values for logging
p = g.pid_throttle .get_p(z_rate_error );

// freeze I term if we've breached throttle limits
if(motors.reached_limit (AP_MOTOR_THR OTTLE_LIMIT )) {
i = g.pid_throttle .get_integrator ();
}else{

157
i = g.pid_throttle .get_i(z_rate_error , .02);
}
d = g.pid_throttle .get_d(z_rate_error , .02);

// consolidate and constrain target acceleration
output += p+i+d;
output = constrain_int32 (output, -32000, 32000);

#if LOGGING_ENABLED == ENABLED
// log output if PID loggins is on and we are tuning the yaw
if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_THROTTLE_KP ||
g.radio_tuning == CH6_THROTTL E_KI || g.radio_tuning == CH6_THROTTLE_KD ) ) {
pid_log_counter ++;
if( pid_log_counter >= 10 ) { // (update rate / desired output rate)
= (50hz / 10hz) = 5hz
pid_log_counter = 0;
Log_Write_PID (CH6_THROTT LE_KP, z_rate_error , p, i, d, output, tuning_value );
}
}
#endif

// send output to accelerometer based throttle controller if enabled otherwise send
directly to motors
if( g.throttle_accel_enabled ) {
// set target for accel bas ed throttle controller
set_throttle_accel_target (output);
}else{
set_throttle_out (g.throttle_cruise +output, true);
}

// update throttle cruise
// TO-DO: this may not be correct because g.rc_3.servo_out has not been updated for
this iteration
if( z_target_speed == 0 ) {
update_throttle_cruise (g.rc_3.servo_out );
}
}

// get_throttle_althold – hold at the desired altitude in cm
// updates accel based throttle controller targets
// Note: max_climb_rate is an opti onal parameter to allow reuse of this function by landing
controller
static void
get_throttle_althold (int32_t target_alt , int16_t min_climb_rate , int16_t max_climb_rate )
{
int32_t alt_error ;
float desired_rate ;
int32_t linear_distance ; // half the distace we swap between linear and sqrt and
the distace we offset sqrt.

// calculate altitude error
alt_error = target_alt – current_loc .alt;

158

// check kP to avoid division by zero
if( g.pi_alt_hold .kP() != 0 ) {
linear_distance = ALT_HOLD_ACCEL_MAX /(2*g.pi_alt_hold .kP()*g.pi_alt_hold .kP());
if( alt_error > 2*linear_distance ) {
desired_rate = safe_sqrt (2*ALT_HOLD_ACCEL_MAX *(alt_error -linear_distance ));
}else if( alt_error < -2*linear_distance ) {
desired_rate = -safe_sqrt (2*ALT_HOLD_ACCEL_MAX *(-alt_error -linear_distance ));
}else{
desired_rate = g.pi_alt_hold .get_p(alt_error );
}
}else{
desired_rate = 0;
}

desired_rate = constrain_floa t(desired_rate , min_climb_rate , max_climb_rate );

// call rate based throttle controller which will update accel based throttle
controller targets
get_throttle_rate (desired_rate );

// update altitude error reported to GCS
altitude_error = alt_error ;

// TO-DO: enabled PID logging for this controller
}

// get_throttle_althold_with_slew – altitude controller with slew to avoid step changes in
altitude target
// calls normal althold controller which updates accel based throttle controller targets
static void
get_throttle_althold_with_slew (int32_t target_alt , int16_t min_climb_rate , int16_t
max_climb_rate )
{
// limit target altitude change
controller_desired_alt += constrain_float (target_alt -controller_desired_alt ,
min_climb_rate *0.02f, max_climb_rate *0.02f);

// do not let target altitude get too far from current altitude
controller_desired_alt = constrain_float (controller_desired_alt ,current_loc .alt-
750,current_loc .alt+750);

get_throttle_althold (controller_desired_alt , min_climb_rate -250, max_climb_rate +250);
// 250 is added to give head room to alt hold controller
}

// get_throttle_rate_stabilized – rate controller with additional 'stabilizer'
// 'stabilizer' ensure desired rate is being met
// calls normal throttle rate controller which updates accel based throttle controller
targets
static void
get_throttle_rate_stabilized (int16_t target_rate )

159
{
controller_desired_alt += target_rate * 0.02f;

// do not let target altitude get too far from current altitude
controller_desired_alt = constrain_float (controller_desired_alt ,current_loc .alt-
750,current_loc .alt+750);

// update target altitude for reporting purposes
set_target_alt_for_reporting (controller_desired_alt );

get_throttle_althold (controller _desired_alt , -g.pilot_velocity_z_max -250,
g.pilot_velocity_z_max +250); // 250 is added to give head room to alt hold controller
}

// get_throttle_land – high level landing logic
// sends the desired acceleration in the accel based throttle controller
// called at 50hz
static void
get_throttle_land ()
{
// if we are above 10m and the sonar does not sense anything perform regular alt hold
descent
if (current_loc .alt >= LAND_START_ALT && !(g.sonar_enabled && sonar_alt_health >=
SONAR_ALT_HEALTH_MAX )) {
get_throttle_althold_with_slew (LAND_START_ALT , -wp_nav.get_descent_velocity (), –
abs(g.land_speed ));
}else{
get_throttle_rate_stabilized (-abs(g.land_speed ));

// detect whether we have landed by watching for minimum throttle and now movement
if (abs(climb_rate ) < 20 && (g.rc_3.servo_out <= get_angle_boost (g.throttle_min ) ||
g.pid_throttle_accel .get_integrator () <= -150)) {
if( land_detector < LAND_DETECTOR_TRIGGER ) {
land_detector ++;
}else{
set_land_complete (true);
if( g.rc_3.control_in == 0 || ap.failsafe_radio ) {
init_disarm_motors ();
}
}
}else{
// we've sensed movement up or do wn so decrease land_detector
if (land_detector > 0 ) {
land_detector –;
}
}
}
}

// get_throttle_surface_tracking – hold copter at the desired distance above the ground
// updates accel based throttle con troller targets
static void

160
get_throttle_surface_tracking (int16_t target_rate )
{
static float target_sonar_alt = 0; // The desired altitude in cm above the ground
static uint32_t last_call_ms = 0;
float distance_error ;
float sonar_induced _slew_rate ;

uint32_t now = millis();

// reset target altitude if this controller has just been engaged
if( now – last_call_ms > 200 ) {
target_sonar_alt = sonar_alt + controller_desired_alt – current_loc .alt;
}
last_call_ms = now;

target_sonar_alt += target_rate * 0.02f;

distance_error = (target_sonar_alt -sonar_alt );
sonar_induced_slew_rate = constrain_float (fabsf(g.sonar_gain *
distance_error ),0,THR_SURFACE_TRACKING_VELZ_MAX );

// do not let target altitude g et too far from current altitude above ground
// Note: the 750cm limit is perhaps too wide but is consistent with the regular althold
limits and helps ensure a smooth transition
target_sonar_alt = constrain_float (target_sonar_alt ,sonar_alt -750,sonar_alt+750);
controller_desired_alt = current_loc .alt+(target_sonar_alt -sonar_alt );

// update target altitude for reporting purposes
set_target_alt_for_reporting (controller_desired_alt );

get_throttle_althold_with_slew (controller_desired_al t, target_rate –
sonar_induced_slew_rate , target_rate +sonar_induced_slew_rate ); // VELZ_MAX limits how
quickly we react
}

/*
* reset all I integrators
*/
static void reset_I_all (void)
{
reset_rate_I ();
reset_stability_I ();
reset_throttle_I ();
reset_optflow_I ();

// This is the only place we reset Yaw
g.pi_stabilize_yaw .reset_I();
}

static void reset_rate_I ()
{
g.pid_rate_roll .reset_I();

161
g.pid_rate_pitch .reset_I();
g.pid_rate_yaw .reset_I();
}

static void reset_optflo w_I(void)
{
g.pid_optflow_roll .reset_I();
g.pid_optflow_pitch .reset_I();
of_roll = 0;
of_pitch = 0;
}

static void reset_throttle_I (void)
{
// For Altitude Hold
g.pi_alt_hold .reset_I();
g.pid_throttle .reset_I();
g.pid_thrott le_accel .reset_I();
}

static void set_accel_throttle_I_from_pilot_throttle (int16_t pilot_throttle )
{
// shift difference between pilot's throttle and hover throttle into accelerometer I
g.pid_throttle_accel .set_integrator (pilot_throttle -g.throttle _cruise);
}

static void reset_stability_I (void)
{
// Used to balance a quad
// This only needs to be reset during Auto -leveling in flight
g.pi_stabilize_roll .reset_I();
g.pi_stabilize_pitch .reset_I();
}

162
GCS.h
// -*- tab-width: 4; Mode: C+ +; c-basic-offset: 4; indent -tabs-mode: nil -*-

/// @file GCS.h
/// @brief Interface definition for the various Ground Control System
// protocols.

#ifndef __GCS_H
#define __GCS_H

#include <AP_HAL.h>
#include <GPS.h>

///
/// @class GCS
/// @brief Class describing the interface between the APM code
/// proper and the GCS implementation.
///
/// GCS' are currently implemented inside the sketch and as such have
/// access to all global state. The sketch should not, however, call GCS
/// internal functio ns – all calls to the GCS should be routed through
/// this interface (or functions explicitly exposed by a subclass).
///
class GCS_Class
{
public:

/// Startup initialisation.
///
/// This routine performs any one -off initialisation required before
/// GCS messages are exchanged.
///
/// @note The stream is expected to be set up and configured for the
/// correct bitrate before ::init is called.
///
/// @note The stream is currently BetterStream so that we can use the _P
/// methods; this may change if Arduino adds them to Print.
///
/// @param port The stream over which messages are exchanged.
///
void init(AP_HAL::UARTDriver *port) {
_port = port;
initialised = true;
}

/// Update GCS state.
///
/// This may involve checking for received bytes on the stream,
/// or sending additional periodic messages.
void update(void) {
}

163

/// Send a message with a single numeric parameter.
///
/// This may be a standalone message, or the GCS driver may
/// have its own way of locating additional parameters to send.
///
/// @param id ID of the message to send.
/// @param param Explicit message parameter.
///
void send_message (enum ap_message id) {
}

/// Send a text message with a PSTR()
///
/// @param severity A value describing the importance of the message.
/// @param str The text to be sent.
///
void send_text_P (gcs_severity severity , const prog_char_t *str) {
}

// send streams which match frequency range
void data_stream_send (void);

// set to true if this GCS link is active
bool initialised ;

protected :
/// The str eam we are communicating over
AP_HAL::UARTDriver * _port;
};

//
// GCS class definitions.
//
// These are here so that we can declare the GCS object early in the sketch
// and then reference it statically rather than via a pointer.
//

///
/// @class GCS_MAVLINK
/// @brief The mavlink protocol for qgroundcontrol
///
class GCS_MAVLINK : public GCS_Class
{
public:
GCS_MAVLINK ();
void update(void);
void init(AP_HAL::UARTDriver *port);
void send_message (enum ap_message id);
void send_text (gcs_severity severity , const char *str);
void send_text_P (gcs_severity severity , const prog_char_t *str);
void data_stream_send (void);

164
void queued_param_send ();
void queued_way point_send ();

static const struct AP_Param ::GroupInfo var_info [];

// NOTE! The streams enum below and the
// set of AP_Int16 stream rates _must_ be
// kept in the same order
enum streams {STREAM_RAW_SENSORS ,
STREAM_EXTENDED_STATUS ,
STREAM_RC_CHANNELS ,
STREAM_RAW_CONTROLLER ,
STREAM_POSITION ,
STREAM_EXTRA1 ,
STREAM_EXTRA2 ,
STREAM_EXTRA3 ,
STREAM_PARAMS ,
NUM_STREAMS };

// see if we should send a stream now. Called at 50Hz
bool stream_trigger (enum streams stream_num );

// call to reset the timeout window for entering the cli
void reset_cli_timeout ();
private:
void handleMessage (mavlink_message_t * msg);

/// Perform queued sending operations
///
AP_Param * _queued_parameter ; ///< next parameter to
// be se nt in queue
enum ap_var_type _queued_parameter_type ; ///< type of the next
// parameter
AP_Param ::ParamToken _queued_parameter_token ; ///AP_Param token for
// next() call
uint16_t _queued_parameter_index ; ///< next queued
// parameter's index
uint16_t _queued_parameter_ count; ///< saved count of
// parameters for
// queued send

/// Count the number of reportable parameters.
///
/// Not all parame ters can be reported via MAVlink. We count the number
// that are
/// so that we can report to a GCS the number of parameters it should
// expect when it
/// requests the full set.
///
/// @return The number of reportable p arameters.
///
uint16_t _count_parameters (); ///< count reportable

165
// parameters

uint16_t _parameter_count ; ///< cache of reportable
// parameters

mavlink_channel_t chan;
uint16_t packet_drops ;

#if CLI_ENABLED == ENABLED
// this allows us to detect the user wanting the CLI to start
uint8_t crlf_coun t;
#endif

// waypoints
uint16_t waypoint_request_i ; // request index
uint16_t waypoint_request_last ; // last request index
uint16_t waypoint_dest_sysid ; // where to send requests
uint16_t waypoint_dest_compi d; // "
bool waypoint_sending ; // currently in send process
bool waypoint_receiving ; // currently receiving
uint16_t waypoint_count ;
uint32_t waypoint_timelast_send ; // milliseconds
uint32_t waypoint_timelast_receive ; // milliseconds
uint32_t waypoint_timelast_request ; // milliseconds
uint16_t waypoint_send_timeout ; // milliseconds
uint16_t waypoint_receive_timeout ; // milliseconds

// data stream rates
AP_Int16 streamRateRawSensors ;
AP_Int16 streamRateExtendedStatus ;
AP_Int16 streamRateRCChannels ;
AP_Int16 streamRateRawController ;
AP_Int16 streamRatePosition ;
AP_Int16 streamRateExtra1 ;
AP_Int16 streamRateExtra2 ;
AP_Int16 streamRateExtra3 ;
AP_Int16 streamRateParams ;

// number of 50Hz ticks until we next send this stream
uint8_t stream_ticks [NUM_STREAMS ];

// number of extra ticks to add to slo w things down for the radio
uint8_t stream_slowdown ;

// millis value to calculate cli timeout relative to.
// exists so we can separate the cli entry time from the system start time
uint32_t _cli_timeout ;
};

#endif // __GCS_H

166
GCS _Mavlink.pde
// -*- tab-width: 4; Mode: C++; c -basic-offset: 4; indent -tabs-mode: nil -*-

// use this to prevent recursion during sensor init
static bool in_mavlink_delay ;

// this costs us 51 bytes, but means that low priority
// messages don't block th e CPU
static mavlink_statustext_t pending_status ;

// true when we have received at least 1 MAVLink packet
static bool mavlink_active ;

// true if we are out of time in our event timeslice
static bool gcs_out_of_time ;

// check if a message will fit in th e payload space available
#define CHECK_PAYLOAD_SIZE (id) if (payload_space < MAVLINK_MSG_ID_ ## id ## _LEN) return
false

// prototype this for use inside the GCS class
static void gcs_send_text_fmt (const prog_char_t *fmt, …);

static void gcs_send_heart beat(void)
{
gcs_send_message (MSG_HEARTBEAT );
}

static void gcs_send_deferred (void)
{
gcs_send_message (MSG_RETRY_DEFERRED );
}

/*
* !!NOTE!!
*
* the use of NOINLINE separate functions for each message type avoids
* a compiler bug in gcc tha t would cause it to use far more stack
* space than is needed. Without the NOINLINE we use the sum of the
* stack needed for each message type. Please be careful to follow the
* pattern below when adding any new messages
*/

static NOINLINE void send_heartbeat (mavlink_channel_t chan )
{
uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED ;
uint8_t system_status = MAV_STATE_ACTIVE ;
uint32_t custom_mode = control_mode ;

167
if (ap.failsafe_radio == true) {
system_status = MAV_STATE_CRITICAL ;
}

// work out the base_mode. This value is not very useful
// for APM, but we calculate it as best we can so a generic
// MAVLink enabled ground station can work out something about
// what the MAV is up to. The actu al bit values are highly
// ambiguous for most of the APM flight modes. In practice, you
// only get useful information from the custom_mode, which maps to
// the APM flight mode and has a well defined meaning in the
// ArduPlane documentat ion
base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED ;
switch (control_mode ) {
case AUTO:
case RTL:
case LOITER:
case GUIDED:
case CIRCLE:
base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED ;
// note that MAV_MODE_FLAG_AUTO_ENAB LED does not match what
// APM does in any mode, as that is defined as "system finds its own goal
// positions", which APM does not currently do
break;
}

// all modes except INITIALISING have some form of manual
// override if stick mixing is enabled
base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED ;

#if HIL_MODE != HIL_MODE_DISABLED
base_mode |= MAV_MODE_FLAG_HIL_ENABLED ;
#endif

// we are armed if we are not initialising
if (motors.armed()) {
base_mode |= MAV_MODE_FLAG_SAFETY_ARMED ;
}

// indicate we have set a custom mode
base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED ;

mavlink_msg_heartbeat_send (
chan,
MAV_TYPE_QUADROTOR ,
MAV_AUTOPILOT_ARDUPILOTMEGA ,
base_mode ,
custom_mode ,
system_status );
}

static NOINLINE void send_attitude (mavlink_channel_t chan )

168
{
mavlink_msg_attitude_send (
chan,
millis(),
ahrs.roll,
ahrs.pitch,
ahrs.yaw,
omega.x,
omega.y,
omega.z);
}

#if AC_FENCE == ENABLED
static NOINLINE void send_limits_status (mavlink_channel_t chan )
{
fence_send_mavlink_status (chan);
}
#endif

static NOINLINE void send_extended_status1 (mavlink_channel_t chan , uint16_t packet_drops )
{
uint32_t control_sensors_present = 0;
uint32_t control_sensors_enabled ;
uint32_t control_sensors_health ;

// first what sensors/controllers we have
control_sensors_present |= (1<<0); // 3D gyro present
control_sensor s_present |= (1<<1); // 3D accelerometer present
if (g.compass_enabled ) {
control_sensors_present |= (1<<2); // compass present
}
control_sensors_present |= (1<<3); // absolute pressure sensor present
if (g_gps != NULL && g_gps->status() >= GPS::NO_FIX) {
control_sensors_present |= (1<<5); // GPS present
}
control_sensors_present |= (1<<10); // 3D angular rate control
control_sensors_present |= (1<<11); // attitude stabilisation
control_sensors_present |= (1<<12); // yaw position
control_sensors_present |= (1<<13); // altitude control
control_sensors_present |= (1<<14); // X/Y position control
control_sensors_present |= (1<<15); // motor control

// now what sensors/controllers are enabled

// first the sensors
control_sensors_enabled = control_sensors_present & 0x1FF;

// now the controllers
control_sensors_enabled = control_sensors_present & 0x1FF;

control_sensors_enabled |= (1<<10); // 3D angular rate control

169
control_s ensors_enabled |= (1<<11); // attitude stabilisation
control_sensors_enabled |= (1<<13); // altitude control
control_sensors_enabled |= (1<<15); // motor control

switch (control_mode ) {
case AUTO:
case RTL:
case LOITER:
case GUIDED:
case CIRCLE:
case POSITION:
control_sensors_enabled |= (1<<12); // yaw position
control_sensors_enabled |= (1<<14); // X/Y position control
break;
}

// at the moment all sensors/controllers are assumed healthy
control_sensors_health = control_sensors_present ;

if (!compass.healthy) {
control_sensors_health &= ~(1<<2); // compass
}
if (!compass.use_for_yaw ()) {
control_sensors_enabled &= ~(1<<2); // compass
}

uint16_t bat tery_current = -1;
uint8_t battery_remaining = -1;

if (current_total1 != 0 && g.pack_capacity != 0) {
battery_remaining = (100.0f * (g.pack_capacity – current_total1 ) /
g.pack_capacity );
}
if (current_total1 != 0) {
battery_current = current_amps1 * 100;
}

if (g.battery_monitoring == BATT_MONITOR_VOLTAGE_ONLY ) {
/*setting a out -of-range value.
* It informs to external devices that
* it cannot be calculated properly just by voltage*/
battery_remaining = 150;
}

mavlink_msg_sys_status_send (
chan,
control_sensors_present ,
control_sensors_enabled ,
control_sensors_health ,
0, // CPU Load not supported in AC yet
battery_voltage1 * 1000, // mV
battery_current , // in 10mA units

170
battery_remaining , // in %
0, // comm drops %,
0, // comm drops in pkts,
0, 0, 0, 0);

}

static void NOINLINE send_meminfo (mavlink_channel_t chan )
{
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2
extern unsigned __brkval ;
mavlink_msg_meminfo_send (chan, __brkval , memcheck_available_memory ());
#endif
}

static void NOINLINE send_location (mavlink_channel_t chan )
{
uint32_t fix_time ;
// if we have a GPS fix, take the time as the last fix time. That
// allows us to correctly calculate velocities and extrapolate
// positions.
// If we don't have a GPS fix then we are dead reckoning, and will
// use the curre nt boot time as the fix time.
if (g_gps->status() >= GPS::GPS_OK_FIX_2D ) {
fix_time = g_gps->last_fix_time ;
} else {
fix_time = millis();
}
mavlink_msg_global_position_int_send (
chan,
fix_time ,
current_loc .lat, // in 1E7 degrees
current_loc .lng, // in 1E7 degrees
g_gps->altitude * 10, // millimeters above sea level
(current_loc .alt – home.alt) * 10, // millimeters above gro und
g_gps->velocity_north () * 100, // X speed cm/s (+ve North)
g_gps->velocity_east () * 100, // Y speed cm/s (+ve East)
g_gps->velocity_down () * -100, // Z speed cm/s (+ve up)
g_gps->ground_course ); // course in 1/100 degree
}

static void NOINLINE send_nav_controller_output (mavlink_channel_t chan )
{
mavlink_msg_nav_controller_output_send (
chan,
nav_roll / 1.0e2f,
nav_pitch / 1.0e2f,
wp_bearing / 1.0e2f,
wp_bearing / 1.0e2f,
wp_distance / 1.0e2f,
altitude_error / 1.0e2f,
0,

171
0);
}

static void NOINLINE send_ahrs (mavlink_channel_t chan )
{
Vector3f omega_I = ahrs.get_gyro_drift ();
mavlink_msg_ahrs_send (
chan,
omega_I.x,
omega_I.y,
omega_I.z,
1,
0,
ahrs.get_error_rp (),
ahrs.get_error_yaw ());
}

// report simulator state
static void NOINLINE send_simstate (mavlink_channel_t chan )
{
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
sitl.simstate_send (chan);
#endif
}

static void NOINLINE send_hwstatus (mavlink_channel_t chan )
{
mavlink_msg_hwstatus_send (
chan,
board_voltage (),
hal.i2c->lockup_count ());
}

static void NOINLINE send_gps_raw (mavlink_channel _t chan)
{
mavlink_msg_gps_raw_int_send (
chan,
g_gps->last_fix_time *(uint64_t )1000,
g_gps->status(),
g_gps->latitude , // in 1E7 degrees
g_gps->longitude , // in 1E7 degrees
g_gps->altitude * 10, // in mm
g_gps->hdop,
65535,
g_gps->ground_speed , // cm/s
g_gps->ground_course , // 1/100 degrees,
g_gps->num_sats );

}

static void NOINLINE send_servo_out (mavlink_channel_t chan )
{

172
// normalized values scaled to -10000 to 10000
// This is used for HIL. Do not change without discussing with HIL maintainers

#if FRAME_CONFIG == HELI_FRAME

mavlink_msg_rc_channels_scaled_send (
chan,
millis(),
0, // port 0
g.rc_1.servo_out ,
g.rc_2.servo_out ,
g.rc_3.radio_out ,
g.rc_4.servo_out ,
0,
0,
0,
0,
receiver_rssi );
#else
#if X_PLANE == ENABLED
/* update by JLN for X -Plane HIL */
if(motors.armed() && ap.auto_armed ) {
mavlink_msg_rc_channels_scaled_send (
chan,
millis(),
0, // port 0
g.rc_1.servo_out ,
g.rc_2.servo_out ,
10000 * g.rc_3.norm_output (),
g.rc_4.servo_out ,
10000 * g.rc_1.norm_output (),
10000 * g.rc_2.norm_output (),
10000 * g.rc_3.norm_output (),
10000 * g.rc_4.norm_output (),
receiver_rssi );
}else{
mavlink_msg_rc_channels_scaled_send (
chan,
millis(),
0, // port 0
0,
0,
-10000,
0,
10000 * g.rc_1.norm_output (),
10000 * g.rc_2.norm_output (),
10000 * g.rc_3.norm_output (),
10000 * g.rc_4.norm_output (),
receiver_rssi );
}

#else

173
mavlink_msg_rc_channels_scaled_send (
chan,
millis(),
0, // port 0
g.rc_1.servo_out ,
g.rc_2.servo_out ,
g.rc_3.radio_out,
g.rc_4.servo_out ,
10000 * g.rc_1.norm_output (),
10000 * g.rc_2.norm_output (),
10000 * g.rc_3.norm_output (),
10000 * g.rc_4.norm_output (),
receiver_rssi );
#endif
#endif
}

static void NOINLINE send_radi o_in(mavlink_channel_t chan )
{
mavlink_msg_rc_channels_raw_send (
chan,
millis(),
0, // port
g.rc_1.radio_in ,
g.rc_2.radio_in ,
g.rc_3.radio_in ,
g.rc_4.radio_in ,
g.rc_5.radio_in ,
g.rc_6.radio_in ,
g.rc_7.radio_in ,
g.rc_8.radio_in ,
receiver_rssi );
}

static void NOINLINE send_radio_out (mavlink_channel_t chan )
{
mavlink_msg_servo_output_raw_send (
chan,
micros(),
0, // port
motors.motor_out [AP_MOTORS_MOT_1 ],
motors.motor_out [AP_MOTORS_MOT_2 ],
motors.motor_out [AP_MOTORS_MOT_3 ],
motors.motor_out [AP_MOTORS_MOT_4 ],
motors.motor_out [AP_MOTORS_MOT_5 ],
motors.motor_out [AP_MOTORS_MOT_6 ],
motors.motor_out [AP_MOTORS_MOT_7 ],
motors.motor_out [AP_MOTORS_MOT_8 ]);
}

static void NOINLINE send_vfr_hud (mavlink_channel_t chan )
{

174
mavlink_msg_vfr_hud_send (
chan,
(float)g_gps->ground_speed / 100.0f,
(float)g_gps->ground_speed / 100.0f,
(ahrs.yaw_sensor / 100) % 360,
g.rc_3.servo_out /10,
current_loc .alt / 100.0f,
climb_rate / 100.0f);
}

static void NOINLINE send_raw_imu1 (mavlink_channel_t chan )
{
Vector3f accel = ins.get_accel ();
Vector3f gyro = ins.get_gyro ();
mavlink_msg_raw_imu_send (
chan,
micros(),
accel.x * 1000.0f / GRAVITY_MSS ,
accel.y * 1000.0f / GRAVITY_MSS ,
accel.z * 1000.0f / GRAVITY_MSS ,
gyro.x * 1000.0f,
gyro.y * 1000.0f,
gyro.z * 1000.0f,
compass.mag_x,
compass.mag_y,
compass.mag_z);
}

static void NOINLINE send_raw_imu2 (mavlink_channel_t chan )
{
mavlink_msg_scaled_pressure_send (
chan,
millis(),
(float)barometer .get_pressure ()/100.0f,
(float)(barometer .get_pressure () – barometer .get_ground_pressure ())/100.0f,
(int)(barometer .get_temperature ()*10));
}

static void NOINLINE send_raw_imu3 (mavlink_channel_t chan )
{
Vector3f mag_offsets = compass.get_offsets ();
Vector3f accel_offsets = ins.get_accel_offsets ();
Vector3f gyro_offsets = ins.get_gyro_offsets ();

mavlink_msg_sensor_offsets_send (chan,
mag_offsets .x,
mag_offsets .y,
mag_offsets .z,
compass.get_declination (),
barometer .get_raw_pressure (),
barometer .get_raw_tem p(),
gyro_offsets .x,

175
gyro_offsets .y,
gyro_offsets .z,
accel_offsets .x,
accel_offsets.y,
accel_offsets .z);
}

static void NOINLINE send_current_waypoint (mavlink_channel_t chan )
{
mavlink_msg_mission_current_send (
chan,
(uint16_t )g.command_index );
}

static void NOINLINE send_statustext (mavlink_channel_t chan )
{
mavlink_msg_statustext_send (
chan,
pending_status .severity ,
pending_status .text);
}

// are we still delaying telemetry to try to avoid Xbee bricking?
static bool telemetry_delayed (mavlink_chan nel_t chan )
{
uint32_t tnow = millis() >> 10;
if (tnow > (uint8_t)g.telem_delay ) {
return false;
}
#if USB_MUX_PIN > 0
if (chan == MAVLINK_COMM_0 && ap_system .usb_connected ) {
// this is an APM2 with USB telemetry
return false;
}
// we're either on the 2nd UART, or no USB cable is connected
// we need to delay telemetry
return true;
#else
if (chan == MAVLINK_COMM_0 ) {
// we're on the USB port
return false;
}
// don't send te lemetry yet
return true;
#endif
}

// try to send a message, return false if it won't fit in the serial tx buffer
static bool mavlink_try_send_message (mavlink_channel_t chan , enum ap_message id , uint16_t
packet_drops )
{

176
int16_t payload_space = comm_get_txspace (chan) – MAVLINK_NUM_NON_PAYLOAD_BYTES ;

if (telemetry_delayed (chan)) {
return false;
}

// if we don't have at least 1ms remaining before the main loop
// wants to fire then don't send a mavlink message. We want to
// prioritise the main flight control loop over communications
if (scheduler .time_available_usec () < 800 && motors.armed()) {
gcs_out_of_time = true;
return false;
}

switch(id) {
case MSG_HEARTBEAT:
CHECK_PAYLOAD_S IZE(HEARTBEAT );
send_heartbeat (chan);
break;

case MSG_EXTENDED_STATUS1:
CHECK_PAYLOAD_SIZE (SYS_STATUS );
send_extended_status1 (chan, packet_drops );
break;

case MSG_EXTENDED_STATUS2:
CHECK_PAYLOAD_SIZ E(MEMINFO);
send_meminfo (chan);
break;

case MSG_ATTITUDE:
CHECK_PAYLOAD_SIZE (ATTITUDE );
send_attitude (chan);
break;

case MSG_LOCATION:
CHECK_PAYLOAD_SIZE (GLOBAL_POSITION_INT );
send_location (chan);
break;

case MSG_NAV_CONTROLLER_OUTPUT:
CHECK_PAYLOAD_SIZE (NAV_CONTROLLER_OUTPUT );
send_nav_controller_output (chan);
break;

case MSG_GPS_RAW:
CHECK_PAYLOAD_SIZE (GPS_RAW_INT );
send_gps_raw (chan);
break;

case MSG_SERVO_OUT:
CHECK_PAYLOAD_SIZE (RC_CHANNELS_SCALED );

177
send_servo_out (chan);
break;

case MSG_RADIO_IN:
CHECK_PAYLOAD_SIZE (RC_CHANNELS_RAW );
send_radio_in (chan);
break;

case MSG_RADIO_OUT:
CHECK_PAYLOAD_SIZE (SERVO_OUTPUT_RAW );
send_radio_out (chan);
break;

case MSG_VFR_HUD:
CHECK_PAYLOAD_SIZE (VFR_HUD);
send_vfr_hud (chan);
break;

case MSG_RAW_IMU1:
CHECK_PAYLOA D_SIZE(RAW_IMU);
send_raw_imu1 (chan);
break;

case MSG_RAW_IMU2:
CHECK_PAYLOAD_SIZE (SCALED_PRESSURE );
send_raw_imu2 (chan);
break;

case MSG_RAW_IMU3:
CHECK_PAYLOAD_SIZE (SENSOR_OFFSETS );
send_raw_imu3(chan);
break;

case MSG_CURRENT_WAYPOINT:
CHECK_PAYLOAD_SIZE (MISSION_CURRENT );
send_current_waypoint (chan);
break;

case MSG_NEXT_PARAM:
CHECK_PAYLOAD_SIZE (PARAM_VALUE );
if (chan == MAVLINK_CO MM_0) {
gcs0.queued_param_send ();
} else if (gcs3.initialised ) {
gcs3.queued_param_send ();
}
break;

case MSG_NEXT_WAYPOINT:
CHECK_PAYLOAD_SIZE (MISSION_REQUEST );
if (chan == MAVLINK_COMM_0 ) {
gcs0.queued_waypoint_send ();
} else {

178
gcs3.queued_waypoint_send ();
}
break;

case MSG_STATUSTEXT:
CHECK_PAYLOAD_SIZE (STATUSTEXT );
send_statustext (chan);
break;

#if AC_FENCE == ENABLED
case MSG_LIMITS_STATUS:
CHECK_PAYLOAD_SIZE (LIMITS_STATUS );
send_limits_status (chan);
break;
#endif

case MSG_AHRS:
CHECK_PAYLOAD_SIZE (AHRS);
send_ahrs (chan);
break;

case MSG_SIMSTATE:
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
CHECK_PAYLOAD_SIZE (SIMSTATE );
send_simstate (chan);
#endif
break;

case MSG_HWSTATUS:
CHECK_PAYLOAD_SIZE (HWSTATUS );
send_hwstatus (chan);
break;

case MSG_RETRY_ DEFERRED:
break; // just here to prevent a warning
}

return true;
}

#define MAX_DEFERRED_MESSAGES MSG_RETRY_DEFERRED
static struct mavlink_queue {
enum ap_message deferred_messages [MAX_DEFERRED_MESSAGES ];
uint8_t next_deferred_me ssage;
uint8_t num_deferred_messages ;
} mavlink_queue [2];

// send a message using mavlink
static void mavlink_send_message (mavlink_channel_t chan , enum ap_message id , uint16_t
packet_drops )
{

179
uint8_t i , nextid;
struct mavlink_queue *q = &mavlink_queue [(uint8_t)chan];

// see if we can send the deferred messages, if any
while (q->num_deferred_messages != 0) {
if (!mavlink_try_send_message (chan,
q->deferred_messages [q->next_deferred_message ],
packet_drops )) {
break;
}
q->next_deferred_message ++;
if (q->next_deferred_message == MAX_DEFERRED_MESSAGES ) {
q->next_deferred_message = 0;
}
q->num_defe rred_messages –;
}

if (id == MSG_RETRY_DEFERRED ) {
return;
}

// this message id might already be deferred
for (i=0, nextid = q->next_deferred_message ; i < q->num_deferred_messages ; i++) {
if (q->deferred_messages [nextid] == id) {
// its already deferred, discard
return;
}
nextid++;
if (nextid == MAX_DEFERRED_MESSAGES ) {
nextid = 0;
}
}

if (q->num_deferred_messages != 0 ||
!mavlink_try_s end_message (chan, id, packet_drops )) {
// can't send it now, so defer it
if (q->num_deferred_messages == MAX_DEFERRED_MESSAGES ) {
// the defer buffer is full, discard
return;
}
nextid = q->next_deferr ed_message + q->num_deferred_messages ;
if (nextid >= MAX_DEFERRED_MESSAGES ) {
nextid -= MAX_DEFERRED_MESSAGES ;
}
q->deferred_messages [nextid] = id;
q->num_deferred_messages ++;
}
}

void mavlink_send_text (mavlink_channel_t chan , gcs_severity severity , const char *str)
{
if (telemetry_delayed (chan)) {

180
return;
}

if (severity == SEVERITY_LOW ) {
// send via the deferred queuing system
pending_status .severity = (uint8_t)severity ;
strncpy((char *)pending_status .text, str, sizeof(pending_status .text));
mavlink_send_message (chan, MSG_STATUSTEXT , 0);
} else {
// send immediately
mavlink_msg_statustext_send (
chan,
severity ,
str);
}
}

const AP_Param: :GroupInfo GCS_MAVLINK: :var_info [] PROGMEM = {
// @Param: RAW_SENS
// @DisplayName: Raw sensor stream rate
// @Description: Raw sensor stream rate to ground station
// @Units: Hz
// @Range: 0 10
// @Increment: 1
// @User: Advanced
AP_GROUPINFO ("RAW_SENS" , 0, GCS_MAVLINK , streamRateRawSensors , 0),

// @Param: EXT_STAT
// @DisplayName: Extended status stream rate to ground station
// @Description: Extended status stream r ate to ground station
// @Units: Hz
// @Range: 0 10
// @Increment: 1
// @User: Advanced
AP_GROUPINFO ("EXT_STAT" , 1, GCS_MAVLINK , streamRateExtendedStatus , 0),

// @Param: RC_CHAN
// @DisplayName: RC Channel stream rate to groun d station
// @Description: RC Channel stream rate to ground station
// @Units: Hz
// @Range: 0 10
// @Increment: 1
// @User: Advanced
AP_GROUPINFO ("RC_CHAN" , 2, GCS_MAVLINK , streamRateRCChannels , 0),

// @Param: RAW_CTRL
// @DisplayName: Raw Control stream rate to ground station
// @Description: Raw Control stream rate to ground station
// @Units: Hz
// @Range: 0 10
// @Increment: 1
// @User: Advanced

181
AP_GROUPINFO ("RAW_CTRL" , 3, GCS_MAVLINK , streamRateRawController , 0),

// @Param: POSITION
// @DisplayName: Position stream rate to ground station
// @Description: Position stream rate to ground station
// @Units: Hz
// @Range: 0 10
// @Increment: 1
// @User: Advanced
AP_GROUPINFO ("POSITION" , 4, GCS_MAVLINK , streamRatePosition , 0),

// @Param: EXTRA1
// @DisplayName: Extra data type 1 stream rate to ground station
// @Description: Extra data type 1 stream rate to ground station
// @Units: Hz
// @Range: 0 10
// @Increment: 1
// @User: Advanced
AP_GROUPINFO ("EXTRA1" , 5, GCS_MAVLINK , streamRateExtra1 , 0),

// @Param: EXTRA2
// @DisplayName: Extra data type 2 stream rate to ground station
// @Description: Extra data type 2 stream rate to ground station
// @Units: Hz
// @Range: 0 10
// @Increment: 1
// @User: Advanced
AP_GROUPINFO ("EXTRA2" , 6, GCS_MAVLINK , streamRateExtra2 , 0),

// @Param: EXTRA3
// @DisplayName: Extra data t ype 3 stream rate to ground station
// @Description: Extra data type 3 stream rate to ground station
// @Units: Hz
// @Range: 0 10
// @Increment: 1
// @User: Advanced
AP_GROUPINFO ("EXTRA3" , 7, GCS_MAVLINK , streamRateExtra3 , 0),

// @Param: PARAMS
// @DisplayName: Parameter stream rate to ground station
// @Description: Parameter stream rate to ground station
// @Units: Hz
// @Range: 0 10
// @Increment: 1
// @User: Advanced
AP_GROUPINFO ("PARAMS", 8, GCS_MAVLINK , streamRateParams , 0),
AP_GROUPEND
};

GCS_MAVLINK: :GCS_MAVLINK () :
packet_drops (0),

182
waypoint_send_timeout (1000), // 1 second
waypoint_receive_timeout (1000) // 1 second
{

}

void
GCS_MAVLINK: :init(AP_HAL::UARTDriver * port)
{
GCS_Class: :init(port);
if (port == hal.uartA) {
mavlink_comm_0_port = port;
chan = MAVLINK_COMM_0 ;
}else{
mavlink_comm_1_port = port;
chan = MAVLINK_COMM_1 ;
}
_queued_parameter = NULL;
reset_cli_timeout ();
}

void
GCS_MAVLINK: :update(void)
{
// receive new packets
mavlink_message_t msg ;
mavlink_status_t status ;
status.packet_rx_drop_count = 0;

// process received bytes
uint16_t nbytes = comm_get_available (chan);
for (uint16_t i =0; i<nbytes; i++) {
uint8_t c = comm_receive_ch (chan);

#if CLI_ENABLED == ENABLED
/* allow CLI to be started by hitting enter 3 times, if no
* heartbeat packets have been received */
if (mavlink_act ive == 0 && (millis() – _cli_timeout ) < 20000 &&
!motors.armed() && comm_is_idle (chan)) {
if (c == '\n' || c == '\r') {
crlf_count ++;
} else {
crlf_count = 0;
}
if (crlf_count == 3) {
run_cli(_port);
}
}
#endif

// Try to get a new message
if (mavlink_parse_char (chan, c, &msg, &status)) {

183
// we exclude radio packets to make it possible to use the
// CLI over the radio
if (msg.msgid != MAVLINK_MSG_ID_RADIO ) {
mavlink_active = true;
}
handleMessage (&msg);
}
}

// Update packet drops counter
packet_drops += status.packet_rx_ drop_count ;

if (!waypoint_receiving && !waypoint_sending ) {
return;
}

uint32_t tnow = millis();

if (waypoint_receiving &&
waypoint_request_i <= waypoint_request_last &&
tnow > waypoint_timelast_request + 500 + (stream_slowdown *20)) {
waypoint_timelast_request = tnow;
send_message (MSG_NEXT_WAYPOINT );
}

// stop waypoint sending if timeout
if (waypoint_sending && (tnow – waypoint_timelast_send ) > waypoint_send_timeout ) {
waypoint_s ending = false;
}

// stop waypoint receiving if timeout
if (waypoint_receiving && (tnow – waypoint_timelast_receive ) >
waypoint_receive_timeout ) {
waypoint_receiving = false;
}
}

// see if we should send a stream now. Called at 50 Hz
bool GCS_MAVLINK: :stream_trigger (enum streams stream_num )
{
uint8_t rate ;
switch (stream_num ) {
case STREAM_RAW_SENSORS:
rate = streamRateRawSensors .get();
break;
case STREAM_EXTENDED_STATUS:
rate = streamRateExtendedStatus .get();
break;
case STREAM_RC_CHANNELS:
rate = streamRateRCChannels .get();
break;
case STREAM_RAW_CONTROLLER:

184
rate = streamRateRawController .get();
break;
case STREAM_POSITION:
rate = streamRatePosition .get();
break;
case STREAM_EXTRA1:
rate = streamRateExtra1 .get();
break;
case STREAM_EXTRA2:
rate = streamRateExtra2 .get();
break;
case STREAM_EXTRA3:
rate = streamRateExtra3 .get();
break;
case STREAM_PARAMS:
rate = streamRateParams .get();
break;
default:
rate = 0;
}

if (rate == 0) {
return false;
}

if (stream_ticks [stream_num ] == 0) {
// we're triggering now, setup the next trigger point
if (rate > 50) {
rate = 50;
}
stream_ticks [stream_num ] = (50 / rate) + stream_slowdown ;
return true;
}

// count down at 50Hz
stream_ticks [stream_num ]–;
return false;
}

void
GCS_MAVLINK: :data_stream_send (void)
{
if (waypoint_receiving || waypoint_sending ) {
// don't interfere with mission tr ansfer
return;
}

gcs_out_of_time = false;

if (_queued_parameter != NULL) {
if (streamRateParams .get() <= 0) {
streamRateParams .set(50);

185
}
if (stream_trigger (STREAM_PARAMS )) {
send_messag e(MSG_NEXT_PARAM );
}
// don't send anything else at the same time as parameters
return;
}

if (gcs_out_of_time ) return;

if (in_mavlink_delay ) {
// don't send any other stream types while in the delay callback
return;
}

if (stream_trigger (STREAM_RAW_SENSORS )) {
send_message (MSG_RAW_IMU1 );
send_message (MSG_RAW_IMU2 );
send_message (MSG_RAW_IMU3 );
//cliSerial ->printf("mav1 %d \n", (int)streamRateRawSensors.get());
}

if (gcs_out_of_time ) return;

if (stream_trigger (STREAM_EXTENDED_STATUS )) {
send_message (MSG_EXTENDED_STATUS1 );
send_message (MSG_EXTENDED_STATUS2 );
send_message (MSG_CURRENT_WAYPOINT );
send_message (MSG_GPS_RAW );
send_message (MSG_NAV_CONTROLLER_OUTPUT );
send_message (MSG_LIMITS_STATUS );
}

if (gcs_out_of_time ) return;

if (stream_trigger (STREAM_POSITION )) {
send_message (MSG_LOCATION );
}

if (gcs_out_of_time ) return;

if (stream_trigger (STREAM_RAW_CONTROLLER )) {
send_message (MSG_SERVO_OUT );
//cliSerial ->printf("mav4 %d \n", (int)streamRateRawController.get());
}

if (gcs_out_of_time ) return;

if (stream_trigger (STREAM_RC_CHANNELS )) {
send_message(MSG_RADIO_OUT );
send_message (MSG_RADIO_IN );
//cliSerial ->printf("mav5 %d \n", (int)streamRateRCChannels.get());

186
}

if (gcs_out_of_time ) return;

if (stream_trigger (STREAM_EXTRA1 )) {
send_message (MSG_ATTITUDE );
send_message (MSG_SIMSTATE );
//cliSerial ->printf("mav6 %d \n", (int)streamRateExtra1.get());
}

if (gcs_out_of_time ) return;

if (stream_trigger (STREAM_EXTRA2 )) {
send_message (MSG_VFR_HUD );
//cliSerial ->printf("mav7 %d \n", (int)streamRateExtra2.get());
}

if (gcs_out_of_time ) return;

if (stream_trigger (STREAM_EXTRA3 )) {
send_message (MSG_AHRS );
send_message (MSG_HWSTATUS );
}
}

void
GCS_MAVLINK: :send_message (enum ap_message id )
{
mavlink_send_message (chan,id, packet_drops );
}

void
GCS_MAVLINK: :send_text_P (gcs_severity severity , const prog_char_t *str)
{
mavlink_statustext_t m ;
uint8_t i ;
for (i=0; i<sizeof(m.text); i++) {
m.text[i] = pgm_read_byte ((const prog_char *)(str++));
if (m.text[i] == '\0') {
break;
}
}
if (i < sizeof(m.text)) m.text[i] = 0;
mavlink_send_text (chan, severity , (const char *)m.text);
}

void GCS_MAVLINK: :handleMessage (mavlink_message_t * msg)
{
struct Location tell_command = {}; // command for
telemetry

187
switch (msg->msgid) {

case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: //66
{
// decode
mavlink_request_data_stream_t packet ;
mavlink_ms g_request_data_stream_decode (msg, &packet);

if (mavlink_check_target (packet.target_system , packet.target_component ))
break;

int16_t freq = 0; // packet frequency

if (packet.start_stop == 0)
freq = 0; // stop sending
else if (packet.start_stop == 1)
freq = packet.req_message_rate ; // start sending
else
break;

switch(packet.req_stream_id ) {

case MAV_DATA_STREAM_ALL:
streamRateRawSensors = freq;
streamRateExtendedStatus = freq;
streamRateRCChannels = freq;
streamRateRawController = freq;
streamRatePositi on = freq;
streamRateExtra1 = freq;
streamRateExtra2 = freq;
//streamRateExtra3.set_and_save(freq); // We just do set and save on the
last as it takes ca re of the whole group.
streamRateExtra3 = freq; //
Don't save!!
break;

case MAV_DATA_STREAM_RAW_SENSORS:
streamRateRawSensors = freq; // We do
not set and save this one so that if HIL is shut down incorrectly
// we will not continue to broadcast raw sensor data at 50Hz.
break;
case MAV_DATA_STREAM_EXTENDED_STATUS:
//streamRateExten dedStatus.set_and_save(freq);
streamRateExtendedStatus = freq;
break;

case MAV_DATA_STREAM_RC_CHANNELS:
streamRateRCChannels = freq;
break;

case MAV_DATA_STREAM_RAW_CONTROLLER:
streamRateRawController = freq;

188
break;

//case MAV_DATA_STREAM_RAW_SENSOR_FUSION:
// streamRateRawSensorFusion.set_and_save(freq);
// break;

case MAV_DATA_STREAM_POSITION:
streamRatePosition = freq;
break;

case MAV_DATA_STREAM_EXTRA1:
streamRateExtra1 = freq;
break;

case MAV_DATA_STREAM_EXTRA2:
streamRateExtra2 = freq;
break;

case MAV_DATA_STREAM_EXTRA3:
streamRateExtra3 = freq;
break;

default:
break;
}
break;
}

case MAVLINK_MSG_ID_COMMAND_LONG:
{
// decode
mavlink_command_long_t packet ;
mavlink_msg_command_long_decode (msg, &packet);
if (mavlink_check_target (packet.target_system , packet.target_component )) break;

uint8_t result = MAV_RESULT_UNSUPPORTED ;

// do command
send_text_P (SEVERITY_LOW ,PSTR("command received: " ));

switch(packet.command) {

case MAV_CMD_NAV_LOITER_UNLIM:
set_mode (LOITER);
result = MAV_RESULT_ACCEPTED ;
break;

case MAV_CMD_NAV_RETURN_TO_LAUNCH:
set_mode (RTL);
result = MAV_RESULT_ACCEPTE D;
break;

189
case MAV_CMD_NAV_LAND:
set_mode (LAND);
result = MAV_RESULT_ACCEPTED ;
break;

case MAV_CMD_MISSION_START:
set_mode (AUTO);
result = MAV_RESULT_ACCEPTED ;
break;

case MAV_CMD_PREFLIGHT_CALIBRATION:
if (packet.param1 == 1 ||
packet.param2 == 1 ||
packet.param3 == 1) {
ins.init_accel (flash_leds );
ahrs.set_trim (Vector3f (0,0,0)); // clear out saved trim
}
if (packet.param4 == 1) {
trim_radio ();
}
if (packet.param5 == 1) {
float trim_roll , trim_pitch ;
// this blocks
AP_InertialSensor_UserInteract_MAVLink interact (chan);
if(ins.calibrate_accel (flash_leds , &interact , trim_roll , trim_pitch )) {
// reset ahrs's trim to suggested values from calibration routine
ahrs.set_trim (Vector3f (trim_roll , trim_pitch , 0));
}
}
result = MAV_RESULT_ACCEPTED ;
break;

case MAV_CMD_COMPONENT_ARM_DISARM:
if (packet.target_component == MAV_COMP_ID_SYSTEM _CONTROL ) {
if (packet.param1 == 1.0f) {
// run pre -arm-checks and display failures
pre_arm_checks (true);
if(ap.pre_arm_check ) {
init_arm_motors ();
}
result = MAV_RESULT_ACCEPTED ;
} else if (packet.param1 == 0.0f) {
init_disarm_motors ();
result = MAV_RESULT_ACCEPTED ;
} else {
result = MAV_RESULT_UNSUPPORTED ;
}
} else {
result = MAV_RESULT_UNSUPPORTED ;
}
break;

190
case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
if (packet.param1 == 1) {
reboot_apm ();
result = MAV_RESULT_ACCEPTED ;
}
break;

default:
result = MAV_RESULT_UNSUPPORTED ;
break;
}

mavlink_msg_command_ack_send (
chan,
packet.command,
result);

break;
}

case MAVLINK_MSG_ID_SET_MODE: //11
{
// decode
mavlink_set_mode_t packet ;
mavlink_msg_set_mode_decode (msg, &packet);

if (!(packet.base_mode & MAV_MODE_F LAG_CUSTOM_MODE_ENABLED )) {
// we ignore base_mode as there is no sane way to map
// from that bitmap to a APM flight mode. We rely on
// custom_mode instead.
break;
}
switch (packet.custom_mo de) {
case STABILIZE:
case ACRO:
case ALT_HOLD:
case AUTO:
case GUIDED:
case LOITER:
case RTL:
case CIRCLE:
case POSITION:
case LAND:
case OF_LOITER:
set_mode(packet.custom_mode );
break;
}

break;
}

191
/*case MAVLINK_MSG_ID_SET_NAV_MODE:
* {
* // decode
* mavlink_set_nav_mode_t packet;
* mavlink_msg_set_nav_m ode_decode(msg, &packet);
* // To set some flight modes we must first receive a "set nav mode"
message and then a "set mode" message
* mav_nav = packet.nav_mode;
* break;
* }
*/
case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: //43
{
//send_text_P(SEVERITY_LOW,PSTR("waypoint request list"));

// decode
mavlink_mission_request_list_t packet ;
mavlink_msg_mission_request_list_decode (msg, &packet);
if (mavlink_check_target (packet.target_system , packet.target_component ))
break;

// Start sending waypoints
mavlink_msg_mission_count_send (
chan,msg->sysid,
msg->compid,
g.command_total ); // includes home

waypoint_timelast_send = millis();
waypoint_sending = true;
waypoint_receiving = false;
waypoint_dest_sysid = msg->sysid;
waypoint_dest_compid = msg->compid;
break;
}

// XXX read a WP from EEPROM and send it to the GCS
case MAVLINK_MSG_ID_MISSION_REQUEST: // 40
{
//send_text_P(SEVERITY_LOW,PSTR("waypoint request "));

// Check if sending waypiont
//if (!waypoint_sending) break;
// 5/10/11 – We are trying out relaxing the requirement that we be in waypoint
sending mode to respond to a waypoint request. DEW

// decode
mavlink_mission_request_t packet ;
mavlink_msg_mission_request_decode (msg, &packet);

if (mavlink_check_target (packet.target_system , packet.target_component ))
break;

192
// send waypoint
tell_command = get_cmd_with_index (packet.seq);

// set frame of waypoint
uint8_t frame ;

if (tell_command .options & MASK_OPTIONS_RELATIVE_ALT ) {
frame = MAV_FRAME_GLOBAL_RELATIVE_ALT ; // reference frame
} else {
frame = MAV_FRAME_GLOBAL ; // reference frame
}

float param1 = 0, param2 = 0, param3 = 0, param4 = 0;

// time that the mav should loiter in milliseconds
uint8_t current = 0; // 1 (true), 0 (f alse)

if (packet.seq == (uint16_t )g.command_index )
current = 1;

uint8_t autocontinue = 1; // 1 (true), 0 (false)

float x = 0, y = 0, z = 0;

if (tell_command .id < MAV_CMD_NAV_LAST ) {
// command needs scaling
x = tell_command .lat/1.0e7f; // local (x), global
(latitude)
y = tell_command .lng/1.0e7f; // local (y), global
(longitude)
// ACM is processing alt insid e each command. so we save and load raw values. –
this is diffrent to APM
z = tell_command .alt/1.0e2f; // local (z), global/relative
(altitude)
}

// Switch to map APM command fields into MAVLink command fiel ds
switch (tell_command .id) {

case MAV_CMD_NAV_LOITER_TURNS:
case MAV_CMD_CONDITION_CHANGE_ALT:
case MAV_CMD_DO_SET_HOME:
param1 = tell_command .p1;
break;

case MAV_CMD_NAV_ROI:
param1 = tell_command .p1; // MAV_ROI (aka roi
mode) is held in wp's parameter but we actually do nothing with it because we only support
pointing at a specific location provided by x,y and z parameters
break;

case MAV_CMD_CONDITION_YAW:

193
param3 = tell_command .p1;
param1 = tell_command .alt;
param2 = tell_command .lat;
param4 = tell_command .lng;
break;

case MAV_CMD_NAV_TAKEOFF:
param1 = 0;
break;

case MAV_CMD_NAV_LOITER_TIME:
param1 = tell_command .p1; // ACM loiter time
is in 1 second increments
break;

case MAV_CMD_CONDITION_DELAY:
case MAV_CMD_CONDITION_DISTANCE:
param1 = tell_command .lat;
break;

case MAV_CMD_DO_JUMP:
param2 = tell_command .lat;
param1 = tell_command .p1;
break;

case MAV_CMD_DO_REPEAT_SERVO:
param4 = tell_command .lng;
case MAV_CMD_DO_REPEAT_RELAY:
case MAV_CMD_DO_CHANGE_SPEED:
param3 = tell_command .lat;
param2 = tell_command .alt;
param1 = tell_command .p1;
break;

case MAV_CMD_NAV_WAYPOINT:
param1 = tell_command .p1;
break;

case MAV_CMD_DO_SET_PARAMETER:
case MAV_CMD_DO_SET_RELAY:
case MAV_CMD_DO_SET_SERVO:
param2 = tell_command .alt;
param1 = tell_command .p1;
break;
}

mavlink_msg_mission_item_send (chan,msg->sysid,
msg->compid,
packet.seq,
frame,
tell_command .id,
current,

194
autocontinue ,
param1,
param2,
param3,
param4,
x,
y,
z);

// update last waypoint comm stamp
waypoint_timelast_send = millis();
break;
}

case MAVLINK_MSG_ID_MISSION_ACK: //47
{
//send_text_P(SEVERITY_LOW,PSTR("waypoint ack"));

// decode
mavlink_mission_ack_t packet ;
mavlink_msg_mission_ack_decode (msg, &packet);
if (mavlink_check_target (packet.target_system ,packet.target_component )) break;

// turn off waypoint send
waypoint_sending = false;
break;
}

case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: // 21
{
// gcs_send_text_P(SEVERITY_LOW,PSTR("param request list"));

// decode
mavlink_param_request_list_t packet ;
mavlink_msg_param_request_list_decode (msg, &packet);
if (mavlink_check_target (packet.target_system ,packet.target_component )) break;

// Start sending parameters – next call to ::update will kick the first one out

_queued_parameter = AP_Param: :first(&_queued_parameter_token ,
&_queued_parameter_type );
_queued_parameter_index = 0;
_queued_parameter_count = _count_parameters ();
break;
}

case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
{
// decode
mavlink_param_request_read_t packet ;
mavlink_msg_param_request_read_decode (msg, &packet);
if (mavlink_check_target (packet.target_system ,packet.target_component )) break;

195
enum ap_var_type p_type ;
AP_Param *vp;
char param_name [AP_MAX_NAME_SIZE +1];
if (packet.param_index != -1) {
AP_Param: :ParamToken token ;
vp = AP_Param: :find_by_index (packet.param_index , &p_type, &token);
if (vp == NULL) {
gcs_send_text_fmt (PSTR("Unknown parameter index %d" ), packet.param_index );
break;
}
vp->copy_nam e_token(token, param_name , AP_MAX_NAME_SIZE , true);
param_name [AP_MAX_NAME_SIZE ] = 0;
} else {
strncpy(param_name , packet.param_id , AP_MAX_NAME_SIZE );
param_name [AP_MAX_NAME_SIZE ] = 0;
vp = AP_Param: :find(param_name , &p_type);
if (vp == NULL) {
gcs_send_text_fmt (PSTR("Unknown parameter %.16s" ), packet.param_id );
break;
}
}

float value = vp->cast_to_float (p_type);
mavlink_m sg_param_value_send (
chan,
param_name ,
value,
mav_var_type (p_type),
_count_parameters (),
packet.param_index );
break;
}

case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: // 45
{
//send_text_P(SEVERITY_LOW,PSTR("waypoint clear all"));

// decode
mavlink_mission_clear_all_t packet ;
mavlink_msg_mission_clear_all_decode (msg, &packet);
if (mavlink_check_target (packet.target_system , packet.target_component )) break;

// clear all waypoints
uint8_t type = 0; // ok (0), error(1)
g.command_total .set_and_save (1);

// send acknowledgement 3 times to makes sure it is received
for (int16_t i =0; i<3; i++)
mavlink_msg_mission_ack_send (chan, msg->sysid, msg->compid, type);

break;
}

196

case MAVLINK_MSG_ID_MISSION_SET_CURRENT: // 41
{
//send_text_P(SEVERITY_LOW,PSTR("waypoint set current"));

// decode
mavlink_mission_set_current_t packet ;
mavlink_msg_mission_set_current_decode (msg, &packet);
if (mavlink_check_target (packet.target_system ,packet.target_component )) break;

// set current command
change_command (packet.seq);

mavlink_msg_mission_current_send (chan, g.command_index );
break;
}

case MAVLINK_MSG_ID_MISSION_COUNT: // 44
{
//send_text_P(SEVERITY_LOW,PSTR("waypoint count"));

// decode
mavlink_mission_ count_t packet ;
mavlink_msg_mission_count_decode (msg, &packet);
if (mavlink_check_target (packet.target_system ,packet.target_component )) break;

// start waypoint receiving
if (packet.count > MAX_WAYPOINTS ) {
packet.count = MAX_WAYPOINTS ;
}
g.command_total .set_and_save (packet.count);

waypoint_timelast_receive = millis();
waypoint_receiving = true;
waypoint_sending = false;
waypoint_request_i = 0;
// note that ArduCopter sets waypoint_request_last to
// command_total -1, whereas plane and rover use
// command_total. This is because the copter code assumes
// command_total includes home
waypoint_request_last = g.command_ total – 1;
waypoint_timelast_request = 0;
break;
}

case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST:
{
// decode
mavlink_mission_write_partial_list_t packet ;
mavlink_msg_mission_write_partial_list_decode (msg, &packet);
if (mavlink_check_target (packet.target_system ,packet.target_component )) break;

197
// start waypoint receiving
if (packet.start_index > g.command_total ||
packet.end_index > g.command_total ||
packet.end_index < packet.start_index ) {
send_text_P (SEVERITY_LOW ,PSTR("flight plan update rejected" ));
break;
}

waypoint_timelast_receive = millis();
waypoint_timelast_request = 0;
waypoint_receiving = true;
waypoint_request_i = packet.start_index ;
waypoint_request_last = packet.end_index ;
break;
}

#ifdef MAVLINK_MSG_ID_SET_MAG_OFFSETS
case MAVLINK_MSG_ID_SET_MAG_OFFSETS:
{
mavlink_set_mag_offsets_t packet ;
mavlink_msg_set_mag_offsets_decode (msg, &packet);
if (mavlink_check_target (packet.target_system ,packet.target_component )) break;
compass.set_offsets (Vector3f (packet.mag_ofs_x , packet.mag_ofs_y ,
packet.mag_ofs_z ));
break;
}
#endif

// XXX receive a WP from GCS and store in EEPROM
case MAVLINK_MSG_ID_MISSION_ITEM: //39
{
// decode
uint8_t result = MAV_MISSION_ACCEPTED ;
mavlink_mission_item_t packet ;
mavlink_msg_mission_item_d ecode(msg, &packet);
if (mavlink_check_target (packet.target_system ,packet.target_component )) break;

// defaults
tell_command .id = packet.command;

/*
* switch (packet.frame){
*
* case MAV_F RAME_MISSION:
* case MAV_FRAME_GLOBAL:
* {
* tell_command.lat = 1.0e7*packet.x; // in as DD converted
to * t7
* tell_command.lng = 1.0e7*packet.y; // in as DD converted
to * t7
* tell_command.alt = packet.z*1.0e2; // in as m converted to
cm

198
* tell_command.options = 0; // absolute altitude
* break;
* }
*
* case MAV_FRAME_LOCAL: // local (relative to home position)
* {
* tell_command.lat = 1.0e7*ToDeg(packet.x/
* (radius_of_earth*cosf(ToRad( home.lat/1.0e7)))) + home.lat;
* tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) +
home.lng;
* tell_command.alt = packet.z*1.0e2;
* tell_command.options = MASK_OPTIONS_RELATIVE_ALT;
* break;
* }
* //case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative
altitude
* default:
* {
* tell_command.lat = 1.0e7 * packet.x; // in as DD converted
to * t7
* tell_command.lng = 1.0e7 * packet.y; // in as DD converted
to * t7
* tell_command.alt = packet.z * 1.0e2;
* tell_command.options = MASK_OPTIONS_RELATIVE_ALT; // store
altitude relative!! Always!!
* break;
* }
* }
*/

// we only are supporting Abs pos ition, relative Alt
tell_command .lat = 1.0e7f * packet.x; // in as DD converted to * t7
tell_command .lng = 1.0e7f * packet.y; // in as DD converted to * t7
tell_command .alt = packet.z * 1.0e2f;
tell_command .options = 1; // store altitude relative to home alt!!
Always!!

switch (tell_command .id) { //
Switch to map APM command fields into MAVLink command fields
case MAV_CMD_NAV_LOITER_TURNS:
case MAV_CMD_DO_SET_HOME:
tell_command .p1 = packet.param1;
break;

case MAV_CMD_NAV_ROI:
tell_command .p1 = packet.param1; // MAV_ROI
(aka roi mode) is held in wp's parameter but we actually do nothing with it because we only
support pointing at a specific location provided by x,y and z parameters
break;

case MAV_CMD_CONDITION_YAW:
tell_command .p1 = packet.param3;

199
tell_command .alt = packet.param1;
tell_command .lat = packet.param2;
tell_command .lng = packet.param4;
break;

case MAV_CMD_NAV_TAKEOFF:
tell_command .p1 = 0;
break;

case MAV_CMD_CONDITION_CHANGE_ALT:
tell_command .p1 = packet.param1 * 100;
break;

case MAV_CMD_NAV_LOITER_TIME:
tell_command .p1 = packet.param1; // APM
loiter time is in ten se cond increments
break;

case MAV_CMD_CONDITION_DELAY:
case MAV_CMD_CONDITION_DISTANCE:
tell_command .lat = packet.param1;
break;

case MAV_CMD_DO_JUMP:
tell_command .lat = packet.param2;
tell_command .p1 = packet.param1;
break;

case MAV_CMD_DO_REPEAT_SERVO:
tell_command .lng = packet.param4;
case MAV_CMD_DO_REPEAT_RELAY:
case MAV_CMD_DO_CHANGE_SPEED:
tell_command .lat = packet.param3;
tell_command .alt = packet.param2;
tell_command .p1 = packet.param1;
break;

case MAV_CMD_NAV_WAYPOINT:
tell_command .p1 = packet.param1;
break;

case MAV_CMD_DO_SET_ PARAMETER:
case MAV_CMD_DO_SET_RELAY:
case MAV_CMD_DO_SET_SERVO:
tell_command .alt = packet.param2;
tell_command .p1 = packet.param1;
break;
}

if(packet.current == 2) { //current =
2 is a flag to tell us this is a "guided mode" waypoint and not for the mission
// initiate guided mode

200
do_guided (&tell_command );

// verify we recevied the command
mavlink_msg_mission_ack_send (
chan,
msg->sysid,
msg->compid,
0);

} else if(packet.current == 3) {
//current = 3 is a flag to tell us this is a alt change only

// add home alt if needed
if (tell_command .options & MASK_OPTIONS_RELATIVE_ALT ) {
tell_command .alt += home.alt;
}

// To-Do: update target altitude for loiter or waypoint con troller depending
upon nav mode
// similar to how do_change_alt works
wp_nav.set_desired_alt (tell_command .alt);

// verify we recevied the command
mavlink_msg_mission_ack_send (
chan,
msg->sysid,
msg->compid,
0);

} else {
// Check if receiving waypoints (mission upload expected)
if (!waypoint_receiving ) {
result = MAV_MISSION_ERROR ;
goto mission_failed ;
}

// check if this is the requested waypoint
if (packet.seq != waypoint_request_i ) {
result = MAV_MISSION_INVALID_SEQUENCE ;
goto mission_failed ;
}

set_cmd_with_index (tell_command , packet.seq);

// update waypoint receiving state machine
waypoint_timelast_receive = millis();
waypoint_timelast_request = 0;
waypoint_request_i ++;

if (waypoint_request_i > waypoint_request_last ) {
mavlink_msg_mission_ack_send (
chan,

201
msg->sysid,
msg->compid,
result);

send_text_P (SEVERITY_LOW ,PSTR("flight plan received" ));
waypoint_receiving = false;
// XXX ignores waypoint radius for individual waypoints, can
// only set WP_RADIUS parameter
}
}
break;

mission_failed:
// we are rejecting the mission/waypoint
mavlink_msg_mission_ack_send (
chan,
msg->sysid,
msg->compid,
result);
break;
}

case MAVLINK_MSG_ID_PARAM_SET: // 23
{
AP_Param *vp;
enum ap_var_type var_type ;

// decode
mavlink_param_set_t packet ;
mavlink_msg_param_set_decode (msg, &packet);

if (mavlink_check_target (packet.target_system , packet.target_component ))
break;

// set parameter

char key[AP_MAX_NAME_SIZE +1];
strncpy(key, (char *)packet.param_id , AP_MAX_NAME_SIZE );
key[AP_MAX_NAME_SIZE ] = 0;

// find the requested parameter
vp = AP_Param: :find(key, &var_type);
if ((NULL != vp) &&
// exists
!isnan(packet.param_value ) &&
// not nan
!isinf(packet.param_value )) {
// not inf

// add a small amount before casting parameter values
// from float to integer to avoid truncating to the
// next lower integer value.

202
float rounding_addition = 0.01;

// handle variables with standard type IDs
if (var_type == AP_PARAM_FLOAT ) {
((AP_Float *)vp)->set_and_save (packet.param_value );
} else if (var_type == AP_PARAM_IN T32) {
#if LOGGING_ENABLED == ENABLED
if (g.log_bitmask != 0) {
Log_Write_Data (DATA_MAVLINK_FLOAT , ((AP_Int32 *)vp)->get());
}
#endif
if (packet.param_value < 0) rounding_addition = -rounding_addition ;
float v = packet.param_value +rounding_addition ;
v = constrain_float (v, -2147483648.0 , 2147483647.0 );
((AP_Int32 *)vp)->set_and_save (v);
} else if (var_type == AP_PARAM_INT16 ) {
#if LOGGING_ENABLED == ENABLED
if (g.log_bitmask != 0) {
Log_Write_Data (DATA_MAVLINK_INT16 , (int16_t)((AP_Int16 *)vp)->get());
}
#endif
if (packet.param_value < 0) rounding_addition = -rounding _addition ;
float v = packet.param_value +rounding_addition ;
v = constrain_float (v, -32768, 32767);
((AP_Int16 *)vp)->set_and_save (v);
} else if (var_type == AP_PARAM_INT8 ) {
#if LOGGING_ENABLED == ENABLED
if (g.log_bitmask != 0) {
Log_Write_Data (DATA_MAVLINK_INT8 , (int8_t)((AP_Int8 *)vp)->get());
}
#endif
if (packet.param_value < 0) rounding_addition = -rounding_addition ;
float v = packet.param_value +rounding_addition ;
v = constrain_float (v, -128, 127);
((AP_Int8 *)vp)->set_and_save (v);
} else {
// we don't support mavlink set on this parameter
break;
}

// Report back the new value if we accepted the change
// we send the value we actually set, which could be
// different from the value sent, in case someone sent
// a fractional value to an integer type
mavlink_msg_param_value_send (
chan,
key,
vp->cast_to_float (var_type ),
mav_var_type (var_type ),
_count_parameters (),
-1); // XXX we don't actually know what its index
is…

203
DataFlash .Log_Write_Parameter (key, vp->cast_to_float (var_type ));
}

break;
} // end case

case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: //70
{
// allow override of RC channel values for HIL
// or for complete GCS control of switch position
// and RC PWM values.
if(msg->sysid != g.sysid_my_gcs ) break; // Only accept
control from our gcs
mavlink_rc_channels_override_t packet ;
int16_t v [8];
mavlink_msg_rc_channels_override_decode (msg, &packet);

if (mavlink_check_target (packet.target_system ,packet.target_component ))
break;

v[0] = packet.chan1_raw;
v[1] = packet.chan2_raw ;
v[2] = packet.chan3_raw ;
v[3] = packet.chan4_raw ;
v[4] = packet.chan5_raw ;
v[5] = packet.chan6_raw ;
v[6] = packet.chan7_raw ;
v[7] = packet.chan8_raw ;
hal.rcin->set_overrides (v, 8);

// record that rc are overwritten so we can trigger a failsafe if we lose contact
with groundstation
ap.rc_override_active = true;
// a RC override message is consiered to be a 'heartbeat' from the ground stati on
for failsafe purposes
last_heartbeat_ms = millis();
break;
}

#if HIL_MODE != HIL_MODE_DISABLED
case MAVLINK_MSG_ID_HIL_STATE:
{
mavlink_hil_state_t packet ;
mavlink_msg_hil_state_decode (msg, &packet);

float vel = pythagorous2 (packet.vx, packet.vy);
float cog = wrap_360_cd (ToDeg(atan2f(packet.vx, packet.vy)) * 100);

// set gps hil sensor
g_gps->setHIL(packet.time_usec /1000,
packet.lat*1.0e-7, packet.lon*1.0e-7, packet.alt*1.0e-3,

204
vel*1.0e-2, cog*1.0e-2, 0, 10);

if (gps_base_alt == 0) {
gps_base_alt = g_gps->altitude ;
current_loc .alt = 0;
}

if (!ap.home_is_set ) {
init_home ();
}

// rad/sec
Vector3f gyros ;
gyros.x = packet.rollspeed ;
gyros.y = packet.pitchspeed ;
gyros.z = packet.yawspeed ;

// m/s/s
Vector3f accels ;
accels.x = packet.xacc * (GRAVITY_MSS /1000.0);
accels.y = packet.yacc * (GRAVITY_MSS /1000.0);
accels.z = packet.zacc * (GRAVITY_MSS /1000.0);

ins.set_gyro (gyros);

ins.set_accel (accels);

barometer .setHIL(packet.alt*0.001f);
compass.setHIL(packet.roll, packet.pitch, packet.yaw);

#if HIL_MODE == HIL_MODE_ATTITUDE
// set AHRS hil sensor
ahrs.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed ,
packet.pitchspeed ,packet.yawspeed );
#endif

break;
}
#endif // HIL_MODE != HIL_MODE_DISABLED

case MAVLINK_MSG_ID_HEARTBEAT:
{
// We keep track of the last time we received a heartbeat from our GCS for failsafe
purposes
if(msg->sysid != g.sysid_my_gcs ) break;
last_heartb eat_ms = millis();
pmTest1++;
break;

205
}

#if CAMERA == ENABLED
case MAVLINK_MSG_ID_DIGICAM_CONFIGURE:
{
camera.configure_msg (msg);
break;
}

case MAVLINK_MSG_ID_DIGICAM_CONTROL:
{
camera.control_msg(msg);
break;
}
#endif // CAMERA == ENABLED

#if MOUNT == ENABLED
case MAVLINK_MSG_ID_MOUNT_CONFIGURE:
{
camera_mount .configure_msg (msg);
break;
}

case MAVLINK_MSG_ID_MOUNT_CONTROL:
{
camera_mou nt.control_msg (msg);
break;
}

case MAVLINK_MSG_ID_MOUNT_STATUS:
{
camera_mount .status_msg (msg);
break;
}
#endif // MOUNT == ENABLED

case MAVLINK_MSG_ID_RADIO:
{
mavlink_radio_t packet ;
mavlink_msg_radio_decode (msg, &packet);
// use the state of the transmit buffer in the radio to
// control the stream rate, giving us adaptive software
// flow control
if (packet.txbuf < 20 && stream_slowdown < 100) {
// we are very low on space – slow down a lot
stream_slowdown += 3;
} else if (packet.txbuf < 50 && stream_slowdown < 100) {
// we are a bit low on space, slow down slightly
stream_slowdown += 1;
} else if (packet.txbuf > 95 && stream_slowdown > 10) {
// the buffer has plenty of space, speed up a lot
stream_slowdown -= 2;

206
} else if (packet.txbuf > 90 && stream_slowdown != 0) {
// the buffer has enough space, spe ed up a bit
stream_slowdown –;
}
break;
}

/* To-Do: add back support for polygon type fence
#if AC_FENCE == ENABLED
// receive an AP_Limits fence point from GCS and store in EEPROM
// receive a fence point from GCS and store in EEPROM
case MAVLINK_MSG_ID_FENCE_POINT: {
mavlink_fence_point_t packet;
mavlink_msg_fence_point_decode(msg, &packet);
if (packet.count != geofence_limit.fence_total()) {
send_text_P(SEVERITY_LOW,PSTR("ba d fence point"));
} else {
Vector2l point;
point.x = packet.lat*1.0e7f;
point.y = packet.lng*1.0e7f;
geofence_limit.set_fence_point_with_index(point, packet.idx);
}
break;
}
// send a fence point to GCS
case MAVLINK_MSG_ID_FENCE_FETCH_POINT: {
mavlink_fence_fetch_point_t packet;
mavlink_msg_fence_fetch_point_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component))
break;
if (packet.idx >= geofence_limit.fence_total()) {
send_text_P(SEVERITY_LOW,PSTR("bad fence point"));
} else {
Vector2l point = geofence_limit.get_fence_point_with_index(packet.idx);
mavlink_msg_fence_point_send(chan, 0, 0, packet.idx,
geofence_limit.fence_total(),
point.x*1.0e -7f, point.y*1.0e -7f);
}
break;
}
#endif // AC_FENCE ENABLED
*/

} // end switch
} // end hand le mavlink

uint16_t
GCS_MAVLINK: :_count_parameters ()
{
// if we haven't cached the parameter count yet…
if (0 == _parameter_count ) {
AP_Param *vp;

207
AP_Param: :ParamToken token ;

vp = AP_Param: :first(&token, NULL);
do {
_parameter_count ++;
} while (NULL != (vp = AP_Param: :next_scalar (&token, NULL)));
}
return _parameter_count ;
}

/**
* queued_param_send – Send the next pending parameter, called from deferred message
* handling code
*/
void
GCS_MAVLINK: :queued_param_send ()
{
// Check to see if we are sending parameters
if (NULL == _queued_parameter ) return;

AP_Param *vp;
float value;

// copy the current parameter and prepare to move to the next
vp = _queued_ parameter ;

// if the parameter can be cast to float, report it here and break out of the loop
value = vp->cast_to_float (_queued_parameter_type );

char param_name [AP_MAX_NAME_SIZE ];
vp->copy_name_token (_queued_parameter_token , param_name , sizeof(param_name ), true);

mavlink_msg_param_value_send (
chan,
param_name ,
value,
mav_var_type (_queued_parameter_type ),
_queued_parameter_count ,
_queued_parameter_index );

_queued_parameter = AP_Param ::next_scalar (&_queued_parameter_token ,
&_queued_parameter_type );
_queued_parameter_index ++;
}

/**
* queued_waypoint_send – Send the next pending waypoint, called from deferred message
* handling code
*/
void
GCS_MAVLINK: :queued_waypoint_send ()
{

208
if (waypoint_receiving &&
waypoint_request_i <= waypoint_request_last ) {
mavlink_msg_mission_request_send (
chan,
waypoint_dest_sysid ,
waypoint_dest_compid ,
waypoint_request_i );
}
}

void GCS_MAVLINK: :reset_cli_timeout () {
_cli_timeout = millis();
}

/*
* a delay() callback that processes MAVLink packets. We set this as the
* callback in long running library initialisation routines to allow
* MAVLink to process packets while w aiting for the initialisation to
* complete
*/
static void mavlink_delay_cb ()
{
static uint32_t last_1hz , last_50hz , last_5s;

if (!gcs0.initialised ) return;

in_mavlink_delay = true;

uint32_t tnow = millis();
if (tnow – last_1hz > 1000) {
last_1hz = tnow;
gcs_send_heartbeat ();
gcs_send_message (MSG_EXTENDED_STATUS1 );
}
if (tnow – last_50hz > 20) {
last_50hz = tnow;
gcs_check_input ();
gcs_data_stream_send ();
gcs_send_defe rred();
}
if (tnow – last_5s > 5000) {
last_5s = tnow;
gcs_send_text_P (SEVERITY_LOW , PSTR("Initialising APM…" ));
}
#if USB_MUX_PIN > 0
check_usb_mux ();
#endif

in_mavlink_delay = false;
}

/*

209
* send a message on both GCS links
*/
static void gcs_send_message (enum ap_message id )
{
gcs0.send_message (id);
if (gcs3.initialised ) {
gcs3.send_message (id);
}
}

/*
* send data streams in the given rate range on both links
*/
static void gcs_data_stream_ send(void)
{
gcs0.data_stream_send ();
if (gcs3.initialised ) {
gcs3.data_stream_send ();
}
}

/*
* look for incoming commands on the GCS links
*/
static void gcs_check_input (void)
{
gcs0.update();
if (gcs3.initialised ) {
gcs3.update();
}
}

static void gcs_send_text_P (gcs_severity severity , const prog_char_t *str)
{
gcs0.send_text_P (severity , str);
if (gcs3.initialised ) {
gcs3.send_text_P (severity , str);
}
}

/*
* send a low priority formatted m essage to the GCS
* only one fits in the queue, so if you send more than one before the
* last one gets into the serial buffer then the old one will be lost
*/
static void gcs_send_text_fmt (const prog_char_t *fmt, …)
{
va_list arg_list ;
pending_status .severity = (uint8_t)SEVERITY_LOW ;
va_start (arg_list , fmt);
hal.util->vsnprintf_P ((char *)pending_status .text,
sizeof(pending_status .text), fmt, arg_list );

210
va_end(arg_list );
mavlink_send_message (MAVLINK_COMM_0 , MSG_STATUSTEXT, 0);
if (gcs3.initialised ) {
mavlink_send_message (MAVLINK_COMM_1 , MSG_STATUSTEXT , 0);
}
}
………..

211
12 CD / DVD
Autorul atașează în această anexă obligatorie, versiunea electronică a aplicației, a acestei
lucrări, precum și prezentarea finală a tezei.

212
13 INDEX
A
Asamblarea ………………………….. …………………….. 58
B
Bibliografie ………………………….. …………………….. 82
C
Cuprins ………………………….. ………………………….. 13
CD/DVD ………………………….. ………………………. 211
Cod sursa ………………………….. ……………………….. 86
Concluzie ………………………….. ……………………….. 80
D
Descriere hardw are ………………………….. ………….. 37
Descriere software ………………………….. …………… 62
Domenii de ulilizare ………………………….. ………… 73
I
Introducere ………………………….. ……………………… 18
Index ………………………….. ………………………….. .. 212
L
Lista figurilor ………………………….. ………………….. 15
P
Programare ESC ………………………….. ……………… 60
R
Referințe web ………………………….. ………………….. 84
S
Scurt breviar teoretic ………………………….. ……….. 28