PFD (Primary Flight Display) pour Flightgear - ESP32

Horizon artificiel +Compas, Altitude, Vitesse, réglages autopilot, ILS pour Flightgear, simulateur de vol Open Source. Puissance ESP32.

1 Un aperçu en vidéo (LFMT)



.

L'ESP32 se connecte au programme de simulation de vol FlightGear par liaison série USB et permet, outre l'affichage des paramètres de vol, de gérer le pilote automatique de Flightgear en temps réel, et de contrôler totalement l'approche et atterrissage la piste choisie.

Cette vidéo est un montage montrant, en incrustation, le fonctionnement de cette réalisation et le comportement de l'avion, ici le Citation X, lors d'un vol complet : Décollage de LFMT, mise en palier, engagement de l'autopilot de FG, puis tour de piste vent arrière, approche et posé ILS totalement automatique.

La vidéo est une peu "crénelée" afin d'éviter un fichier trop gros, mais bien entendu, sur le PC sous Linux, avec un écran FUll HD, l'image est bien plus belle. Vous avez quand même intérêt à visionner celle-ci en tout écran.

Concernant les paysages : il en est de bien plus beau (dans Flightgear j'entends) que celui-ci, je pense à la Corse (après mise à jour de la scène), ou Annecy, voire Tarbes, c'est à dire près des montagnes (pour ce qui concerne la France)...

2 Vue d'ensemble de cette réalisation :

L'intégration dans un boîtier maison (imprimé en 3D) est prévu, avec peut être d'autres instruments de bord.

3 Le schéma

A noter que les ports GPIO 34 et 35 ne sont pas pourvus de résistances de rappel à VCC (Pull-up) internes dans l'ESP32, d'où l'ajout des deux résistances externes de 10k.
La saisie des paramètres pour l'autopilot se fait avec deux classiques encodeurs rotatifs. Les quatre capas de 10nF sont en cms à souder au au plus près des encodeurs (hors implantation du board donc).

Il ne reste plus beaucoup de broches libres sur le module ESP32 !

4 Le programme pour l'ESP32 à compiler avec l'IDE Arduino

CODE SOURCE du PFD en C++
  1. //
  2. // ==================================
  3. String version="26.1";
  4. // ==================================
  5.  
  6. /*
  7. PFD.ino - Primary Flight Display pour Flightgear et ESP32 - version Afficheur TFT 480x320
  8.  
  9. par Silicium628
  10.  
  11. Ce logiciel est libre et gratuit sous licence GNU GPL
  12.  
  13. */
  14.  
  15. /**---------------------------------------------------------------------------------------
  16. Pour les #includes issus de l'univers Arduino (que je ne fournis pas), il faut voir au cas par cas.
  17. (drivers d'affichage en particulier)
  18.  
  19. ---------------------------------------------------------------------------------------
  20. De petites images à placer sur la SDcard centrées sur les aérodromes proviennent de OpenStreetMap
  21.  
  22. OpenStreetMap® est un ensemble de données ouvertes,
  23. disponibles sous la licence libre Open Data Commons Open Database License (ODbL)
  24. accordée par la Fondation OpenStreetMap (OSMF).
  25.  
  26. Voir ce lien pour plus de détails :
  27. https://www.openstreetmap.org/copyright
  28. --------------------------------------------------------------------------------------**/
  29.  
  30. /*=====================================================================================================
  31. CONCERNANT L'AFFICHAGE TFT : connexion :
  32.  
  33. ( Pensez à configurer le fichier User_Setup.h de la bibliothèque ~/Arduino/libraries/TFT_eSPI/ )
  34.  
  35. les lignes qui suivent ne sont q'un commentaire pour vous indiquer la config à utiliser
  36. placée ici, elle ne sont pas fonctionnelles
  37. Il FAUT modifier le fichier User_Setup.h installé par le système Arduino dans ~/Arduino/libraries/TFT_eSPI/
  38.  
  39. // ESP32 pins used for the parallel interface TFT
  40. #define TFT_CS 27 // Chip select control pin
  41. #define TFT_DC 14 // Data Command control pin - must use a pin in the range 0-31
  42. #define TFT_RST 26 // Reset pin
  43.  
  44. #define TFT_WR 12 // Write strobe control pin - must use a pin in the range 0-31
  45. #define TFT_RD 13
  46.  
  47. #define TFT_D0 16 // Must use pins in the range 0-31 for the data bus
  48. #define TFT_D1 4 // so a single register write sets/clears all bits
  49. #define TFT_D2 2 // 23
  50. #define TFT_D3 22
  51. #define TFT_D4 21
  52. #define TFT_D5 15 // 19
  53. #define TFT_D6 25 // 18
  54. #define TFT_D7 17
  55. =====================================================================================================
  56.  
  57. Notes :
  58. - Si les data de FG ne sont pas reçues, il faut vérifier que le PFD est bien connecté sur le port USB0 (et pas USB1 ou autre...)
  59. */
  60.  
  61.  
  62. // v14 la fonction Autoland() utilise le glide pour la descente et non plus une altitude calculée théorique
  63. // v15.2 modif du fichier hardware4.xml (nav-distance)
  64. // v15.3 revu autoland - possibilité de poser auto si localizer seul (aérodrome non équipé de glide de glide).
  65. // v15.6 nombreuses modifs dans tous les fichiers, dont fichier FG_data.h
  66. // v16.0 prise en charge du module "SW" (boutons poussoirs) par WiFi
  67. // v16.2 prise en charge des 2 nouveaux boutons (target_speed) + le fichier "hardware4.xml" a été modifié en conséquence
  68. // v20.0 autoland possible (par GPS au lieu de l'ILS) même pour aérodromes non équipé ILS
  69. // v21.0 autoland toujours calculé par GPS, avec prise en compte de l'altitude de l'aérodrome
  70. // v22.0 modifs fonction autolang & fichier 'hardware4.xml' (prise en charge du trim de profondeur, voir 'desengage_autoland()'
  71. // v24.2 tous les calculs de position et de direction se font directement avec les coordonnées GPS, sans passer par système LAMBERT
  72. // v26.0 prise en compte de la longueur de la piste pour l'autolanding
  73.  
  74.  
  75. /* le numéro de version doit être identique à celui du ND (Navigation Display) et des autres fichiers sources */
  76.  
  77.  
  78. /** un peu de théorie : ****************************
  79.  
  80. ALTITUDE vs HAUTEUR
  81.  
  82. Une hauteur est la distance verticale entre un aéronef et la surface qu'il survole (terre ou eau).
  83.  
  84. Pour exprimer une hauteur, il est défini les hauteurs AGL (Above Ground Level) ou ASFC (Above Surface).
  85. Il s'agit de la hauteur entre l'avion et le sol juste en dessous de sa position. Elle suit donc le relief.
  86.  
  87. Pour exprimer une hauteur au dessus de l'aérodrome, il est défini la hauteur AAL (Above Aerodrome Level).
  88. Il s'agit de la hauteur entre l'avion et le point de référence de l'aérodrome comme s'il était en dessous de la
  89. position de l'appareil (même s'il n'y est pas). Cette hauteur ne suit pas le relief.
  90.  
  91. source : https://aeroclub-narbonne.com/download/2017/04/BASE_ALT.pdf
  92. IVAO TM © ELH FLA septembre 2014
  93. je conseille de lire et relire l'ensemble de ce PDF.
  94.  
  95. *****************************************************/
  96.  
  97.  
  98. #include "PFD.h"
  99. #include <stdint.h>
  100. #include <TFT_eSPI.h> // Hardware-specific library
  101. #include "Free_Fonts.h"
  102.  
  103. #include "FS.h"
  104. #include "SD.h"
  105. #include "SPI.h"
  106.  
  107. #include "FG_data.h"
  108. #include "Fonctions1.h"
  109.  
  110. #include <WiFi.h> // Ce PFD est un serveur WiFi
  111. #include "ESPAsyncWebServer.h"
  112.  
  113.  
  114. const char* ssid = "PFD_srv";
  115. const char* password = "72r4TsJ28";
  116.  
  117. AsyncWebServer server(80); // Create AsyncWebServer object on port 80
  118.  
  119. String argument_recu1;
  120. String argument_recu2;
  121.  
  122. // TFT_eSPI TFT = TFT_eSPI(); // Configurer le fichier User_Setup.h de la bibliothèque TFT_eSPI au préalable
  123.  
  124. TFT_eSprite SPR_E = TFT_eSprite(&TFT480); // Declare Sprite object "SPR_11" with pointer to "TFT" object
  125. TFT_eSprite SPR_N = TFT_eSprite(&TFT480);
  126. TFT_eSprite SPR_O = TFT_eSprite(&TFT480);
  127. TFT_eSprite SPR_S = TFT_eSprite(&TFT480);
  128.  
  129. TFT_eSprite SPR_trajectoire = TFT_eSprite(&TFT480);
  130.  
  131. VOYANT voyant_L; // Localizer
  132. VOYANT voyant_G; // Glide
  133. //VOYANT voyant_descente_GPS;
  134.  
  135.  
  136. //uint16_t s_width = TFT480.Get_Display_Width();
  137. //uint16_t s_heigh = TFT480.Get_Display_Height();
  138.  
  139. int16_t Ax_actu, Ay_actu;
  140. int16_t Bx_actu, By_actu;
  141.  
  142.  
  143. // COULEURS RGB565
  144. // Outil de sélection -> http://www.barth-dev.de/online/rgb565-color-picker/
  145.  
  146. #define NOIR 0x0000
  147. #define MARRON 0x9240
  148. #define ROUGE 0xF800
  149. #define ROSE 0xFBDD
  150. #define ORANGE 0xFBC0
  151. #define JAUNE 0xFFE0
  152. #define JAUNE_PALE 0xF7F4
  153. #define VERT 0x07E0
  154. #define VERT_FONCE 0x02E2
  155. #define PAMPA 0xBED6
  156. #define OLIVE 0x05A3
  157. #define CYAN 0x07FF
  158. #define BLEU_CLAIR 0x455F
  159. #define AZUR 0x1BF9
  160. #define BLEU 0x001F
  161. #define MAGENTA 0xF81F
  162. #define VIOLET1 0x781A
  163. #define VIOLET2 0xECBE
  164. #define GRIS_TRES_CLAIR 0xDEFB
  165. #define GRIS_CLAIR 0xA534
  166. #define GRIS 0x8410
  167. #define GRIS_FONCE 0x5ACB
  168. #define GRIS_TRES_FONCE 0x2124
  169.  
  170. #define GRIS_AF 0x51C5 // 0x3985
  171. #define BLANC 0xFFFF
  172.  
  173.  
  174. // AVIONIQUE : Horizon Artificiel (HA):
  175.  
  176. #define HA_CIEL 0x33FE
  177. #define HA_SOL 0xAA81 //0xDB60
  178.  
  179. //position et dimensions de l'horizon artificiel
  180.  
  181. #define HA_x0 210
  182. #define HA_y0 130
  183. #define HA_w 120 // demi largeur
  184. #define HA_h 100 // demi hauteur
  185.  
  186. #define x_autopilot 320
  187.  
  188. // Width and height of sprite
  189. #define SPR_W 14
  190. #define SPR_H 14
  191.  
  192.  
  193. /*
  194. #define taille1 100000 //480*320*2
  195. uint8_t data_ecran[taille1];
  196. */
  197.  
  198. uint32_t memo_micros = 0;
  199. uint32_t temps_ecoule;
  200. uint16_t nb_secondes=0;
  201. uint8_t nb_acqui;
  202.  
  203. String parametre; //garde en mémoire les datéess reçues par USB entre les passages dans la fonction "void acquisitions()"
  204.  
  205. uint8_t landing_light1=0;
  206. uint8_t landing_light2=0;
  207.  
  208. float roulis;
  209. float tangage;
  210.  
  211.  
  212. float altitude_GPS_float;
  213. int32_t altitude_GPS; // accepte les valeurs négatives (par exemple si QNH mal réglé avant décollage)
  214. int32_t hauteur_AAL; // (Above Aerodrome Level)
  215.  
  216. #define ASL 0
  217. #define AAL 1
  218.  
  219. uint8_t mode_affi_hauteur = ASL;
  220.  
  221. int32_t gnd_elv;
  222. int32_t vitesse;
  223. int32_t memo_vitesse;
  224. int16_t target_speed =160; // consigne de vitesse pour l'autopilot
  225. int16_t dV;
  226. int16_t acceleration;
  227. int16_t vspeed; // vitesse verticale
  228.  
  229. float cap; // en degrés d'angle; direction actuelle du nez de l'avion
  230.  
  231. int16_t hdg1 = 150; // en degrés d'angle; consigne cap = Heading (HDG) Bug
  232. int16_t memo_hdg1;
  233. uint8_t flag_refresh_hdg=0;
  234. uint8_t flag_traiter_SW=0;
  235.  
  236. float latitude_avion; // WGS84
  237. float longitude_avion; // WGS84
  238.  
  239. float px_par_km;
  240. float px_par_NM;
  241.  
  242.  
  243. float GPS_distance_piste; // en NM
  244. float memo_GPS_distance_piste; // servira à savoir si on se rapproche du centre de la piste ou si on s'en éloigne après
  245. // l'avoir dépassé (lors du décollage)
  246.  
  247. float GPS_azimut_piste;
  248. uint8_t sens_piste = 1; // 0 ou 1
  249.  
  250. float correction_az_piste=0; // pour appliquer de légères corrections à l'orientation de la piste lors d'une approche GPS
  251.  
  252.  
  253. int16_t asel1 = 30; // consigne altitude 30 -> 3000ft
  254. float climb_rate=0; // taux de montée (négatif pour descendre - sert pour attérissage automatique)
  255. float elv_trim=0;
  256.  
  257. float rudder=0;
  258. uint8_t flag_auto_rudder=1; // =1 au décollage, ensuite =0 (en vol et à l'attéro, ou suite à une manoeuvre importante du potar)
  259. uint8_t flag_eloignement=0; // du centre de la piste, utilisé au décollage par l'auto_rudder
  260.  
  261. uint8_t view_number=0;
  262.  
  263. char var_array4[5]; // 4 char + zero terminal - pour envoi par WiFi
  264. char var_array10[11]; // 10 char + zero terminal - pour envoi par WiFi
  265.  
  266. String locks_type;
  267.  
  268. int16_t num_bali=0;
  269.  
  270. uint8_t flag_SDcardOk=0;
  271. uint32_t data_ok=0; // ce n'est pas un flag
  272. //uint8_t gs_ok=0;
  273. uint8_t QNH_ok=0;
  274.  
  275. uint8_t flag_1er_passage =1;
  276. uint8_t au_sol=1;
  277. uint8_t attente_data=1;
  278.  
  279. uint16_t hauteur_mini_autopilot = 100; // ou 300 ou 500 à tester...
  280. uint8_t autoland=0;
  281.  
  282. int16_t loc=0; // localizer
  283. int16_t memo_loc=0;
  284.  
  285. //uint8_t take_off_ok =0; // passe à 1 si altitude atteinte = 3000ft
  286.  
  287.  
  288. float memo_R2;
  289. int16_t memo_y0;
  290.  
  291. uint16_t memo_x_avion=0; // pour fonction "affi_approche()"
  292. uint16_t memo_y_avion=0;
  293.  
  294. const int bouton1 = 36; // attention: le GPIO 36 n'a pas de R de pullup interne, il faut en câbler une (10k) au +3V3
  295. bool bouton1_etat;
  296. bool memo_bouton1_etat;
  297.  
  298. const int bouton2 = 39; // attention: le GPIO 39 n'a pas de R de pullup interne, il faut en câbler une (10k) au +3V3
  299. bool bouton2_etat;
  300. bool memo_bouton2_etat;
  301.  
  302. String switches; // boutons connectés au 3eme ESP32 (SW), reçus par WiFi
  303. uint16_t v_switches=0;
  304. uint16_t memo_v_switches=0;
  305.  
  306. String switches_ND; // boutons connectés au 2eme ESP32 (ND), reçus par WiFi
  307. uint16_t v_switches_ND=0;
  308. uint16_t memo_v_switches_ND=0;
  309.  
  310.  
  311. uint32_t params=0; // destinés au ND
  312.  
  313. String potar1;
  314. int8_t v_potar1=0; // peut être négatif
  315.  
  316. // deux encodeurs rotatifs pas à pas
  317. const int rot1a = 32; // GPIO32 -> câbler une R au +3V3
  318. const int rot1b = 33; // GPIO33 -> câbler une R au +3V3
  319.  
  320. const int rot2a = 35; // GPIO35 -> câbler une R au +3V3
  321. const int rot2b = 34; // GPIO34 -> câbler une R au +3V3
  322.  
  323.  
  324. //const int led1 = 25; // GPIO15
  325.  
  326.  
  327. #define TEMPO 5 // tempo anti-rebond pour l'acquisition des encodeurs rotatifs
  328. volatile uint32_t timer1 = 0;
  329. volatile uint32_t timer2 = 0;
  330.  
  331. uint16_t compteur1;
  332.  
  333. uint8_t heures=0;
  334. uint8_t minutes=0;
  335. uint8_t secondes=0;
  336.  
  337. float v_test1=-1.0;
  338.  
  339. // TFT480.setTextColor(TFT_BLACK, TFT_WHITE);
  340. // TFT480.drawLine(x0, y0, x1, y1, TFT_BLACK);
  341. // TFT480.fillTriangle(x0, y0, x1, y1, x2, y2, TFT_GREEN);
  342. // TFT480.drawRect(5, 3, 230, 119, TFT_BLACK);
  343. // TFT480.fillRect(5, 3, 230, 119, TFT_WHITE);
  344.  
  345.  
  346. void RAZ_variables()
  347. {
  348. roulis=0;
  349. tangage=0;
  350. altitude_GPS=0;
  351. gnd_elv=0;
  352. vitesse=0;
  353. vspeed=0;
  354. cap=0;
  355. memo_hdg1=0;
  356.  
  357. ////ils_loc=0;
  358. ////ils_glide=0;
  359.  
  360. loc=0;
  361. memo_loc=0;
  362.  
  363.  
  364. }
  365.  
  366.  
  367.  
  368. /** ***********************************************************************************
  369. IMAGE.bmp
  370. ***************************************************************************************/
  371.  
  372. /**
  373. Rappel et décryptage de la fonction Color_To_565 : (elle se trouve dans le fichier LCDWIKI_KBV.cpp)
  374.  
  375. //Pass 8-bit (each) R,G,B, get back 16-bit packed color
  376.  
  377. uint16_t Color_To_565(uint8_t r, uint8_t g, uint8_t b)
  378. {
  379. return ((r & 0xF8) << 8) | ((g & 0xFC) << 3) | ((b & 0xF8) >> 3);
  380. }
  381.  
  382. 0xF8 = 11111000
  383. 0xFC = 11111100
  384.  
  385. (r & 0xF8) -> 5 bit de gauche de r (on ignore donc les 3 bits de poids faible)
  386. (g & 0xFC) -> 6 bit de gauche de g (on ignore donc les 2 bits de poids faible)
  387. (b & 0xF8) -> 5 bit de gauche de b (on ignore donc les 3 bits de poids faible)
  388.  
  389. rrrrr---
  390. gggggg--
  391. bbbbb---
  392.  
  393. après les décallages on obtient les 16 bits suivants:
  394.  
  395. rrrrr---========
  396.   gggggg--===
  397.   ===bbbbb
  398.  
  399. soit après le ou :
  400.  
  401. rrrrrggggggbbbbb
  402.  
  403. calcul de la Fonction inverse :
  404. RGB565_to_888
  405. **/
  406.  
  407.  
  408.  
  409. void RGB565_to_888(uint16_t color565, uint8_t *R, uint8_t *G, uint8_t *B)
  410. {
  411. *R=(color565 & 0xFFFFF800) >> 8;
  412. *G=(color565 & 0x7E0) >> 3;
  413. *B=(color565 & 0x1F) << 3 ;
  414. }
  415.  
  416.  
  417.  
  418. /** -----------------------------------------------------------------------------------
  419. CAPTURE D'ECRAN vers SDcard
  420. /** ----------------------------------------------------------------------------------- */
  421.  
  422. void write_TFT_on_SDcard() // enregistre le fichier .bmp
  423. {
  424.  
  425. //TFT480.setTextColor(VERT, NOIR);
  426. //TFT480.drawString("CP", 450, 300);
  427.  
  428. if (flag_SDcardOk==0) {return;}
  429.  
  430. String s1;
  431. uint16_t ys=200;
  432. TFT480.setFreeFont(FF1);
  433. TFT480.setTextColor(JAUNE, NOIR);
  434.  
  435. uint16_t x, y;
  436. uint16_t color565;
  437. uint16_t bmp_color;
  438. uint8_t R, G, B;
  439.  
  440. if( ! SD.exists("/bmp/capture2.bmp"))
  441. {
  442. TFT480.fillRect(0, 0, 480, 320, NOIR); // efface
  443. TFT480.setTextColor(ROUGE, NOIR);
  444. TFT480.drawString("NO /bmp/capture2.bmp !", 100, ys);
  445. delay(300);
  446. TFT480.fillRect(100, ys, 220, 20, NOIR); // efface
  447. return;
  448. }
  449.  
  450.  
  451. File File1 = SD.open("/bmp/capture2.bmp", FILE_WRITE); // ouverture du fichier binaire (vierge) en écriture
  452. if (File1)
  453. {
  454. /*
  455. Les images en couleurs réelles BMP888 utilisent 24 bits par pixel:
  456. Il faut 3 octets pour coder chaque pixel, en respectant l'ordre de l'alternance bleu, vert et rouge.
  457. */
  458. uint16_t bmp_offset = 138;
  459. File1.seek(bmp_offset);
  460.  
  461.  
  462. TFT480.setTextColor(VERT, NOIR);;
  463.  
  464. for (y=320; y>0; y--)
  465. {
  466. for (x=0; x<480; x++)
  467. {
  468. color565=TFT480.readPixel(x, y);
  469.  
  470. RGB565_to_888(color565, &R, &G, &B);
  471.  
  472. File1.write(B); //G
  473. File1.write(G); //R
  474. File1.write(R); //B
  475. }
  476.  
  477. s1=(String) (y/10);
  478. TFT480.fillRect(450, 300, 20, 20, NOIR);
  479. TFT480.drawString(s1, 450, 300);// affi compte à rebour
  480. }
  481.  
  482. File1.close(); // referme le fichier
  483.  
  484. TFT480.fillRect(450, 300, 20, 20, NOIR); // efface le compte à rebour
  485. }
  486. }
  487.  
  488. /** ----------------------------------------------------------------------------------- */
  489.  
  490.  
  491.  
  492.  
  493. void Draw_arc_elliptique(uint16_t x0, uint16_t y0, int16_t dx, int16_t dy, float alpha1, float alpha2, uint16_t couleur)
  494. // alpha1 et alpha2 en radians
  495. {
  496. /*
  497. REMARQUES :
  498. -cette fonction permet également de dessiner un arc de cercle (si dx=dy), voire le cercle complet
  499. - dx et dy sont du type int (et pas uint) et peuvent êtres négafifs, ou nuls.
  500. -alpha1 et alpha2 sont les angles (en radians) des caps des extrémités de l'arc
  501. */
  502. uint16_t n;
  503. float i;
  504. float x,y;
  505.  
  506. i=alpha1;
  507. while(i<alpha2)
  508. {
  509. x=x0+dx*cos(i);
  510. y=y0+dy*cos(i+M_PI/2.0);
  511. TFT480.drawPixel(x,y, couleur);
  512. i+=0.01; // radians
  513. }
  514. }
  515.  
  516.  
  517. void affi_rayon1(uint16_t x0, uint16_t y0, uint16_t rayon, double angle, float pourcent, uint16_t couleur_i, bool gras)
  518. {
  519. // trace une portion de rayon de cercle depuis 100%...à pourcent du rayon du cercle
  520. // angle en radians - sens trigo
  521. float x1, x2;
  522. float y1, y2;
  523.  
  524. x1=x0+rayon* cos(angle);
  525. y1=y0-rayon* sin(angle);
  526.  
  527. x2=x0+pourcent*rayon* cos(angle);
  528. y2=y0-pourcent*rayon* sin(angle);
  529.  
  530. TFT480.drawLine(x1, y1, x2, y2, couleur_i);
  531. if (gras)
  532. {
  533. TFT480.drawLine(x1, y1-1, x2, y2-1, couleur_i);
  534. TFT480.drawLine(x1, y1+1, x2, y2+1, couleur_i);
  535. }
  536. }
  537.  
  538.  
  539.  
  540. void affi_rayon2(uint16_t x0, uint16_t y0, float r1, float r2, float angle_i, uint16_t couleur_i, bool gras)
  541. {
  542. // trace une portion de rayon de cercle entre les distances r1 et r2 du centre
  543. // angle_i en degrés décimaux - sens trigo
  544.  
  545. float angle =angle_i/57.3; // (57.3 ~ 180/pi)
  546. int16_t x1, x2;
  547. int16_t y1, y2;
  548.  
  549. x1=x0+int16_t(r1* cos(angle));
  550. y1=y0-int16_t(r1* sin(angle));
  551.  
  552. x2=x0+int16_t(r2* cos(angle));
  553. y2=y0-int16_t(r2* sin(angle));
  554.  
  555. if ((x1>0) && (x2>0) && (y1>0) && (y2>0) && (x1<480) && (x2<480) && (y1<320) && (y2<320) )
  556. {
  557. TFT480.drawLine(x1, y1, x2, y2, couleur_i);
  558.  
  559. if (gras)
  560. {
  561. TFT480.drawLine(x1, y1-1, x2, y2-1, couleur_i);
  562. TFT480.drawLine(x1, y1+1, x2, y2+1, couleur_i);
  563. }
  564. }
  565.  
  566. //TFT480.fillCircle(x2, y2, 2, ROUGE);
  567. }
  568.  
  569.  
  570.  
  571.  
  572. void affi_tiret_H(uint16_t x0, uint16_t y0, uint16_t r, double angle_i, uint16_t couleur_i)
  573. {
  574. // trace un tiret perpendiculaire à un rayon de cercle de rayon r
  575. // angle_i en degrés décimaux
  576.  
  577. float angle =angle_i/57.3; // (57.3 ~ 180/pi)
  578. float x1, x2;
  579. float y1, y2;
  580.  
  581. x1=x0+(r)* cos(angle-1);
  582. y1=y0-(r)* sin(angle-1);
  583.  
  584. x2=x0+(r)* cos(angle+1);
  585. y2=y0-(r)* sin(angle+1);
  586.  
  587. TFT480.drawLine(x1, y1, x2, y2, couleur_i);
  588. }
  589.  
  590.  
  591.  
  592. void affi_pointe(uint16_t x0, uint16_t y0, uint16_t r, double angle_i, float taille, uint16_t couleur_i)
  593. {
  594. // trace une pointe de flèche sur un cercle de rayon r
  595. // angle_i en degrés décimaux - sens trigo
  596.  
  597. float angle =angle_i/57.3; // (57.3 ~ 180/pi)
  598. int16_t x1, x2, x3;
  599. int16_t y1, y2, y3;
  600.  
  601. x1=x0+r* cos(angle); // pointe
  602. y1=y0-r* sin(angle); // pointe
  603.  
  604. x2=x0+(r-7)* cos(angle-taille); // base A
  605. y2=y0-(r-7)* sin(angle-taille); // base A
  606.  
  607. x3=x0+(r-7)* cos(angle+taille); // base B
  608. y3=y0-(r-7)* sin(angle+taille); // base B
  609.  
  610. TFT480.fillTriangle(x1, y1, x2, y2, x3, y3, couleur_i);
  611. }
  612.  
  613.  
  614. void affi_rectangle_incline(uint16_t x0, uint16_t y0, uint16_t r, double angle_i, uint16_t couleur_i)
  615. {
  616. //rectangle inscrit dans le cerce de rayon r
  617. // angle_i en degrés décimaux - sens trigo
  618.  
  619. float angle =angle_i/57.3; // (57.3 ~ 180/pi)
  620. int16_t x1, x2, x3, x4;
  621. int16_t y1, y2, y3, y4;
  622. float d_alpha=0.08; // détermine la largeur du rectangle
  623.  
  624. // point 1
  625. x1=x0+r*cos(angle-d_alpha);
  626. y1=y0+r*sin(angle-d_alpha);
  627. // point 2
  628. x2=x0+r*cos(angle+d_alpha);
  629. y2=y0+r*sin(angle+d_alpha);
  630. // point 3
  631. x3=x0+r*cos(M_PI + angle-d_alpha);
  632. y3=y0+r*sin(M_PI + angle-d_alpha);
  633. // point 4
  634. x4=x0+r*cos(M_PI + angle+d_alpha);
  635. y4=y0+r*sin(M_PI + angle+d_alpha);
  636.  
  637. TFT480.drawLine(x1, y1, x2, y2, couleur_i);
  638. TFT480.drawLine(x2, y2, x3, y3, couleur_i);
  639. TFT480.drawLine(x3, y3, x4, y4, couleur_i);
  640. TFT480.drawLine(x4, y4, x1, y1, couleur_i);
  641.  
  642. }
  643.  
  644.  
  645.  
  646. void affi_indexH(uint16_t x, uint16_t y, int8_t sens, uint16_t couleur)
  647. {
  648. // petite pointe de flèche horizontale
  649. // sens = +1 ou -1 pour orienter la pointe vers la droite ou vers la gauche
  650.  
  651. TFT480.fillTriangle(x, y-4, x, y+4, x+8*sens, y, couleur);
  652. }
  653.  
  654.  
  655.  
  656. void affi_indexV(uint16_t x, uint16_t y, int8_t sens, uint16_t couleur)
  657. {
  658. // petite pointe de flèche verticale
  659. // sens = +1 ou -1 pour orienter la pointe vers le haut ou vers le bas
  660.  
  661. TFT480.fillTriangle(x-4, y, x+4, y, x, y+8*sens, couleur);
  662. }
  663.  
  664.  
  665.  
  666. float degTOrad(float angle)
  667. {
  668. return (angle * M_PI / 180.0);
  669. }
  670.  
  671.  
  672.  
  673. void init_affi_HA()
  674. {
  675. TFT480.fillRect(HA_x0-HA_w, HA_y0-HA_h-1, 2*HA_w, HA_h+1, HA_CIEL);
  676. TFT480.fillRect(HA_x0-HA_w, HA_y0-HA_h + HA_h, 2*HA_w, HA_h, HA_SOL);
  677.  
  678. }
  679.  
  680.  
  681.  
  682.  
  683. void dessine_avion() // sous forme d'équerres horizontales noires entourées de blanc
  684. {
  685. // aile gauche
  686. TFT480.fillRect(HA_x0-102, HA_y0-3, 60, 10, BLANC); //H contour en blanc
  687. TFT480.fillRect(HA_x0-42, HA_y0-3, 10, 19, BLANC); //V
  688.  
  689. TFT480.fillRect(HA_x0-100, HA_y0-1, 60, 5, NOIR); //H
  690. TFT480.fillRect(HA_x0-40, HA_y0-1, 5, 15, NOIR); //V
  691.  
  692.  
  693. // aile droite
  694. TFT480.fillRect(HA_x0+28, HA_y0-3, 64, 10, BLANC); //H contour en blanc
  695. TFT480.fillRect(HA_x0+28, HA_y0-3, 10, 19, BLANC); //V
  696.  
  697. TFT480.fillRect(HA_x0+30, HA_y0-1, 60, 5, NOIR); //H
  698. TFT480.fillRect(HA_x0+30, HA_y0-1, 5, 15, NOIR); //V
  699.  
  700. //carré blanc au centre
  701.  
  702. TFT480.fillRect(HA_x0-4, HA_y0-3, 8, 2, BLANC);
  703. TFT480.fillRect(HA_x0-4, HA_y0-3, 2, 8, BLANC);
  704.  
  705. TFT480.fillRect(HA_x0-4, HA_y0+3, 10, 2, BLANC);
  706. TFT480.fillRect(HA_x0+4, HA_y0-3, 2, 8, BLANC);
  707.  
  708. //affi_dst_NAV();
  709.  
  710. }
  711.  
  712.  
  713.  
  714.  
  715. void affiche_chrono()
  716. {
  717. uint16_t x0=200;
  718. uint16_t y0=0;
  719. TFT480.setFreeFont(FM9);
  720. TFT480.setTextColor(JAUNE);
  721. String s1;
  722. ////if(heures<10){s1+="0";}
  723. ////s1+=String(heures);
  724. ////s1+=":";
  725. if(minutes<10){s1+="0";}
  726. s1+=String(minutes);
  727. s1+=":";
  728. if(secondes<10){s1+="0";}
  729. s1+=String(secondes);
  730. TFT480.fillRect(x0, y0, 55, 15, BLEU); //efface
  731. TFT480.drawString(s1, x0, y0);
  732.  
  733. }
  734.  
  735.  
  736. void inc_chrono()
  737. {
  738. secondes++;
  739. if (secondes>59)
  740. {
  741. secondes=0;
  742. minutes++;
  743. if(minutes>59)
  744. {
  745. minutes=0;
  746. heures++;
  747. if (heures>23)
  748. heures=0;
  749. }
  750. }
  751. }
  752.  
  753.  
  754. void RAZ_chrono()
  755. {
  756. heures=0;
  757. minutes=0;
  758. secondes=0;
  759. }
  760.  
  761.  
  762.  
  763. void lign_sep(uint16_t Ax, uint16_t Ay, uint16_t Bx, uint16_t By)
  764. {
  765. // actualise la ligne de séparation ciel-sol
  766.  
  767. TFT480.drawLine(Ax, Ay-1, Bx, By-1, HA_CIEL);
  768. TFT480.drawLine(Ax, Ay, Bx, By, BLANC);
  769. TFT480.drawLine(Ax, Ay+1, Bx, By+1, HA_SOL);
  770. }
  771.  
  772.  
  773.  
  774. void arrondissement_coins()
  775. {
  776.  
  777. // fillTriangle(int32_t x1,int32_t y1, int32_t x2,int32_t y2, int32_t x3,int32_t y3, uint32_t color);
  778.  
  779. //HG
  780. TFT480.fillTriangle(
  781. HA_x0-HA_w, HA_y0-HA_h-1,
  782. HA_x0-HA_w, HA_y0-HA_h+20,
  783. HA_x0-HA_w+20, HA_y0-HA_h-1,
  784. NOIR);
  785.  
  786.  
  787. //----------------------------------------------
  788. //HD
  789. TFT480.fillTriangle(
  790. HA_x0+HA_w, HA_y0-HA_h-1,
  791. HA_x0+HA_w, HA_y0-HA_h+20,
  792. HA_x0+HA_w-20, HA_y0-HA_h-1,
  793. NOIR);
  794.  
  795.  
  796.  
  797. //----------------------------------------------
  798. //BG
  799. TFT480.fillTriangle(
  800. HA_x0-HA_w, HA_y0+HA_h+1,
  801. HA_x0-HA_w, HA_y0+HA_h-20,
  802. HA_x0-HA_w+20, HA_y0+HA_h+1,
  803. NOIR);
  804.  
  805. //----------------------------------------------
  806. //BD
  807. TFT480.fillTriangle(
  808. HA_x0+HA_w, HA_y0+HA_h+1,
  809. HA_x0+HA_w, HA_y0+HA_h-20,
  810. HA_x0+HA_w-20, HA_y0+HA_h+1,
  811. NOIR);
  812.  
  813. }
  814.  
  815.  
  816. void affi_APP(uint16_t couleur1, uint16_t couleur2)
  817. {
  818. TFT480.setTextColor(couleur1, couleur2);
  819. TFT480.setFreeFont(FF1);
  820. TFT480.drawString("APP", 100, 0);
  821. }
  822.  
  823.  
  824.  
  825. void affi_HA() // Horizon Artificiel
  826. {
  827. String s1;
  828.  
  829. ////String s1=(String) roulis;
  830. ////TFT480.drawString(s1, 400, 20);
  831.  
  832.  
  833. // pivot
  834. int16_t x0=0;
  835. int16_t y0=0;
  836.  
  837. //points d'intersection avec le bord du carré
  838. int16_t Ax, Ay; // sur le bord gauche
  839. int16_t Bx, By; // sur le bord droit
  840. // Le dessin consistera à tracer des segments colorés entre les points A et B
  841.  
  842.  
  843. // roulis -> [-90..+90]
  844.  
  845. // normalisation de la valeur R2 -> toujours >0
  846. float R2 = -1*roulis;
  847. if (R2<0) {R2+=360;} // ce qui est un angle identique, de valeur positive (sens trigo)
  848.  
  849. // le pivot reste centré horizontalement mais se déplace verticalement en fonction du tangage
  850. y0 += 2*tangage;
  851.  
  852. //calcul & memorisation de ces deux facteurs, ce qui évitera 2 calculs de tangente à chaque passage dan la fonction
  853. float tgt_moins = tan(degTOrad(90-R2));
  854. float tgt_plus = tan(degTOrad(90+R2));
  855.  
  856. //-----------------------------------------------------------------------------
  857. // CALCUL COTE DROIT (point B)
  858.  
  859. // calcul du point B d'intersection
  860. Bx=HA_w;
  861. By=y0 + HA_w*tan(degTOrad(R2));
  862.  
  863. //test si le point d'intersection se trouve plus haut que le haut du carré :
  864.  
  865. if(By>HA_h)
  866. {
  867. By=HA_h;
  868. Bx = x0 + (HA_h-y0)*tgt_moins;
  869. }
  870.  
  871. if(By< -HA_h)
  872. {
  873. By= -HA_h;
  874. Bx = x0 + (HA_h+y0)*tgt_plus;
  875. }
  876. //-----------------------------------------------------------------------------
  877. // CALCUL COTE GAUCHE (point A)
  878.  
  879. Ax=-HA_w;
  880. Ay=y0 - HA_w*tan(degTOrad(R2));
  881.  
  882. if(Ay> HA_h)
  883. {
  884. Ay= HA_h;
  885. Ax = x0 + (HA_h-y0)*tgt_moins;
  886. }
  887.  
  888. if(Ay< -HA_h)
  889. {
  890. Ay= -HA_h;
  891. Ax = x0 + (HA_h+y0)*tgt_plus;
  892. }
  893.  
  894.  
  895. //-----------------------------------------------------------------------------
  896. // positionnement de l'ensemble sur l'écran
  897.  
  898. Ax += HA_x0;
  899. Ay += HA_y0;
  900.  
  901. Bx += HA_x0;
  902. By += HA_y0;
  903.  
  904. // pour éviter un tracé hors cadre au premier passage :
  905. if (flag_1er_passage == 1)
  906. {
  907. Ax_actu = Ax;
  908. Ay_actu = Ay;
  909.  
  910. Bx_actu = Bx;
  911. By_actu = By;
  912. flag_1er_passage=0;
  913. }
  914.  
  915.  
  916. //-----------------------------------------------------------------------------
  917. // ligne "verticale" d'inclinaison (tangage)
  918.  
  919. affi_rayon2(HA_x0, HA_y0, 85, -memo_y0, 90-memo_R2, HA_CIEL, false); // efface partie supérieure
  920. affi_rayon2(HA_x0, HA_y0, 85, -y0, 90-R2, BLANC, false); // retrace ligne partie supérieure
  921.  
  922. affi_rayon2(HA_x0, HA_y0, -85,-memo_y0, 90-memo_R2, HA_SOL, false); // efface partie inférieure
  923. affi_rayon2(HA_x0, HA_y0, -85,-y0, 90-R2, VERT, false); // retrace ligne partie inférieure
  924.  
  925. affi_pointe(HA_x0, HA_y0, 85, 90-memo_R2, 0.1, HA_CIEL); // efface
  926. affi_pointe(HA_x0, HA_y0, 85, 90-R2, 0.1, BLANC); // retrace
  927.  
  928.  
  929. //-----------------------------------------------------------------------------
  930. // graduation fixe
  931. TFT480.setFreeFont(FF1);
  932. TFT480.setTextColor(BLANC, GRIS_AF);
  933. TFT480.drawString("30", HA_x0-70, HA_y0-98);
  934. TFT480.drawString("60", HA_x0-120, HA_y0-55);
  935.  
  936. TFT480.drawString("30", HA_x0+60, HA_y0-98);
  937. TFT480.drawString("60", HA_x0+100, HA_y0-55);
  938.  
  939. //-----------------------------------------------------------------------------
  940. // animation de la ligne de séparation horizontale
  941.  
  942. while ((Bx_actu != Bx) || (By_actu != By) || (Ax_actu != Ax) || (Ay_actu != Ay))
  943. {
  944. // déplacements successifs de 1 pixel de chaque extrémités de la ligne
  945.  
  946. //TFT480.drawLine(Bx, By, x2, y2, BLANC);
  947.  
  948. if (Bx_actu < Bx) { Bx_actu++; lign_sep(Ax_actu, Ay_actu, Bx_actu, By_actu); }
  949. if (Bx_actu > Bx) { Bx_actu--; lign_sep(Ax_actu, Ay_actu, Bx_actu, By_actu); }
  950.  
  951. if (Ax_actu < Ax) { Ax_actu++; lign_sep(Ax_actu, Ay_actu, Bx_actu, By_actu); }
  952. if (Ax_actu > Ax) { Ax_actu--; lign_sep(Ax_actu, Ay_actu, Bx_actu, By_actu); }
  953.  
  954. if (By_actu < By) { By_actu++; lign_sep(Ax_actu, Ay_actu, Bx_actu, By_actu); }
  955. if (By_actu > By) { By_actu--; lign_sep(Ax_actu, Ay_actu, Bx_actu, By_actu); }
  956.  
  957. if (Ay_actu < Ay) { Ay_actu++; lign_sep(Ax_actu, Ay_actu, Bx_actu, By_actu); }
  958. if (Ay_actu > Ay) { Ay_actu--; lign_sep(Ax_actu, Ay_actu, Bx_actu, By_actu); }
  959.  
  960. }
  961.  
  962.  
  963. // graduation roulis qui se déplace angulairement avec la ligne de tangage
  964. for (int8_t n=0; n<4; n++)
  965. {
  966. Draw_arc_elliptique(HA_x0, HA_y0, 20*n, 20*n, degTOrad(80-memo_R2+n), degTOrad(100-memo_R2-n), HA_CIEL); // efface bas
  967. Draw_arc_elliptique(HA_x0, HA_y0, 20*n, 20*n, degTOrad(80-R2+n), degTOrad(100-R2-n), BLANC); // trace bas
  968.  
  969. Draw_arc_elliptique(HA_x0, HA_y0, 20*n, 20*n, degTOrad(260-memo_R2+n), degTOrad(280-memo_R2-n), HA_SOL); // efface haut
  970. Draw_arc_elliptique(HA_x0, HA_y0, 20*n, 20*n, degTOrad(260-R2+n), degTOrad(280-R2-n), BLANC); // trace haut
  971. }
  972.  
  973. memo_R2 = R2;
  974. memo_y0 = y0;
  975.  
  976.  
  977.  
  978.  
  979. //-----------------------------------------------------------------------------
  980. arrondissement_coins();
  981.  
  982. if (autoland == 0)
  983. {
  984. affi_dst_GPS();
  985. }
  986.  
  987. }
  988.  
  989.  
  990.  
  991. void affi_acceleration()
  992. {
  993. // POUR TEST **********
  994. ////String s2= (String) acceleration;
  995. ////TFT480.fillRect(100, 50, 200, 20, TFT_BLACK);
  996. ////TFT480.setFreeFont(FF5);
  997. ////TFT480.setTextColor(BLANC, NOIR);
  998. ////TFT480.drawString("Acceleration=", 100, 50);
  999. ////TFT480.drawString(s2, 250, 50);
  1000. // ********************
  1001.  
  1002. //barres verticales colorées juste à droite de la vitesse indiquant sa variation
  1003. uint16_t x0=60;
  1004. uint16_t Y_zero=162;
  1005.  
  1006. int16_t dy=0;
  1007.  
  1008. //"fleche" haute
  1009. TFT480.fillRect(x0, 40, 8, Y_zero, GRIS_TRES_FONCE); // efface haut
  1010. if (acceleration > 1)
  1011. {
  1012. dy= acceleration;
  1013.  
  1014. TFT480.fillRect(x0, Y_zero-dy, 8, dy, VERT); // fleche
  1015. }
  1016.  
  1017.  
  1018. //"fleche" basse
  1019. TFT480.fillRect(x0, Y_zero, 8, 150, GRIS_TRES_FONCE); // efface bas
  1020. if (acceleration < -1)
  1021. {
  1022. dy= -acceleration;
  1023.  
  1024. TFT480.fillRect(x0, Y_zero, 8, dy, JAUNE); // fleche
  1025. }
  1026.  
  1027. TFT480.fillRect(x0, Y_zero, 10, 2, BLANC); // tiret horizontal blanc
  1028.  
  1029. TFT480.fillRect(x0, 310, 8, 20, NOIR);
  1030.  
  1031. }
  1032.  
  1033.  
  1034. void bride(int16_t *valeur)
  1035. {
  1036. int16_t y_min =40;
  1037. int16_t y_max =310;
  1038. if (*valeur<y_min) {*valeur=y_min;}
  1039. if (*valeur>y_max) {*valeur=y_max;}
  1040. }
  1041.  
  1042.  
  1043. void affi_switches()
  1044. {
  1045. TFT480.setTextFont(1);
  1046. TFT480.setTextColor(GRIS);
  1047.  
  1048. TFT480.fillRect(440, 0, 30, 10, NOIR); // efface le nombre précédemment affiché
  1049. TFT480.drawString(switches, 440, 0);
  1050.  
  1051. TFT480.fillRect(440, 10, 30, 10, NOIR); // efface le nombre précédemment affiché
  1052. TFT480.drawString(switches_ND, 440, 10);
  1053.  
  1054.  
  1055. }
  1056.  
  1057.  
  1058. void affi_potars()
  1059. {
  1060. TFT480.fillRect(440, 20, 30, 10, NOIR); // efface le nombre précédemment affiché
  1061. TFT480.setTextColor(ORANGE);
  1062. TFT480.drawString(potar1, 440, 20);
  1063. }
  1064.  
  1065.  
  1066.  
  1067. void affi_vitesse()
  1068. {
  1069. uint16_t x1;
  1070. String s1;
  1071.  
  1072. int16_t y_min =40;
  1073. int16_t y_max =300;
  1074.  
  1075. TFT480.setTextColor(BLANC); // Background is not defined so it is transparent
  1076.  
  1077. //---------------------------------------------------------------------------------------
  1078. //bande verticale multicolore
  1079.  
  1080. #define vitesse_sol 40
  1081. int16_t vitesse_mini1 =90;
  1082. int16_t vitesse_mini2 =130;
  1083. int16_t vitesse_maxi1 =200;
  1084. int16_t vitesse_maxi2 =280;
  1085.  
  1086.  
  1087.  
  1088. //calcul de la position des limites entre les différentes couleurs verticales
  1089.  
  1090. int16_t d1, d2, d3, d4, d5;
  1091.  
  1092. d1=(int16_t)(100 + 3.2*((vitesse - vitesse_sol)));
  1093. d2=(int16_t)(100 + 3.2*((vitesse - vitesse_mini1)));
  1094. d3=(int16_t)(100 + 3.2*((vitesse - vitesse_mini2)));
  1095. d4=(int16_t)(100 + 3.2*((vitesse - vitesse_maxi1)));
  1096. d5=(int16_t)(100 + 3.2*((vitesse - vitesse_maxi2)));
  1097.  
  1098. bride(&d1);
  1099. bride(&d2);
  1100. bride(&d3);
  1101. bride(&d4);
  1102. bride(&d5);
  1103.  
  1104. int16_t h1, h2, h3, h4, h5;
  1105.  
  1106. h1 = y_max-(int16_t)d1;
  1107. h2 = d1-d2;
  1108. h3 = d2-d3;
  1109. h4 = d3-d4;
  1110. h5 = d4-d5;
  1111.  
  1112. TFT480.fillRect(50, 40, 6, (int16_t)d5, ORANGE);
  1113. TFT480.fillRect(50, d5, 6, h5, JAUNE);
  1114. TFT480.fillRect(50, d4, 6, h4, VERT);
  1115. TFT480.fillRect(50, d3, 6, h3, ORANGE);
  1116. TFT480.fillRect(50, d2, 6, h2, ROUGE);
  1117. TFT480.fillRect(50, d1, 6, 300-(int16_t)d1, GRIS);
  1118.  
  1119. TFT480.fillRect(50, 300, 6, 20, NOIR);
  1120.  
  1121. //---------------------------------------------------------------------------------------
  1122. //échelle verticale graduée glissante
  1123.  
  1124. uint16_t y0;
  1125.  
  1126. int16_t vit1;
  1127. float d6;
  1128.  
  1129. TFT480.setFreeFont(FF1);
  1130.  
  1131. y0=3.2*(vitesse%10);
  1132.  
  1133. TFT480.fillRect(0, y_min, 50, y_max-30, GRIS_AF); // bande verticale à gauche
  1134. for(int n=0; n<10; n++)
  1135. {
  1136. d6 =2+y0+32.0*n; // 24 pixels verticalement entre chaque trait -> 10*24 = 240px (hauteur de l'affiur)
  1137. {
  1138. if ( (d6>y_min) && (d6<y_max-10) && (vit1>=0) && (vit1<1000) )
  1139. {
  1140. TFT480.fillRect(45, (int16_t)d6, 10, 2, BLANC); // petits tirets horizontaux
  1141. }
  1142.  
  1143. vit1 = vitesse -10*(n-5);
  1144. vit1 /= 10;
  1145. vit1 *= 10;
  1146. s1=(String) vit1;
  1147.  
  1148. if ( (d6>y_min) && (d6<y_max-10) && (vit1>=0) && (vit1<1000) )
  1149. {
  1150. TFT480.setTextColor(BLANC, GRIS_AF);
  1151. //TFT480.drawString(" ", 9, d6);
  1152. x1=0;
  1153. if(vit1<100){x1=7;} // pour affichage centré
  1154. if(vit1<10){x1=14;}
  1155. if (vit1>=10)
  1156. {
  1157. TFT480.drawString(s1, x1, (uint16_t)d6-5); // Graduation (tous les 20kts)
  1158. }
  1159. }
  1160. }
  1161. }
  1162. TFT480.fillRect(0, 38, 68, 2, NOIR); // efface ; BLEU pour test
  1163.  
  1164. //---------------------------------------------------------------------------------------
  1165. // affichage de la valeur principale
  1166.  
  1167. uint16_t VP_y0 = 150;
  1168.  
  1169. TFT480.setTextColor(BLANC, NOIR);
  1170. TFT480.setFreeFont(FF18);
  1171. s1=(String)vitesse;
  1172.  
  1173. TFT480.fillRect(3, VP_y0, 42, 26, NOIR); //efface le nombre précédemment affiché (pour le cas où on passe de 3 à 2 chiffres)
  1174.  
  1175. if ((vitesse>=0) && (vitesse <1000))
  1176. {
  1177. x1=3;
  1178. if(vitesse<100){x1=10;} // pour affichage centré
  1179. if(vitesse<10){x1=20;}
  1180. TFT480.drawString(s1, x1, VP_y0+3); // affi le nombre
  1181. } // affi en gros à mi-hauteur de l'écran
  1182. else
  1183. { TFT480.fillRect(3, VP_y0, 42, 26, GRIS); }
  1184.  
  1185. TFT480.drawRoundRect(1, VP_y0-1, 45, 28, 5, BLANC); // encadrement de la valeur centrale affichée
  1186.  
  1187. TFT480.fillTriangle(45, VP_y0+7, 45, VP_y0+17, 55, VP_y0+12, NOIR); // petit triangle (curseur) noir
  1188. }
  1189.  
  1190.  
  1191.  
  1192. void affi_asel(int16_t asel_i)
  1193. {
  1194. // consigne ALTITUDE de l'autopilot
  1195. uint16_t x1 =360;
  1196. TFT480.setFreeFont(FF5);
  1197.  
  1198. if(asel_i >=0)
  1199. {
  1200. // ( chiffres en roses en haut à droite)
  1201. String s2 =(String)(asel_i);
  1202. TFT480.setTextColor(ROSE, NOIR);
  1203.  
  1204. TFT480.fillRect(x1, 0, 77, 20, NOIR); // efface
  1205. if(asel_i<10000){x1+=7;}
  1206. if(asel_i<1000){x1+=7;} // pour affichage centré
  1207. if(asel_i<100){x1+=7;}
  1208. if(asel_i<10){x1+=7;}
  1209.  
  1210. TFT480.drawString(s2, x1, 5);
  1211. }
  1212.  
  1213. }
  1214.  
  1215.  
  1216. void affi_target_speed()
  1217. {
  1218. // consigne de vitesse de l'autopilot
  1219. // ( chiffres en rose en haut à gauche )
  1220.  
  1221. String s2 =(String)(target_speed);
  1222. TFT480.setTextColor(ROSE, NOIR);
  1223. TFT480.setFreeFont(FF5);
  1224. uint8_t x1=7;
  1225. TFT480.fillRect(x1, 20, 60, 15, NOIR); // efface
  1226. TFT480.drawString(s2, x1, 20);
  1227. }
  1228.  
  1229.  
  1230.  
  1231.  
  1232. void affi_vt_verticale()
  1233. {
  1234. // affichage analogique sur la droite de l'écran
  1235.  
  1236. uint16_t x0=435;
  1237. uint16_t y0=165;
  1238.  
  1239. float y1;
  1240.  
  1241. uint16_t x1;
  1242. String s1;
  1243.  
  1244. TFT480.fillRect(x0, y0-90, 45, 180, GRIS_AF); // barre grise
  1245. TFT480.fillRect(x0, y0, 25, 2, BLEU_CLAIR); // centre
  1246.  
  1247. // ------------------------
  1248. // graduations sur un arc vertical
  1249. TFT480.setFreeFont(FF1);
  1250.  
  1251. TFT480.setTextColor(BLANC, NOIR);
  1252. TFT480.drawString("ft/mn", x0-8, y0+125);
  1253.  
  1254.  
  1255. TFT480.setTextColor(BLANC, GRIS_AF);
  1256.  
  1257.  
  1258.  
  1259. float angle;
  1260. for(uint8_t n=0; n<7; n++ )
  1261. {
  1262. angle =135+ n*15; // 1 tiret tous les 15 degrés
  1263. affi_rayon1(HA_x0+340, y0, 110, degTOrad(angle), 0.9, BLANC, false); // tirets de graduation
  1264. }
  1265.  
  1266.  
  1267. TFT480.drawString("3", x0+9, y0-90);
  1268. TFT480.drawString("2", x0-3, y0-65);
  1269. TFT480.drawString("1", x0-8, y0-35);
  1270. TFT480.drawString("0", x0-3, y0-5 + 0);
  1271. TFT480.drawString("1", x0-8, y0+25);
  1272. TFT480.drawString("2", x0-3, y0+50);
  1273. TFT480.drawString("3", x0+9, y0+75);
  1274.  
  1275. // ------------------------
  1276. // aiguille à droite de l'écran
  1277. float angle2;
  1278.  
  1279. TFT480.setFreeFont(FF1);
  1280. s1=(String) (vspeed*60);
  1281.  
  1282. angle2 = 180.0 - vspeed *0.92;
  1283.  
  1284. TFT480.fillRect(x0-10, y0-110, 55, 15, GRIS_TRES_FONCE); // efface haut
  1285. TFT480.fillRect(x0-10, y0+105, 55, 15, GRIS_TRES_FONCE); // efface bas
  1286.  
  1287. if ((vspeed > -50) && (vspeed < 50))
  1288. {
  1289. affi_rayon1(HA_x0+330, y0, 100, degTOrad(angle2), 0.7, JAUNE, true);
  1290.  
  1291. TFT480.setTextColor(JAUNE, NOIR);
  1292. }
  1293. else if (vspeed > 50)
  1294. {
  1295. affi_rayon1(HA_x0+330, y0, 110, degTOrad(132), 0.7, JAUNE, true);
  1296. TFT480.setTextColor(JAUNE, NOIR);
  1297.  
  1298. TFT480.drawString(s1, x0-10, y0-110);
  1299. }
  1300. else if (vspeed < -50)
  1301. {
  1302. affi_rayon1(HA_x0+330, y0, 110, degTOrad(228), 0.7, JAUNE, true);
  1303. TFT480.setTextColor(JAUNE, NOIR);
  1304.  
  1305. TFT480.drawString(s1, x0-10, y0+105);
  1306. }
  1307.  
  1308.  
  1309. // affichage digital de la valeur
  1310.  
  1311.  
  1312. /*
  1313. // = vitesse ascensionnelle, sous forme de barres verticales vertes, à droite, près de l'echelle d'altitude
  1314. uint16_t x0=405;
  1315. uint16_t y0=40;
  1316.  
  1317. int16_t dy=0;
  1318.  
  1319. //fleche haute
  1320. TFT480.fillRect(x0, 0, 10, 140, GRIS_FONCE); // efface haut
  1321. if (vspeed > 1)
  1322. {
  1323. dy= vspeed;
  1324.  
  1325. TFT480.fillRect(x0, y0+100-dy, 10, dy, VERT); // fleche
  1326. }
  1327.  
  1328.  
  1329. //fleche basse
  1330. TFT480.fillRect(x0, y0+150, 10, 135, GRIS_FONCE); // efface bas
  1331. if (vspeed < -1)
  1332. {
  1333. dy= -vspeed;
  1334.  
  1335. TFT480.fillRect(x0, y0+150, 10, dy, VERT); // fleche
  1336. }
  1337. */
  1338.  
  1339. }
  1340.  
  1341.  
  1342.  
  1343.  
  1344. void affi_cap()
  1345. {
  1346. // cercle tournant de CAP gradué en bas au centre de l'écran
  1347. // Les lettres 'N' 'S' 'E' 'O' pour Nord Sud Est Ouset sont initialisées sous forme de sprites dans la fonction setup()
  1348.  
  1349. uint16_t x02 = 200;
  1350. uint16_t y02 = 350;
  1351. float angle; // en radians
  1352. //float cap_RD; // en radians (le cap fourni par FG étant en degrés d'angle)
  1353. uint16_t x_spr;
  1354. uint16_t y_spr;
  1355.  
  1356. uint16_t x_hdg;
  1357. uint16_t y_hdg;
  1358.  
  1359. uint8_t R =70;
  1360. uint8_t R2 =R-6;
  1361. /**
  1362. 360° =2 pi rad
  1363. 1° = 2 pi/360 rad = pi/180 rad
  1364. **/
  1365.  
  1366. TFT480.fillCircle(x02,y02, R, GRIS_AF);
  1367.  
  1368. for(uint8_t n=0; n<24; n++ )
  1369. {
  1370. angle = (int16_t)cap+15 + n*15; // 1 tiret tous les 15 degrés
  1371. affi_rayon1(x02, y02, (R-5), degTOrad(angle), 0.9, BLANC, false); // tirets de graduation
  1372. }
  1373. x_hdg = x02 + R2*cos(degTOrad(hdg1-90-cap));
  1374. y_hdg = y02 + R2*sin(degTOrad(hdg1-90-cap));
  1375.  
  1376. TFT480.drawLine(x02, y02, x_hdg, y_hdg, VERT);
  1377. TFT480.drawCircle(x_hdg, y_hdg, 5, VERT); // rond vert sur le cercle = consigne de cap de l'autopilot
  1378.  
  1379. x_spr = x02+R2 * cos(degTOrad(angle));
  1380. y_spr = y02-R2 * sin(degTOrad(angle));
  1381. TFT480.setPivot(x_spr, y_spr);
  1382. SPR_E.pushRotated(-cap+90, TFT_BLACK); // Plot rotated Sprite, black = transparent
  1383.  
  1384. x_spr = x02+R2* cos(degTOrad(angle+90));
  1385. y_spr = y02-R2 * sin(degTOrad(angle+90));
  1386. TFT480.setPivot(x_spr, y_spr);
  1387. SPR_N.pushRotated(-cap, TFT_BLACK);
  1388.  
  1389. x_spr = x02+R2 * cos(degTOrad(angle+180));
  1390. y_spr = y02-R2 * sin(degTOrad(angle+180));
  1391. TFT480.setPivot(x_spr, y_spr);
  1392. SPR_O.pushRotated(-cap-90, TFT_BLACK);
  1393.  
  1394. x_spr = x02+R2 * cos(degTOrad(angle-90));
  1395. y_spr = y02-R2 * sin(degTOrad(angle-90));
  1396. TFT480.setPivot(x_spr, y_spr);
  1397. SPR_S.pushRotated(-cap, TFT_BLACK);
  1398.  
  1399.  
  1400. // petite "maison" dans le cercle (valeur du cap)
  1401.  
  1402. #define a 170 // x général
  1403. #define b a+30
  1404. #define c b+30
  1405. #define d 288 // y général
  1406. #define e d+10
  1407. #define f e+20
  1408.  
  1409. TFT480.drawLine(a, f, c, f, BLANC); // sol
  1410. TFT480.drawLine(a, f, a, e, BLANC); // mur de gauche
  1411. TFT480.drawLine(c, f, c, e, BLANC); // mur de droite
  1412. TFT480.drawLine(a, e, b, d, BLANC); // toit pente gauche
  1413. TFT480.drawLine(c, e, b, d, BLANC); // toit pente droite
  1414.  
  1415.  
  1416. // affi la valeur
  1417. String s1;
  1418. uint16_t x0 = a+1;
  1419. uint16_t y0 = e;
  1420.  
  1421. uint16_t x1= x0;
  1422. if(cap<100){x1+=5;} // pour affichage centré
  1423. if(cap<10){x1+=5;}
  1424.  
  1425. s1=String (cap, 1);
  1426.  
  1427. TFT480.fillRect(x0, y0, 57, 20, NOIR); // efface le nombre précédemment affiché
  1428. TFT480.setTextColor(BLANC, NOIR);
  1429. TFT480.setFreeFont(FM9);
  1430. TFT480.drawString(s1, x1, y0);
  1431.  
  1432.  
  1433. }
  1434.  
  1435.  
  1436.  
  1437. void affi_hauteur()
  1438. {
  1439. /**
  1440. Pour exprimer une hauteur au dessus de l'aérodrome, il est définie la hauteur AAL (Above Aerodrome Level).Il
  1441. s'agit de la hauteur entre l'avion et le point de référence de l'aérodrome comme s'il était en dessous de la
  1442. position de l'appareil (même s'il n'y est pas). Cette hauteur ne suit pas le relief.
  1443. On la calculera ici en retranchant [l'altitude de l'aéroport sélectionné] à [l'altitude GPS].
  1444.  
  1445. En conséquense, il faut impérativement penser à sélectionner dans le module SD le bon aérodrome, celui d'où l'on décolle,
  1446. puis en cas de voyage, celui où l'on va se poser (se qui renseignera son altitude) sinon l'affichage sera faux.
  1447.  
  1448. par exemple si l'on choisit "Montpellier" en étant à Clermont-Ferrand, l'erreur sera de 1089 ft
  1449.  
  1450. Les altitudes des aérodromes sont enregistées ici dans le fichier FG_data.h
  1451. */
  1452.  
  1453. String s1;
  1454. uint16_t x0 =365;
  1455. //---------------------------------------------------------------------------------------
  1456. //échelle verticale graduée glissante
  1457.  
  1458. uint16_t x1;
  1459. uint16_t y0;
  1460. uint16_t hauteur;
  1461. int16_t alt1;
  1462. float d5;
  1463.  
  1464. if (mode_affi_hauteur == AAL) {hauteur = hauteur_AAL;}
  1465. if (mode_affi_hauteur == ASL) {hauteur = altitude_GPS;}
  1466.  
  1467. TFT480.setFreeFont(FF1);
  1468.  
  1469.  
  1470. y0=3.2*(hauteur_AAL%10);
  1471.  
  1472. TFT480.fillRect(x0, 20, 60, 319, GRIS_AF); //efface bande verticale à droite
  1473.  
  1474.  
  1475. for(int n=0; n<10; n++)
  1476. {
  1477. d5 =0+y0+32.0*n; // pixels verticalement entre chaque trait -> 10*24 = 240px (hauteur de l'affi)
  1478. {
  1479. if (d5>=20) // marge en haut
  1480. {
  1481. TFT480.fillRect(x0, (int16_t)d5+5, 5, 2, BLANC); // petits tirets horizontaux
  1482.  
  1483. alt1 = hauteur -10*(n-5);
  1484. alt1 /= 10;
  1485. alt1 *= 10;
  1486. s1=(String) alt1;
  1487.  
  1488. if(alt1>=0)
  1489. {
  1490. TFT480.setTextColor(BLANC, GRIS_AF);
  1491. //TFT480.drawString(" ", 9, d5);
  1492. x1=x0;
  1493. if(alt1<10000){x1+=7;} // pour affichage centré
  1494. if(alt1<1000){x1+=7;}
  1495. if(alt1<100){x1+=7;}
  1496. if(alt1<10){x1+=7;}
  1497. TFT480.drawString(s1, x1, (uint16_t)d5); // Graduation (tous les 20kts)
  1498. }
  1499. }
  1500. }
  1501. }
  1502.  
  1503.  
  1504. //---------------------------------------------------------------------------------------
  1505. // affichage de la valeur principale
  1506.  
  1507. uint16_t x2;
  1508. uint16_t y0b = 155;
  1509. TFT480.fillRect(x0-20, y0b, 80, 25, NOIR); // efface le nombre précédemment affiché
  1510. TFT480.setTextColor(BLANC, NOIR);
  1511. TFT480.setFreeFont(FF18);
  1512.  
  1513. if ((1) && (hauteur < 60000))
  1514. {
  1515. s1=(String) hauteur;
  1516. }
  1517. else {s1="----";}
  1518. x2=x0-20;
  1519. if(hauteur<10000){x2+=10;} // pour affichage centré
  1520. if(hauteur<1000){x2+=10;}
  1521. if(hauteur<100){x2+=10;}
  1522. if(hauteur<10){x2+=10;}
  1523.  
  1524. if(hauteur<0)
  1525. {
  1526. TFT480.setTextColor(ROUGE);
  1527. x2=x0-20; // si valeur négative affichée avec signe "-"
  1528. }
  1529.  
  1530.  
  1531. TFT480.drawString(s1, x2, y0b);
  1532. uint16_t couleur1=GRIS;
  1533. if (mode_affi_hauteur == ASL) {couleur1=BLEU;}
  1534. if (mode_affi_hauteur == AAL) {couleur1=VERT;}
  1535.  
  1536. TFT480.drawRoundRect(x0-20, y0b-3, 75, 28, 5, couleur1); // encadrement de la valeur centrale affichée
  1537.  
  1538. }
  1539.  
  1540.  
  1541.  
  1542.  
  1543. void affi_dst_GPS()
  1544. {
  1545. String s1;
  1546. uint16_t x0=190;
  1547. uint16_t y0=255;
  1548. float nav_nm;
  1549. // rappel: 1 mile marin (NM nautical mile) = 1852m
  1550. //ils_nm = (float)ils_dst / 1852.0;
  1551. //if (ils_nm >99) {ils_nm=0;}
  1552.  
  1553. TFT480.drawRect(x0-47, y0-15, 190, 35, GRIS_FONCE); //encadrement
  1554.  
  1555. TFT480.setTextFont(1);
  1556. TFT480.setTextColor(BLANC, NOIR);
  1557. TFT480.drawString("distance", x0, y0-12);
  1558.  
  1559. TFT480.setFreeFont(FM9);
  1560. TFT480.setTextColor(JAUNE, NOIR);
  1561. TFT480.drawString("RWY", x0-45, y0-12);
  1562.  
  1563. TFT480.setTextColor(BLANC, NOIR);
  1564. uint8_t nb_decimales;
  1565. if(GPS_distance_piste>99) {nb_decimales =0;} else {nb_decimales =1;}
  1566. s1 = String(GPS_distance_piste, nb_decimales);
  1567. if (data_ok == 0) {s1=" --";}
  1568. TFT480.fillRect(x0, y0, 52, 18, NOIR); // efface
  1569. TFT480.setFreeFont(FM9);
  1570. TFT480.drawString(s1, x0, y0);
  1571. TFT480.drawRoundRect(x0, y0-2, 50, 18, 5, GRIS_FONCE); // encadrement de la valeur affichée
  1572.  
  1573. //affi_float_test(GPS_distance_piste_new, 100, 3, VERT, NOIR);
  1574.  
  1575.  
  1576. TFT480.setTextColor(JAUNE, NOIR);
  1577. TFT480.drawString("NM", x0+55, y0);
  1578.  
  1579. //affi_direction_piste // direction de la piste vue de l'avion
  1580. TFT480.setTextFont(1);
  1581. TFT480.setTextColor(BLANC, NOIR);
  1582. TFT480.drawString("direction", x0+80, y0-12);
  1583.  
  1584. TFT480.setTextColor(BLANC, NOIR);
  1585. s1 = String(GPS_azimut_piste, 0); // 0 -> 0 décimales
  1586. if (data_ok == 0) {s1=" --";}
  1587. TFT480.fillRect(x0+90, y0, 52, 18, NOIR); // efface
  1588. TFT480.setFreeFont(FM9);
  1589. TFT480.drawString(s1, x0+90, y0);
  1590. TFT480.drawRoundRect(x0+90, y0-2, 40, 18, 5, GRIS_FONCE); // encadrement de la valeur affichée
  1591.  
  1592. TFT480.drawCircle(x0+135, y0, 2, JAUNE); // caractère 'degré'
  1593.  
  1594. }
  1595.  
  1596.  
  1597.  
  1598.  
  1599. void affi_indicateurs()
  1600. {
  1601.  
  1602. TFT480.setFreeFont(FSS9);
  1603.  
  1604.  
  1605. }
  1606.  
  1607.  
  1608. void affi_Airport()
  1609. {
  1610. uint16_t n;
  1611. float v1;
  1612. String s1;
  1613.  
  1614.  
  1615. TFT480.fillRect(255, 280, 108, 20, NOIR); // efface - BLEU pour test
  1616. TFT480.setTextFont(1);
  1617.  
  1618. TFT480.setTextColor(BLEU_CLAIR, NOIR);
  1619. s1= liste_bali[num_bali].ID_OACI;
  1620. TFT480.drawString(s1, 255, 280);
  1621.  
  1622. s1= (String)liste_bali[num_bali].altitude;
  1623. s1 +=" ft";
  1624. TFT480.setTextColor(VIOLET2, NOIR);
  1625. TFT480.drawString(s1, 300, 280);
  1626.  
  1627. TFT480.fillRect(270, 300, 60, 30, NOIR); // efface - GRIS pour test
  1628. s1= liste_bali[num_bali].nom;
  1629. TFT480.setTextColor(BLEU_CLAIR, NOIR);
  1630. TFT480.drawString(s1, 255, 290);
  1631.  
  1632. }
  1633.  
  1634.  
  1635. void affi_mode_affi_hauteur()
  1636. {
  1637. if (mode_affi_hauteur == AAL)
  1638. {
  1639. TFT480.setFreeFont(FF1);
  1640. TFT480.setTextColor(VERT, GRIS_AF); // Autolanding en cours, ok
  1641. TFT480.drawString("AAL", 290, 0);
  1642. }
  1643. if (mode_affi_hauteur == ASL)
  1644. {
  1645. TFT480.setFreeFont(FF1);
  1646. TFT480.setTextColor(BLEU_CLAIR, GRIS_AF); // Autolanding en cours, ok
  1647. TFT480.drawString("ASL", 290, 0);
  1648. }
  1649. }
  1650.  
  1651.  
  1652. void calculs_GPS()
  1653. {
  1654. String s1;
  1655. // calculs de la position de l'avion / piste (distance et direction)
  1656.  
  1657. float lat1=latitude_avion;
  1658. float lon1=longitude_avion;
  1659. float lat2=liste_bali[num_bali].latitude; // du centre de la piste
  1660. float lon2=liste_bali[num_bali].longitude; // du centre de la piste
  1661.  
  1662. // DISTANCE (variable globale)
  1663. // voir la fonction "distance_AB()" dans le fichier "Fonctions1.h"
  1664. GPS_distance_piste = distance_AB(lat1, lon1, lat2, lon2) / 1.852; // du centre de la piste, en NM
  1665.  
  1666. // DIRECTION (variable globale)
  1667. GPS_azimut_piste = azimut_AB(lat1, lon1, lat2, lon2); // latitudes et longitudes en degrés décimaux
  1668. }
  1669.  
  1670.  
  1671.  
  1672. float orientation_piste() // détermine dans quel sens on aborde la piste
  1673. {
  1674. float OP = liste_bali[num_bali].orient_piste;
  1675.  
  1676. //détermination du sens de l'approche
  1677. float delta_1 = OP - GPS_azimut_piste;
  1678. if (delta_1<0) {delta_1+=360;}
  1679.  
  1680. if ((delta_1 >90.0) && (delta_1 <270.0)) { sens_piste=1; }
  1681. else { sens_piste=0; }
  1682.  
  1683. if(sens_piste==1) {OP +=180.0;}
  1684. if (OP > 360.0) {OP -= 360.0;}
  1685.  
  1686. return OP;
  1687. }
  1688.  
  1689.  
  1690.  
  1691. void auto_rudder_au_decollage()
  1692. {
  1693. // asservissement (facultatif) de la trajectoire suivant l'axe de la piste
  1694. // se désengage manuellement avec le bouton "ILS", et aussi automatiquement
  1695.  
  1696.  
  1697. float or1 = orientation_piste();
  1698.  
  1699. float delta_1;
  1700.  
  1701. if(flag_eloignement==0) {delta_1 = or1 - cap;} else {delta_1 = cap - or1;}
  1702.  
  1703. if (delta_1 < -3.0) {delta_1 = -3.0;}
  1704. if (delta_1 > 3.0) {delta_1 = 3.0;}
  1705. rudder = delta_1 / 50.0;
  1706.  
  1707. }
  1708.  
  1709.  
  1710.  
  1711.  
  1712. void auto_landing()
  1713. {
  1714. /**
  1715.  voir: https://en.wikipedia.org/wiki/Autoland
  1716.  
  1717.  Atterrissage automatique
  1718.  CAPTURE l'avion et le POSE !
  1719.  
  1720.  -vitesse conseillée : 140kts, 160kts max
  1721.  -distance conseillée : entre 20 et 10 nautiques
  1722.  -avec une trajectoire qui recoupe l'axe de la piste, < 90°
  1723.  (si capture à moins de 10 NM, la trajectoire sera difficilement corrigée)
  1724.  -hauteur conseillée : 3000ft à 10NM (= niveau 30)
  1725.  
  1726.  -volets 1/2 sortis
  1727.  à priori pas d'AF si vitesse correcte
  1728.  
  1729.  -sortir le train !
  1730.  -allumer feux d'attérissage
  1731.  
  1732.  notes: l'autopilot se désengage automatiquement (par FlightGear) sous 500ft de hauteur
  1733.  (réglable, voir la variable 'hauteur_mini_autopilot' au début de ce programme)
  1734.  (Donc garder le manche en main pour l'arrondi et le touché final, en surveillant
  1735.  - la hauteur
  1736.  - la vitesse
  1737.  - le pitch
  1738.  - position des volets
  1739.  - éventuellement petit coup d'AF (aérofreins -> CTRL + B au clavier)
  1740.  - si piste très courte, inverseurs de poussée (au sol) + gaz (touche 'suppr')
  1741.  - toutefois si approche visiblement trop courte ou trop longue, pas d'attéro kamikaze ! -> remise des gaz !!
  1742.  - si système visuel "papi" présent, le respecter !!
  1743.  
  1744. **/
  1745. String s1;
  1746.  
  1747. float alti1;
  1748. float GPS_distance_seuil_piste;
  1749. float oriPiste1;
  1750.  
  1751. TFT480.setFreeFont(FSS9);
  1752.  
  1753. TFT480.setTextColor(GRIS_FONCE, NOIR);
  1754. //TFT480.fillRect(180, 0, 20, 16, JAUNE); // JAUNE pour test. efface 1/2 le bandeau d'information en haut
  1755.  
  1756. voyant_L.affiche(BLANC, GRIS_TRES_FONCE);
  1757.  
  1758. voyant_G.affiche(BLANC, GRIS_TRES_FONCE);
  1759.  
  1760. //voyant_descente_GPS.affiche(BLANC, GRIS_TRES_FONCE);
  1761.  
  1762.  
  1763. //--------------------- (si autoland engagé (par le bouton 'APP ILS' du module SW), sinon on ne fait rien de plus)--------------
  1764.  
  1765. TFT480.fillRect(HA_x0-40, HA_y0+80, 87, 15, HA_SOL); // efface "APP"
  1766.  
  1767. if (autoland != 0)
  1768. {
  1769. if (hauteur_AAL <10){return;}
  1770. if (GPS_distance_piste > 20.0)
  1771. {
  1772. affi_APP( NOIR, JAUNE);
  1773. return; // rien de plus
  1774. }
  1775. else
  1776. {
  1777. //affi_float_test(liste_bali[num_bali].orient_piste,110, 2, BLANC, BLEU); // pour test
  1778. //affi_float_test(GPS_azimut_piste,110, 3, BLANC, GRIS_FONCE); // pour test
  1779.  
  1780. affi_APP( NOIR, VERT);
  1781.  
  1782.  
  1783. // -------------------- AZIMUT -----------------------------------------------------------------
  1784.  
  1785. oriPiste1 = orientation_piste();
  1786.  
  1787. float delta_AZM = 15.0 * ( oriPiste1 - GPS_azimut_piste);
  1788.  
  1789. if (delta_AZM < -45.0) {delta_AZM = -45.0;}
  1790. if (delta_AZM > 45.0) {delta_AZM = 45.0;}
  1791.  
  1792. //affi_float_test(delta_AZM,110, 2, VERT, GRIS_FONCE); // pour test
  1793.  
  1794. if((delta_AZM >-10.0)&&(delta_AZM <10.0))
  1795. {
  1796. voyant_L.affiche(NOIR, VERT);
  1797. }
  1798.  
  1799. affi_localizer(delta_AZM * -2.5);
  1800.  
  1801.  
  1802. hdg1 = (uint16_t)(oriPiste1 - delta_AZM);
  1803.  
  1804.  
  1805.  
  1806. //affi_float_test(hdg1,110, 5, VERT, GRIS_FONCE); // pour test
  1807.  
  1808. // -------------------- VITESSE -----------------------------------------------------------------
  1809.  
  1810. if ( (GPS_distance_piste < 20.0) && (target_speed>160) ) {target_speed =160;}
  1811. if ( (GPS_distance_piste < 10.0) && (target_speed>150) ) {target_speed =150;}
  1812. if ( (GPS_distance_piste < 5.0) && (target_speed>140) ) {target_speed =140;}
  1813.  
  1814. // -------------------- HAUTEUR -----------------------------------------------------------------
  1815.  
  1816. //voyant_descente_GPS.affiche(NOIR, JAUNE);
  1817. uint16_t longueur_piste_m = liste_bali[num_bali].longueur; // en m
  1818. float longueur_piste_NM = longueur_piste_m / 1852.0;
  1819. GPS_distance_seuil_piste = GPS_distance_piste - (longueur_piste_NM/2.0) + 0.1;
  1820. // todo : ajouter la longueur des pistes dans le fichier FG_data.h afin de'affiner ce paramètre
  1821. // Rappel : "GPS_distance_piste" est la distance au point CENTRAL de la piste
  1822.  
  1823. TFT480.setFreeFont(FM12);
  1824. TFT480.setTextColor(BLANC);
  1825. String s1=String(GPS_distance_seuil_piste, 1);
  1826. s1+= " NM";
  1827. TFT480.fillRect(150, 300, 100, 25, GRIS_AF); // efface
  1828. TFT480.drawString(s1, 150, 300);
  1829.  
  1830.  
  1831. //affi_float_test(GPS_distance_seuil_piste, 110, 2, BLANC, BLEU); // pour test
  1832.  
  1833. float alti_correcte = liste_bali[num_bali].altitude + 300.0 * GPS_distance_seuil_piste;
  1834.  
  1835. affi_asel(alti_correcte);
  1836.  
  1837. // soit 3000ft pour 10 nautiques -> pente 5%
  1838. //sachant que la ref de position est située au milieu de la longueur de la piste
  1839.  
  1840. //affi_float_test(alti_correcte, 110, 2, BLANC, BLEU); // pour test
  1841.  
  1842. float erreur_altitude = altitude_GPS - alti_correcte;
  1843. //affi_float_test(erreur_altitude, 110, 3, BLANC, BLEU); // pour test
  1844.  
  1845. if((erreur_altitude > -20)&& (erreur_altitude < 20))
  1846. {
  1847. //voyant_G.affiche(NOIR, VERT);
  1848. }
  1849.  
  1850. affi_index_lateral( - erreur_altitude / 3.0); // affiche les triangles roses latéraux
  1851.  
  1852. /**Rappels :
  1853. 1 NM (nautical mile ou 'nautique') = 1852 m
  1854. 1 feet = 0,3048 m
  1855. 1 NM = 1852/0.3048 = 6076.12 feet
  1856. 1 noeud (nd) = 1NM/h = 1852/3600 = 0.51444 m/s
  1857.  
  1858. début de descente (5%) vers la piste : 3000ft à 10NM, vitesse 150 nd (par exemple)
  1859. v=150*0.51444 = 77.17m/s
  1860. temps pour parcourir la distance : v=d/t
  1861. t=d/v = 10*1852m / 77.17 = 240 s (soit 4 minutes)
  1862.  
  1863. taux de descente = 3000ft/240s = 12.5 fps
  1864. **/
  1865. if ((GPS_distance_piste < 15) && (hauteur_AAL > 1500) && (hauteur_AAL <= 6000))
  1866. {
  1867. // initialisation de l'approche auto (palier puis descente)
  1868. //voyant_descente_GPS.affiche(NOIR, VERT);
  1869.  
  1870. locks_type = "VS"; // bascule le pilote auto de FG en mode vertical speed
  1871. //climb_rate = -5.0;
  1872.  
  1873. }
  1874.  
  1875. if ((GPS_distance_piste < 15) && (hauteur_AAL < 6000))
  1876. {
  1877. // descente
  1878. // correction du taux de descente (climb_rate) pour respecter la pente à 3° (=5%)
  1879. voyant_G.affiche(NOIR, VERT);
  1880.  
  1881. //if (erreur_altitude > 4) {climb_rate -= 5; }
  1882. //if (erreur_altitude < -4) {climb_rate += 5; }
  1883.  
  1884. climb_rate = erreur_altitude * -1.0;
  1885. if (climb_rate > +20) {climb_rate = +20;}
  1886. if (climb_rate < -40) {climb_rate = -40;}
  1887.  
  1888. //affi_float_test( erreur_altitude, 110, 4, NOIR, JAUNE); // pour test
  1889. }
  1890.  
  1891. if ((GPS_distance_seuil_piste < 1.0) && (hauteur_AAL < 10))
  1892. {
  1893. asel1=0; // plaque au sol... à voir...
  1894. desengage_autoland();
  1895.  
  1896. }
  1897.  
  1898. /**
  1899. alti1 = 3.0*GPS_distance_piste + gnd_elv/100.0 -1;
  1900. if (alti1 < asel1) //empêche de (re)monter lors de la capture ILS, reste en palier le cas échéant
  1901. {
  1902. asel1 = alti1;
  1903. }
  1904. **/
  1905. }
  1906.  
  1907. // =============================================================================================================================
  1908. }
  1909.  
  1910. //------------------------------------------------------------------------------------------------------------------------------
  1911.  
  1912. }
  1913.  
  1914.  
  1915.  
  1916. void affi_localizer(float valeur)
  1917. {
  1918. //ILS dans le plan horizontal ; affiche l'erreur de position par rapport à l'axe de la piste
  1919.  
  1920. uint16_t y1 = HA_y0-HA_h-14;
  1921.  
  1922. // ils_loc = -10 *1000 .. +10 *1000
  1923. // l'affichage doit se faire en HA_x0 +/- HA_w
  1924. // 10000/HA_w = 10000/120 = 83
  1925.  
  1926. uint16_t couleur1 = ROSE;
  1927.  
  1928. loc = HA_x0 + valeur;
  1929.  
  1930. if ( loc < (HA_x0-HA_w+5)) {loc = HA_x0-HA_w+5; couleur1 = GRIS;}
  1931. if ( loc > (HA_x0+HA_w-5)) {loc= HA_x0+HA_w-5; couleur1 = GRIS;}
  1932.  
  1933.  
  1934. TFT480.fillRect(HA_x0-HA_w, y1, 2*HA_w, 9, GRIS_TRES_FONCE);
  1935. TFT480.drawLine(HA_x0, y1-5, HA_x0, y1+5, BLANC);
  1936.  
  1937. affi_indexV(loc, y1, 1, couleur1); // petit triangle rose en haut, se déplaçant horizontalement
  1938.  
  1939. memo_loc=loc;
  1940. }
  1941.  
  1942.  
  1943.  
  1944. void affi_index_lateral(uint16_t position_i)
  1945. {
  1946. // petits triangles roses de chaque côtés du PFD
  1947. // (à mi-hauteur du PFD si =0)
  1948.  
  1949. uint16_t x1 = 75;
  1950. uint16_t x2 = 332;
  1951.  
  1952. uint16_t position_V = HA_y0 - position_i;
  1953.  
  1954. TFT480.fillRect(x1, 30, 9, 2*HA_h, GRIS_TRES_FONCE); // efface
  1955. TFT480.fillRect(x2, 30, 9, 2*HA_h, GRIS_TRES_FONCE); // efface
  1956.  
  1957. TFT480.drawRect(x1, HA_y0, 12, 5, BLANC);
  1958. TFT480.drawRect(x2, HA_y0, 12, 5, BLANC);
  1959.  
  1960. uint16_t couleur1 = ROSE;
  1961. if ( position_V < (HA_y0-HA_h+5)) {position_V = HA_y0-HA_h+5; couleur1 = GRIS;}
  1962. if ( position_V > (HA_y0+HA_h-5)) {position_V = HA_y0+HA_h-5; couleur1 = GRIS;}
  1963.  
  1964. affi_indexH(x1, position_V, 1, couleur1);
  1965. affi_indexH(x2+8, position_V, -1, couleur1);
  1966. }
  1967.  
  1968.  
  1969.  
  1970. void trace_arc_gradu()
  1971. {
  1972. //arc gradué en haut au centre, indiquant la valeur de l'inclinaison
  1973.  
  1974. float angle;
  1975. //Draw_arc_elliptique(HA_x0, 120, 120, 80, 0.6, 2.6, BLANC);
  1976.  
  1977.  
  1978. for(uint8_t n=0; n<9; n++ )
  1979. {
  1980. angle =30+ n*15; // 1 tiret tous les 15 degrés
  1981. float pourcent = 0.9;
  1982. if (((n+2)%2) == 0) {pourcent = 0.8;}
  1983.  
  1984. affi_rayon1(HA_x0, HA_y0+10, 110, degTOrad(angle), pourcent, BLANC, false); // tirets de graduation
  1985. }
  1986. }
  1987.  
  1988.  
  1989.  
  1990. void rotation1()
  1991. {
  1992. // consigne de cap
  1993. // acquisition de l'encodeur pas à pas (1)
  1994. if ( millis() - TEMPO >= timer1 )
  1995. {
  1996. timer1 = millis();
  1997. bool etat = digitalRead(rot1b);
  1998. if(etat == 0) { hdg1+=1;} else { hdg1-=1;}
  1999. if (hdg1<0){hdg1=359;}
  2000.  
  2001. if (hdg1>359){hdg1=0;}
  2002. }
  2003. }
  2004.  
  2005.  
  2006.  
  2007. void rotation2()
  2008. {
  2009. // consigne d'altitude
  2010. // acquisition de l'encodeur pas à pas (2)
  2011. if ( millis() - TEMPO >= timer2 )
  2012. {
  2013. timer2 = millis();
  2014. bool etat = digitalRead(rot2b);
  2015. if(etat == 0) { asel1+=1; } else { asel1-=1; }
  2016. if (asel1<1){asel1=1;} // 100 pieds -> 30m
  2017. if (asel1>600){asel1=600;}
  2018. }
  2019. }
  2020.  
  2021.  
  2022.  
  2023. void init_SDcard()
  2024. {
  2025. String s1;
  2026.  
  2027. TFT480.fillRect(0, 0, 480, 320, NOIR); // efface
  2028. TFT480.setTextColor(BLANC, NOIR);
  2029. TFT480.setFreeFont(FF1);
  2030.  
  2031. uint16_t y=0;
  2032.  
  2033. TFT480.drawString("PRIMARY FLIGHT DISPLAY", 0, y);
  2034. y+=20;
  2035.  
  2036. s1="version " + version;
  2037. TFT480.drawString(s1, 0, y);
  2038.  
  2039. y+=40;
  2040. TFT480.setTextColor(VERT, NOIR);
  2041. TFT480.drawString("Init SDcard", 0, y);
  2042. y+=20;
  2043.  
  2044.  
  2045. if(!SD.begin())
  2046. {
  2047. TFT480.drawString("Card Mount Failed", 0, y);
  2048. delay (2000);
  2049. TFT480.fillRect(0, 0, 480, 320, NOIR); // efface
  2050. return;
  2051. }
  2052.  
  2053.  
  2054. uint8_t cardType = SD.cardType();
  2055.  
  2056. if(cardType == CARD_NONE)
  2057. {
  2058. TFT480.drawString("No SDcard", 0, y);
  2059. delay (2000);
  2060. TFT480.fillRect(0, 0, 480, 320, NOIR); // efface
  2061. return;
  2062. }
  2063.  
  2064. flag_SDcardOk=1;
  2065.  
  2066. TFT480.drawString("SDcard Type: ", 0, y);
  2067. if(cardType == CARD_SD) {TFT480.drawString("SDSC", 150, y);}
  2068. else if(cardType == CARD_SDHC) {TFT480.drawString("SDHC", 150, y);}
  2069.  
  2070. y+=20;
  2071.  
  2072. uint32_t cardSize = SD.cardSize() / (1024 * 1024);
  2073. s1=(String)cardSize + " GB";
  2074. TFT480.drawString("SDcard size: ", 0, y);
  2075. TFT480.drawString(s1, 150, y);
  2076.  
  2077. // listDir(SD, "/", 0);
  2078.  
  2079. //Serial.printf("Total space: %lluMB\n", SD.totalBytes() / (1024 * 1024));
  2080. //Serial.printf("Used space: %lluMB\n", SD.usedBytes() / (1024 * 1024));
  2081.  
  2082. delay (1000);
  2083. TFT480.fillRect(0, 0, 480, 320, NOIR); // efface
  2084.  
  2085. }
  2086.  
  2087.  
  2088. void init_sprites()
  2089. {
  2090. // sprites représentant les lettres 'N' 'S' 'E' 'O' qui seront affichées sur un cercle, inclinées donc.
  2091.  
  2092. SPR_E.setFreeFont(FF1);
  2093. SPR_E.setTextColor(JAUNE);
  2094. SPR_E.createSprite(SPR_W, SPR_H);
  2095. SPR_E.setPivot(SPR_W/2, SPR_H/2); // Set pivot relative to top left corner of Sprite
  2096. SPR_E.fillSprite(GRIS_TRES_FONCE);
  2097. SPR_E.drawString("E", 2, 1 );
  2098.  
  2099. SPR_N.setFreeFont(FF1);
  2100. SPR_N.setTextColor(JAUNE);
  2101. SPR_N.createSprite(SPR_W, SPR_H);
  2102. SPR_N.setPivot(SPR_W/2, SPR_H/2);
  2103. SPR_N.fillSprite(GRIS_TRES_FONCE);
  2104. SPR_N.drawString("N", 2, 1 );
  2105.  
  2106. SPR_O.setFreeFont(FF1);
  2107. SPR_O.setTextColor(JAUNE);
  2108. SPR_O.createSprite(SPR_W, SPR_H);
  2109. SPR_O.setPivot(SPR_W/2, SPR_H/2);
  2110. SPR_O.fillSprite(GRIS_TRES_FONCE);
  2111. SPR_O.drawString("W", 2, 1 );
  2112.  
  2113. SPR_S.setFreeFont(FF1);
  2114. SPR_S.setTextColor(JAUNE);
  2115. SPR_S.createSprite(SPR_W, SPR_H);
  2116. SPR_S.setPivot(SPR_W/2, SPR_H/2);
  2117. SPR_S.fillSprite(GRIS_TRES_FONCE);
  2118. SPR_S.drawString("S", 2, 1 );
  2119.  
  2120. SPR_trajectoire.setFreeFont(FF1);
  2121. SPR_trajectoire.setTextColor(JAUNE);
  2122. SPR_trajectoire.createSprite(292, 88);
  2123. SPR_trajectoire.fillSprite(TFT_BLACK);
  2124. SPR_trajectoire.drawString("DESCENTE 5%", 170, 1 );
  2125. }
  2126.  
  2127.  
  2128. void int16_to_array(int16_t valeur_i)
  2129. {
  2130. // prépare la chaine de caract à zéro terminal pour l'envoi
  2131.  
  2132. String s1= (String) valeur_i;
  2133. for(int n=0; n<4; n++)
  2134. {
  2135. var_array4[n]=s1[n];
  2136. }
  2137. var_array4[4]=0; // zéro terminal -> chaine C
  2138. }
  2139.  
  2140.  
  2141. void int32_to_array(int32_t valeur_i)
  2142. {
  2143. // prépare la chaine de caract à zéro terminal pour l'envoi
  2144. String s1= (String) valeur_i;
  2145. for(int n=0; n<10; n++)
  2146. {
  2147. var_array10[n]=s1[n];
  2148. }
  2149. var_array10[10]=0; // zéro terminal -> chaine C
  2150. }
  2151.  
  2152.  
  2153.  
  2154.  
  2155. void setup()
  2156. {
  2157. Serial.begin(38400); // 19200
  2158.  
  2159. locks_type ="ALT";
  2160. WiFi.persistent(false);
  2161. WiFi.softAP(ssid, password); // Crée un réseau WiFi en mode privé (indépendant de celui de la box internet...)
  2162. IPAddress IP = WiFi.softAPIP();
  2163.  
  2164.  
  2165. server.on("/switch", HTTP_GET, [](AsyncWebServerRequest *request) // lecture des boutons de l'ESP 3 (module SW)
  2166. {
  2167. // attention: ce code est appelé par une interruption WiFi qui intervient hors timing. Donc pas d'affichage ici !!
  2168.  
  2169. argument_recu1 = request->arg("sw1"); // réception de l'argument n°1 de la requête
  2170. switches=argument_recu1; // réception des boutons du module SW
  2171. v_switches=switches.toInt();
  2172. flag_traiter_SW=1; //positionne ce drapeau afin que le traitement se fasse dans le timming général, pas ici !
  2173.  
  2174. int16_to_array(0);
  2175.  
  2176. argument_recu2 = request->arg("pot1"); // réception de l'argument n°2 de la requête
  2177. potar1=argument_recu2; // = "0".."255"
  2178. v_potar1 = -128 + potar1.toInt();
  2179.  
  2180.  
  2181.  
  2182. float valeur1 = v_potar1 * v_potar1; // au carré pour avoir une bonne précision aux faibles débattements,
  2183. //et une bonne réponse à fond (taxi au sol)
  2184. if (v_potar1<0) {valeur1 = -valeur1;} // because un carré est toujours positif
  2185.  
  2186. if (flag_auto_rudder == 0)
  2187. {
  2188. rudder = valeur1 / 20000.0; // 10000.0 détermine la sensibilité de la gouverne de direction
  2189. }
  2190. //cet array because la fonction "request->send_P()" n'accèpte pas directement le string
  2191. //rappel :
  2192. //void send_P(int code, const String& contentType, const uint8_t * content, size_t len, AwsTemplateProcessor callback=nullptr);
  2193. //void send_P(int code, const String& contentType, PGM_P content, AwsTemplateProcessor callback=nullptr);
  2194.  
  2195. request->send_P(200, "text/plain", var_array4); // envoie comme réponse au client
  2196. });
  2197.  
  2198.  
  2199. server.on("/hdg", HTTP_GET, [](AsyncWebServerRequest *request) // consigne de cap
  2200. {
  2201. // attention: ce code est appelé par une interruption WiFi qui intervient hors timing. Donc pas d'affichage ici !!
  2202.  
  2203. argument_recu1 = request->arg("a1"); // reception de l'argument n°1 de la requête
  2204. num_bali=argument_recu1.toInt();
  2205.  
  2206. argument_recu2 = request->arg("swND");
  2207. flag_traiter_SW=1; //positionne ce drapeau afin que le traitement se fasse dans le timming général, pas ici !
  2208.  
  2209. v_switches_ND = argument_recu2.toInt();
  2210. switches_ND = String(v_switches_ND);
  2211.  
  2212.  
  2213. int16_to_array(hdg1);
  2214.  
  2215. //cet array because la fonction "request->send_P()" n'accèpte pas directement le string
  2216. //rappel :
  2217. //void send_P(int code, const String& contentType, const uint8_t * content, size_t len, AwsTemplateProcessor callback=nullptr);
  2218. //void send_P(int code, const String& contentType, PGM_P content, AwsTemplateProcessor callback=nullptr);
  2219.  
  2220. request->send_P(200, "text/plain", var_array4); // envoie hdg1 comme réponse au client
  2221. });
  2222.  
  2223. // réponses aux requêtes :
  2224. // VOIR la fonction "void interroge_WiFi()" dans le code du ND (l'affichage de la carte...)
  2225. // pour la réception des données qui suivent
  2226. server.on("/cap", HTTP_GET, [](AsyncWebServerRequest *request)
  2227. {
  2228. int16_to_array(cap); // prépare la chaine de caract à zéro terminal pour l'envoi
  2229. request->send_P(200, "text/plain", var_array4); // envoie réponse au client
  2230. });
  2231.  
  2232.  
  2233. server.on("/latitude", HTTP_GET, [](AsyncWebServerRequest *request) // latitude de l'avion
  2234. {
  2235. int32_t lati1 = (int32_t) (latitude_avion * 10000.0);
  2236.  
  2237. int32_to_array(lati1);
  2238. request->send_P(200, "text/plain", var_array10); // envoie réponse au client
  2239. });
  2240.  
  2241.  
  2242. server.on("/longitude", HTTP_GET, [](AsyncWebServerRequest *request) // longitude de l'avion
  2243. {
  2244. int32_t longi1 = (int32_t) (longitude_avion * 10000.0);
  2245.  
  2246. int32_to_array(longi1);
  2247. request->send_P(200, "text/plain", var_array10); // envoie réponse au client
  2248. });
  2249.  
  2250.  
  2251. server.on("/hauteur", HTTP_GET, [](AsyncWebServerRequest *request) // hauteur de l'avion / sol
  2252. {
  2253. int32_t haut1 = (int32_t) (hauteur_AAL * 10.0);
  2254.  
  2255. int32_to_array(haut1);
  2256. request->send_P(200, "text/plain", var_array10); // envoie réponse au client
  2257. });
  2258.  
  2259.  
  2260. server.on("/params", HTTP_GET, [](AsyncWebServerRequest *request) // parametres divers PFD -> ND
  2261. {
  2262. int32_to_array(params);
  2263. request->send_P(200, "text/plain", var_array10); // envoie réponse au client
  2264. });
  2265.  
  2266. server.begin();
  2267.  
  2268. /*
  2269. // ************* BLUETOOTH *************
  2270. SerialBT.begin("PFD_BT01"); //Bluetooth device name
  2271. //Serial.println("The device started, now you can pair it with bluetooth!");
  2272. // *************************************
  2273. */
  2274. pinMode(bouton1, INPUT);
  2275. pinMode(bouton2, INPUT);
  2276.  
  2277. //pinMode(led1, OUTPUT);
  2278.  
  2279. pinMode(rot1a, INPUT_PULLUP);
  2280. pinMode(rot1b, INPUT_PULLUP);
  2281. pinMode(rot2a, INPUT_PULLUP);
  2282. pinMode(rot2b, INPUT_PULLUP);
  2283.  
  2284.  
  2285. attachInterrupt(rot1a, rotation1, RISING);
  2286. attachInterrupt(rot2a, rotation2, RISING);
  2287.  
  2288. TFT480.init();
  2289. TFT480.setRotation(3); // 0..3 à voir, suivant disposition de l'afficheur et sa disposition
  2290. TFT480.fillScreen(TFT_BLACK);
  2291.  
  2292. init_SDcard();
  2293.  
  2294. init_sprites();
  2295.  
  2296. delay(100);
  2297.  
  2298.  
  2299.  
  2300. TFT480.setTextColor(NOIR, BLANC);
  2301.  
  2302. //TFT480.setFreeFont(FF19);
  2303.  
  2304. init_affi_HA();
  2305.  
  2306. delay(100);
  2307.  
  2308. //TFT480.fillRect(48, 0, 6, 100, 0xFFE0);
  2309.  
  2310. // TFT480.fillRect(0, 0, 479, 30, NOIR);
  2311. TFT480.setTextColor(BLANC, NOIR);
  2312. TFT480.setFreeFont(FF19);
  2313. String s1 = "PFD v";
  2314. s1+= version;
  2315. //TFT480.drawString(s1, 70, 3);
  2316.  
  2317. Ay_actu=120;
  2318. By_actu=120;
  2319.  
  2320. altitude_GPS =0;
  2321. vitesse =0;
  2322. roulis =0;
  2323. tangage =0;
  2324. cap=0;
  2325. vspeed=0; // vitesse verticale
  2326.  
  2327. //vor_frq=123500;
  2328.  
  2329. //vor_dst=1852*102; // 102km
  2330. //vor_actual_deg=45;
  2331. //vor_actual=45.0 * 100.0;
  2332. // affichages();
  2333.  
  2334. bouton1_etat = digitalRead(bouton1);
  2335. memo_bouton1_etat = bouton1_etat;
  2336.  
  2337. bouton2_etat = digitalRead(bouton2);
  2338. memo_bouton2_etat = bouton2_etat;
  2339. if (bouton2_etat==0)
  2340. {
  2341. mode_affi_hauteur = AAL;
  2342. affi_mode_affi_hauteur();
  2343. }
  2344. if (bouton2_etat==1)
  2345. {
  2346. mode_affi_hauteur = ASL;
  2347. affi_mode_affi_hauteur();
  2348. }
  2349.  
  2350. init_FG_bali();
  2351.  
  2352. //init_affi_autopilot();
  2353. //affi_indicateurs();
  2354.  
  2355. affi_dst_GPS();
  2356.  
  2357. affi_Airport();
  2358.  
  2359. voyant_L.init(0,0,30,20);
  2360. voyant_L.caract ='L';
  2361.  
  2362.  
  2363. voyant_G.init(35,0,30,20);
  2364. voyant_G.caract ='G';
  2365.  
  2366. //voyant_descente_GPS.init(70,0,30,20);
  2367. //voyant_descente_GPS.caract ='v';
  2368.  
  2369. }
  2370.  
  2371.  
  2372.  
  2373. uint8_t p1;
  2374.  
  2375. int32_t number = 0;
  2376.  
  2377.  
  2378. String s1;
  2379. String s2;
  2380.  
  2381.  
  2382. void acquisitions()
  2383. {
  2384. // cette fonction reçoit les données de Flightgear par la liaison USB, voir le fichier "hardware4.xml" pour le protocole
  2385.  
  2386. // Remarque : les données des autres modules (ND et SW) sont reçues par WiFi,
  2387. // voir les sous-fonctions ("server.on(...)") dans la fonction setup()
  2388.  
  2389. char buf[50];
  2390. int32_t valeur;
  2391. TFT480.setFreeFont(FM9);
  2392. TFT480.setTextColor(VERT, NOIR);
  2393.  
  2394. if(Serial.available() > 14) // 14
  2395. {
  2396. parametre="";
  2397. s1="";
  2398. char octet_in;
  2399.  
  2400. while(octet_in != '=')
  2401. {
  2402. octet_in = Serial.read();
  2403. if(octet_in != '=') {parametre += octet_in; }
  2404. }
  2405. while(octet_in != '\n')
  2406. {
  2407. octet_in = Serial.read();
  2408. if(octet_in != '\n') {s1 += octet_in; }
  2409. }
  2410.  
  2411. if(parametre == "alti" )
  2412. {
  2413. /*
  2414.  ALTITUDE GPS (et pas 'altimetre', volontairement, voir le fichier hardware4.xml)
  2415.  voir la ligne '<node>/instrumentation/gps/indicated-altitude-ft</node>'
  2416.  marre des QFE, QNH, AGL, ASFC, AMSL, STD...
  2417.  j'ai joué avec tout ça, mais finalement je décide d'utiliser le GPS,
  2418.  ce qui est contraire aux recommandations aéronautiques,
  2419.  sans doute parce que le jour où le GPS mondial viendrait à tomber en panne, c'est 275643 avions qui iraient se poser
  2420.  dans le Triangle des Bermudes.
  2421.  mais ici on est dans FlightGear, on ne risque rien !
  2422.  Et puis on a Galiléo et le système EGNOS développé à l'origine par le cnes puis sur la planète B612 à Toulouse (ESSP)...
  2423. */
  2424. s1.toCharArray(buf, 50);
  2425.  
  2426. valeur = atol(buf);
  2427. altitude_GPS_float = (float)valeur / 1000.0;
  2428. altitude_GPS = valeur/1000; // integer
  2429. hauteur_AAL = altitude_GPS - liste_bali[num_bali].altitude;
  2430.  
  2431. data_ok |= 1; // positionne bit0
  2432. }
  2433.  
  2434. if(parametre == "gnd_elv" )
  2435. {
  2436. s1.toCharArray(buf, 50);
  2437. gnd_elv = atol(buf);
  2438. if (gnd_elv <0) {gnd_elv =0;}
  2439. data_ok |= 1<<1; // positionne bit1
  2440. }
  2441.  
  2442. if(parametre == "speed" )
  2443. {
  2444. s1.toCharArray(buf, 50);
  2445. vitesse = atol(buf);
  2446. data_ok |= 1<<2; // positionne bit2
  2447. }
  2448.  
  2449. if(parametre == "pitch" )
  2450. {
  2451. //char buf[50];
  2452. s1.toCharArray(buf, 50);
  2453. tangage = atol(buf);
  2454. data_ok |= 1<<3; // positionne bit3
  2455. }
  2456.  
  2457. if(parametre == "roll" )
  2458. {
  2459. s1.toCharArray(buf, 50);
  2460. roulis = atol(buf);
  2461. data_ok |= 1<<4; // positionne bit4
  2462. }
  2463.  
  2464.  
  2465.  
  2466. if(parametre == "heading" ) // /orientation/heading-deg = cap actuel de l'avion ; ne pas confondre avec HDG bug !
  2467. {
  2468. s1.toCharArray(buf, 50);
  2469. valeur = atol(buf);
  2470. cap= (float) valeur / 100.0;
  2471. data_ok |= 1<<5; // positionne bit5
  2472. }
  2473.  
  2474.  
  2475. if(parametre == "vspeed" )
  2476. {
  2477. s1.toCharArray(buf, 50);
  2478. vspeed = atol(buf);
  2479. data_ok |= 1<<6; // positionne bit6
  2480. }
  2481.  
  2482.  
  2483. if(parametre == "latitude" )
  2484. {
  2485. s1.toCharArray(buf, 50);
  2486. valeur = atol(buf);
  2487. latitude_avion = (float)valeur / 100000.0;
  2488. data_ok |= 1<<7; // positionne bit16 (qui est le 17eme bit, mais pas de panique, data_ok est un uint_32)
  2489. }
  2490.  
  2491.  
  2492. if(parametre == "longitude" )
  2493. {
  2494. s1.toCharArray(buf, 50);
  2495. valeur = atol(buf);
  2496. longitude_avion = (float)valeur / 100000.0;
  2497. data_ok |= 1<<8;
  2498. }
  2499.  
  2500. nb_acqui=9; // erreur non permise !!!
  2501.  
  2502.  
  2503. }
  2504.  
  2505. delay(3); // 3 important sinon ne recevra pas la totalité des données (qui se fait en plusieurs passes)
  2506.  
  2507.  
  2508. ////// pour test
  2509. ////TFT480.drawString("data= ", 90, 50);
  2510. ////s2= (String) data_ok;
  2511. ////TFT480.fillRect(140,50, 50, 15, TFT_BLACK);
  2512. ////TFT480.drawString(s2, 150, 50);
  2513.  
  2514. }
  2515.  
  2516.  
  2517.  
  2518. void data_out()
  2519. {
  2520. // destination FlightGear par la liaison série USB ; voir le fichier "hardware4.xml" pour le protocole
  2521.  
  2522. Serial.print(hdg1); // consigne de Cap -> autopilot
  2523. Serial.print(',');
  2524.  
  2525. Serial.print(asel1); // consigne d'altitude -> autopilot
  2526. Serial.print(',');
  2527.  
  2528.  
  2529. uint16_t v3 = landing_light1;
  2530. Serial.print(v3);
  2531. Serial.print(',');
  2532.  
  2533. uint16_t v4 = landing_light2;
  2534. Serial.print(v4);
  2535. Serial.print(',');
  2536.  
  2537. uint16_t v5 = target_speed;
  2538. Serial.print(v5); // écrit la consigne de vitesse (target_speed)
  2539. Serial.print(',');
  2540.  
  2541. uint16_t v6 = hauteur_mini_autopilot;
  2542. Serial.print(v6); // écrit la hauteur minimum d'engagement du pilote auto de Flightgear (par défaut = 300)
  2543. Serial.print(',');
  2544.  
  2545. float v7 = rudder; // position de la gouverne de direction (lacet)
  2546. Serial.print(v7);
  2547. Serial.print(',');
  2548.  
  2549. uint8_t v8 = view_number;
  2550. Serial.print(v8);
  2551. Serial.print(',');
  2552.  
  2553. float v9 = climb_rate; // vertical speed en fps (feet per second)
  2554. Serial.print(v9);
  2555. Serial.print(',');
  2556.  
  2557. Serial.print(locks_type); // "ALT" ou "VS"
  2558. Serial.print(',');
  2559.  
  2560. float v10 = elv_trim;
  2561. Serial.print(v10);
  2562. Serial.print('\n');
  2563.  
  2564. }
  2565.  
  2566.  
  2567.  
  2568. void affi_nop()
  2569. {
  2570. for(int8_t dy=-2; dy<3; dy++)
  2571. {
  2572. TFT480.drawLine(HA_x0-HA_w, HA_y0-HA_h +dy, HA_x0 +HA_w, HA_y0 +HA_h +dy, ROUGE);
  2573. TFT480.drawLine(HA_x0-HA_w, HA_y0+HA_h +dy, HA_x0 +HA_w, HA_y0 -HA_h +dy, ROUGE);
  2574. }
  2575.  
  2576. //TFT480.fillRect(0, 0, 239, 30, NOIR);
  2577. TFT480.setTextColor(BLANC, ROUGE);
  2578. TFT480.setFreeFont(FF18);
  2579. TFT480.drawString("No Data", HA_x0-40, HA_y0+30);
  2580.  
  2581. }
  2582.  
  2583.  
  2584.  
  2585.  
  2586. void init_affi_autopilot()
  2587. {
  2588.  
  2589. TFT480.setFreeFont(FF1);
  2590. TFT480.setTextColor(JAUNE, GRIS_AF);
  2591.  
  2592. // ALT
  2593.  
  2594. //TFT480.drawString("ALT", x_autopilot, 260, 1);
  2595. //TFT480.drawRoundRect(x_autopilot-4, 255, 45, 42, 5, BLANC);
  2596. }
  2597.  
  2598.  
  2599.  
  2600. void affi_autopilot(uint8_t complet)
  2601. {
  2602. // dans le petit cercle en bas à gauche :
  2603. // affiche HDG (flèche jaune), piste (rectangle bleu), VOR (Nav1, ligne verte)
  2604.  
  2605. uint16_t x0=70; // 70
  2606. uint16_t y0=248; // 255
  2607.  
  2608. TFT480.setFreeFont(FF1);
  2609.  
  2610. TFT480.fillRect(x0, y0+2, 70, 80, NOIR); // efface
  2611.  
  2612. TFT480.drawCircle(x0+35, y0+34, 30, BLANC);
  2613.  
  2614.  
  2615. TFT480.setTextColor(BLANC, NOIR);
  2616. TFT480.drawString("N", x0+30, y0-5);
  2617.  
  2618. TFT480.setTextColor(BLANC, NOIR);
  2619. TFT480.drawString("S", x0+30, y0+60);
  2620.  
  2621. TFT480.setTextColor(BLANC, NOIR);
  2622. TFT480.drawString("E", x0+60, y0+27);
  2623. TFT480.drawString("W", x0, y0+27);
  2624.  
  2625. //uint16_t x1,y1;
  2626.  
  2627.  
  2628. // rectangle bleu, très fin -> orientation ('radial') de l'axe de la piste
  2629. float angle2 = liste_bali[num_bali].orient_piste;
  2630. affi_rectangle_incline(x0+35, y0+34, 35, 90 + angle2, BLEU_CLAIR);
  2631.  
  2632. if (complet ==1)
  2633. {
  2634. TFT480.setTextColor(JAUNE, NOIR);
  2635. TFT480.drawString("HDG", x0, y0-18);
  2636.  
  2637. String s1 =(String)hdg1;
  2638. TFT480.setTextColor(BLANC, NOIR);
  2639. TFT480.drawString(s1, x0+18, y0+35);
  2640.  
  2641. // flèche jaune = règlage HDG de l'autopilot
  2642. float angle1 = 90-hdg1;
  2643. affi_rayon2(x0+35, y0+34, 0, 30, angle1, JAUNE, 0); // tige de la flèche
  2644. affi_pointe(x0+35, y0+34, 30, angle1, 0.1, JAUNE); // pointe triangulaire en bout de flèche
  2645.  
  2646.  
  2647. }
  2648. else
  2649. {
  2650.  
  2651. if(sens_piste == 0)
  2652. {
  2653. affi_pointe(x0+35, y0+34, 20, 90.0 - angle2, 0.5, JAUNE);
  2654. }
  2655. else
  2656. {
  2657. affi_pointe(x0+35, y0+34, 20, 270.0 - angle2, 0.5, JAUNE);
  2658. }
  2659. }
  2660.  
  2661. }
  2662.  
  2663.  
  2664. void efface_cadre_bas(uint16_t couleur)
  2665. {
  2666. TFT480.fillRect(70, 232, 292, 88, couleur);
  2667. TFT480.drawRect(70, 232, 292, 88, GRIS_FONCE);
  2668. }
  2669.  
  2670.  
  2671.  
  2672.  
  2673.  
  2674. void affi_approche() // grand rectangle en bas avec le tracé de la pente de descente
  2675. {
  2676.  
  2677. uint16_t x1=90;
  2678. uint16_t x2=140;
  2679. uint16_t x3=310;
  2680. uint16_t x4=350;
  2681.  
  2682. uint16_t y1=240;
  2683. uint16_t y2=310;
  2684.  
  2685. uint16_t x_avion;
  2686. uint16_t y_avion;
  2687.  
  2688. float xF;
  2689. float yF;
  2690. float pente;
  2691.  
  2692. xF = (float)x3 - GPS_distance_piste * (float)(x3-x2) / 10.0;
  2693. yF = (float)y2 - (altitude_GPS - liste_bali[num_bali].altitude) * (float)(y2-y1) / 3000.0;
  2694. x_avion = (uint16_t) xF;
  2695.  
  2696. pente = (float)(y2-y1)/(float)(x3-x2);
  2697.  
  2698. TFT480.drawLine(x1, y1, x2, y1, BLANC); // trace ligne blanche (trajectoire)
  2699. TFT480.drawLine(x2, y1, x3, y2, BLANC);
  2700. TFT480.drawLine(x3, y2, x4, y2, BLANC);
  2701. affi_autopilot(0);
  2702.  
  2703. //dessine l'avion sur la ligne, avec une trainée jaune comme trace
  2704.  
  2705.  
  2706. if (memo_y_avion >235) // pour ne pas effacer hors cadre
  2707. {
  2708. TFT480.fillCircle(memo_x_avion, memo_y_avion, 5, GRIS_TRES_FONCE); // efface
  2709. SPR_trajectoire.pushSprite(70, 232, TFT_BLACK); // TFT_BLACK -> défini la couleur de transparence
  2710.  
  2711. //TFT480.drawCircle(memo_x_avion, memo_y_avion, 10, JAUNE); // efface
  2712.  
  2713. //TFT480.drawPixel( memo_x_avion, memo_y_avion, JAUNE); // trace la trajectoire réelle en jaune
  2714.  
  2715. }
  2716.  
  2717. if((x_avion > x1)&&(x_avion<=x2))
  2718. {
  2719. y_avion = y1;
  2720. TFT480.drawLine(x1, y1, x2, y1, BLANC); // trace ligne blanche (trajectoire théorique)
  2721. //TFT480.fillRect(x_avion+1, y_avion, 8, 4, BLEU); // dessine avion
  2722. if (y_avion >235)
  2723. {
  2724. TFT480.fillCircle(x_avion, y_avion, 5, BLEU);
  2725. SPR_trajectoire.drawPixel( x_avion-70, y_avion-232, JAUNE);
  2726.  
  2727. }
  2728. }
  2729.  
  2730. if((x_avion > x2)&&(x_avion<=x3))
  2731. {
  2732. y_avion = (uint16_t) yF;
  2733. TFT480.drawLine(x2, y1, x3, y2, BLANC);
  2734. if (y_avion >235)
  2735. {
  2736. TFT480.fillCircle(x_avion, y_avion, 5, BLEU);
  2737. SPR_trajectoire.drawPixel( x_avion-70, y_avion-232, JAUNE);
  2738.  
  2739. }
  2740. }
  2741.  
  2742. if((x_avion > x3)&&(x_avion<=x4))
  2743. {
  2744. y_avion = y2;
  2745. TFT480.drawLine(x3, y2, x4, y2, BLANC);
  2746. if (y_avion >235)
  2747. {
  2748. TFT480.fillCircle(x_avion, y_avion, 5, BLEU);
  2749. SPR_trajectoire.drawPixel( x_avion-70, y_avion-232, JAUNE);
  2750. }
  2751. }
  2752.  
  2753. memo_x_avion = x_avion;
  2754. memo_y_avion = y_avion;
  2755.  
  2756. //SPR_trajectoire.pushSprite(70, 232, TFT_BLACK); // TFT_BLACK -> défini la couleur de transparence
  2757.  
  2758. affi_correction_az();
  2759. }
  2760.  
  2761.  
  2762.  
  2763. void affichages()
  2764. {
  2765. if (roulis < -45) {roulis = -45;}
  2766. if (roulis > 45) {roulis = 45;}
  2767.  
  2768. if (tangage < -30) {tangage = -30;}
  2769. if (tangage > 30) {tangage = 30;}
  2770.  
  2771. affi_HA();
  2772. dessine_avion();
  2773.  
  2774. affi_vitesse();
  2775. affi_hauteur();
  2776. affi_vt_verticale();
  2777. affi_acceleration();
  2778. if(locks_type != "VS") {affi_asel(asel1*100);}
  2779. affi_target_speed();
  2780.  
  2781. //autoland = 1; // POUR TEST
  2782.  
  2783. if (autoland == 0)
  2784. {
  2785. trace_arc_gradu();
  2786. affi_cap();
  2787. affi_autopilot(1); // dans le petit cercle en bas à gauche
  2788. affi_indicateurs();
  2789. affi_switches();
  2790. affi_potars();
  2791. //affi_radial_VOR();
  2792. affi_Airport();
  2793. }
  2794. else
  2795. {
  2796. affi_approche();
  2797. }
  2798.  
  2799. //bargraph_float_test(GPS_azimut_piste/360.0);
  2800. //bargraph_float_test(GPS_distance_piste);
  2801. }
  2802.  
  2803.  
  2804. void desengage_autoland()
  2805. {
  2806. elv_trim=-10.0; // en degrés
  2807. autoland=0;
  2808. params &= ~1;
  2809. efface_cadre_bas(NOIR);
  2810. //SPR_trajectoire.fillSprite(TFT_BLACK);
  2811. //SPR_trajectoire.drawString("DESCENTE 5%", 170, 1 );
  2812. init_affi_HA();
  2813. locks_type = "ALT";
  2814. voyant_L.affiche(BLANC, GRIS_FONCE);
  2815. voyant_G.affiche(BLANC, GRIS_FONCE);
  2816.  
  2817. }
  2818.  
  2819.  
  2820.  
  2821. void traitement_switches()
  2822. {
  2823. // les switches en question sont situés sur le SW (panel des boutons)
  2824. // leurs valeurs sont tranmises par WiFi
  2825. // voir leur réception ici dans la fonction "setup()"
  2826. flag_traiter_SW =0;
  2827. if(v_switches != memo_v_switches)
  2828. {
  2829. memo_v_switches = v_switches;
  2830. if ((v_switches & 1)==1)
  2831. {
  2832. if (autoland == 0)
  2833. {
  2834. if (hdg1>30) {hdg1 -=30;} else {hdg1+=330;} // la barre à tribord 30° !
  2835. RAZ_chrono();
  2836. }
  2837.  
  2838. if ((autoland == 1) || (flag_auto_rudder == 1)) // à l'attérissage ou au décollage
  2839. {
  2840. correction_az_piste -= 0.5; // pour autoland en finale
  2841. if (correction_az_piste > 3.0) {correction_az_piste = 3.0;}
  2842. affi_correction_az();
  2843. }
  2844.  
  2845. }
  2846.  
  2847. if ((v_switches & 2)==2)
  2848. {
  2849. if (autoland == 0)
  2850. {
  2851. if (hdg1<330) {hdg1 +=30;} else {hdg1-=330 ;} // la barre à babord 30° !
  2852. RAZ_chrono();
  2853. }
  2854.  
  2855. if ((autoland == 1) || (flag_auto_rudder == 1)) // à l'attérissage ou au décollage
  2856. {
  2857. correction_az_piste += 0.5; // pour autoland en finale
  2858. if (correction_az_piste < -3.0) {correction_az_piste = -3.0;}
  2859. affi_correction_az();
  2860. }
  2861. }
  2862.  
  2863. if ((v_switches & 4)==4)
  2864. {
  2865. if (asel1<600) {asel1 +=10;} // consigne d'altitude +1000ft
  2866. }
  2867.  
  2868. if ((v_switches & 8)==8)
  2869. {
  2870. if (asel1>10) {asel1 -=10;} // consigne d'altitude -1000ft
  2871. }
  2872.  
  2873. if ((v_switches & 16)==16)
  2874. {
  2875. if (target_speed<160) {target_speed +=5;} // consigne de vitesse
  2876. else if ((target_speed>=160) && (target_speed<600)) {target_speed +=20;}
  2877. }
  2878.  
  2879. if ((v_switches & 32)==32)
  2880. {
  2881. if ((target_speed>160) && (target_speed<600)) {target_speed -=20;}
  2882. else if ((target_speed<=160) && (target_speed>90)) {target_speed -=5;}
  2883. }
  2884.  
  2885. if ((v_switches & 64)==64)
  2886. {
  2887. // toggle autoland
  2888. switch (autoland)
  2889. {
  2890. case 0:
  2891. {
  2892. if (hauteur_AAL >100)
  2893. {
  2894. memo_hdg1 = hdg1; // mémorise le cap avant de passer la main à l'autoland
  2895. if(asel1==0) {asel1 = 30;}
  2896. autoland=1;
  2897. flag_auto_rudder=0; // le lacet se fera manuellement pour l'attérrissage
  2898. correction_az_piste =0;
  2899. params |= 1;
  2900. efface_cadre_bas(GRIS_TRES_FONCE);
  2901. affichages();
  2902. }
  2903. if (hauteur_AAL < 20)
  2904. {
  2905. flag_auto_rudder ^= 1;
  2906. if (flag_auto_rudder == 0) correction_az_piste=0;
  2907. affiche_etat_auto_rudder();
  2908. }
  2909. }
  2910. break;
  2911. case 1:
  2912. {
  2913. // si, suite à un enclenchement non correct de l'autoland (sur un mauvais sens du localizer en particulier) le cap a été changé
  2914. // on le remet à sa valeur mémorisée. Evite de devoir tourner le bouton pendant une heure avec l'avion qui part à l'aventure !
  2915. //hdg1 = memo_hdg1;
  2916.  
  2917. desengage_autoland();
  2918. affichages();
  2919. }
  2920. break;
  2921. }
  2922. }
  2923.  
  2924. if ((v_switches & 128)==128)
  2925. // HDG SYNC
  2926. {
  2927. hdg1 = cap;
  2928. RAZ_chrono();
  2929. }
  2930.  
  2931. }
  2932.  
  2933. if(v_switches_ND != memo_v_switches_ND)
  2934. {
  2935. memo_v_switches_ND = v_switches_ND;
  2936. if ((v_switches_ND & 1)==1)
  2937. {
  2938. TFT480.fillCircle(465, 310, 5, BLEU_CLAIR);
  2939. landing_light1=1;
  2940. landing_light2=1;
  2941.  
  2942. }
  2943. else
  2944. {
  2945. TFT480.fillCircle(465, 310, 5, NOIR);
  2946. landing_light1=0;
  2947. landing_light2=0;
  2948. }
  2949. }
  2950. }
  2951.  
  2952.  
  2953.  
  2954. void affi_correction_az()
  2955. {
  2956. // affiche correction azimut piste (demi deg par demi deg en + ou en -)
  2957. // (qui applique de légères corrections à l'orientation de la piste lors d'une approche GPS)
  2958. // ces corrections se font avec les boutons < HDG > du panel des boutons, en mode APP (qui a remplacé ici le mode ILS)
  2959. // en effet, les orientations fournies dans le fichier FG_data.h peuvent se révéler très légèrement fausses
  2960. // et puis la variable hdg_bug de FlightGear est un integer, pas un float !
  2961. // dans ce cas, si visibilité OK, l'application de ces corrections en temps réel permet de faire un attéro bien dans l'axe
  2962. TFT480.setFreeFont(FM9);
  2963. TFT480.setTextColor(VERT);
  2964. TFT480.fillRect(280, 0, 50, 18, NOIR); //efface
  2965. String s1 = String(correction_az_piste, 1);
  2966. TFT480.drawString(s1, 280, 0);
  2967.  
  2968. }
  2969.  
  2970.  
  2971.  
  2972. void affiche_etat_auto_rudder()
  2973. {
  2974. if (flag_auto_rudder ==1)
  2975. {
  2976. TFT480.setFreeFont(FM9);
  2977. TFT480.setTextColor(VERT, NOIR);
  2978. TFT480.drawString("auto rudder", 65, 0);
  2979. }
  2980. else
  2981. {
  2982. TFT480.fillRect(65, 0, 120, 18, NOIR); // efface l'étiquette
  2983. TFT480.fillRect(280, 0, 45, 18, NOIR); // efface la valeur à droite (voir la fonction ci-dessus)
  2984. }
  2985. }
  2986.  
  2987.  
  2988.  
  2989.  
  2990. void toutes_les_10s()
  2991. {
  2992.  
  2993.  
  2994. }
  2995.  
  2996.  
  2997.  
  2998. void toutes_les_1s()
  2999. {
  3000. nb_secondes++; // ne concerne pas le chrono de la ligne suivante
  3001.  
  3002. affiche_chrono();
  3003. inc_chrono();
  3004.  
  3005.  
  3006.  
  3007.  
  3008. //// size_t write(uint8_t c);
  3009. //// size_t write(const uint8_t *buffer, size_t size);
  3010.  
  3011. if(nb_secondes>10)
  3012. {
  3013. nb_secondes=0;
  3014. toutes_les_10s();
  3015. }
  3016.  
  3017. dV =10*(vitesse - memo_vitesse);
  3018. memo_vitesse = vitesse;
  3019.  
  3020. if (vitesse > 180)
  3021. {
  3022. flag_auto_rudder=0;
  3023. correction_az_piste =0;
  3024. } // désengage auto_rudder en fin de décollage
  3025.  
  3026. if (au_sol==1)
  3027. {
  3028. // désengage l'autoland ILS après attéro pour éviter de re-décoller avec cet autoland engagé
  3029.  
  3030. if (autoland == 1) {desengage_autoland();}
  3031. //autoland=0;
  3032.  
  3033. //asel1 =30;
  3034. }
  3035.  
  3036. affiche_etat_auto_rudder();
  3037.  
  3038. calculs_GPS();
  3039. //affi_dst_GPS();
  3040. auto_landing(); // ne sera effectif que si la variable autoland !=0;
  3041. //toutefois la fonction est toujours appelée afin d'actualiser les affichages
  3042.  
  3043. if ((memo_GPS_distance_piste - GPS_distance_piste) > 0 )
  3044. {
  3045. flag_eloignement = 0;
  3046. TFT480.fillCircle(300, 5, 5, VERT);
  3047. }
  3048. else
  3049. {
  3050. flag_eloignement = 1;
  3051. TFT480.fillCircle(300, 5, 5, ROUGE);
  3052. }
  3053.  
  3054. memo_GPS_distance_piste = GPS_distance_piste;
  3055.  
  3056. }
  3057.  
  3058.  
  3059.  
  3060.  
  3061.  
  3062.  
  3063. /** ==================================================================
  3064.  variables à gérer obligatoirement */
  3065.  
  3066. uint8_t TEST_AFFI=0;// =0 pour un fonctionnement normal; =1 pour tester les affichages et la fluidité
  3067.  
  3068. //le nombre de ports GPIO libres étant atteint, on va utiiser un switch unique pour deux fonctions :
  3069. uint8_t fonction_bt1 = 1; // 0=saisie écran ; 1=changement de vue
  3070.  
  3071.  
  3072. /**======================================================================================================
  3073.  
  3074.   LOOP
  3075.  
  3076. ====================================================================================================== */
  3077.  
  3078.  
  3079. uint16_t t=0; // temps -> rebouclera si dépassement
  3080. void loop()
  3081. {
  3082. //if (SerialBT.available()) { Serial.write(SerialBT.read()); }
  3083.  
  3084. //le bouton connecté à CET ESP32-ci est partagé entre deux fonctions, voir ci-dessus "variables à gérer obligatoirement"
  3085. bouton1_etat = digitalRead(bouton1);
  3086.  
  3087. if (bouton1_etat != memo_bouton1_etat) // vue
  3088. {
  3089. memo_bouton1_etat = bouton1_etat;
  3090. if (bouton1_etat==0)
  3091. {
  3092. TFT480.fillCircle(450, 310, 5, VERT);
  3093.  
  3094.  
  3095. if (fonction_bt1 == 0) {write_TFT_on_SDcard(); }
  3096. if (fonction_bt1 == 1)
  3097. {
  3098. if (view_number == 0) {view_number = 1;} else {view_number =0;}
  3099. }
  3100. }
  3101. if (bouton1_etat==1)
  3102. {
  3103. TFT480.fillCircle(450, 310, 5, NOIR);
  3104. if (fonction_bt1==1)
  3105. {
  3106. ;
  3107. }
  3108. }
  3109. }
  3110.  
  3111. bouton2_etat = digitalRead(bouton2); // mode affichege de la hauteur / altitude
  3112.  
  3113. if (bouton2_etat != memo_bouton2_etat)
  3114. {
  3115. memo_bouton2_etat = bouton2_etat;
  3116. if (bouton2_etat==0)
  3117. {
  3118. mode_affi_hauteur = AAL;
  3119. affi_mode_affi_hauteur();
  3120. }
  3121. if (bouton2_etat==1)
  3122. {
  3123. mode_affi_hauteur = ASL;
  3124. affi_mode_affi_hauteur();
  3125. }
  3126. }
  3127.  
  3128. if (flag_traiter_SW ==1)
  3129. // le positionnement de ce drapeau se fait dans l'interruption WiFi "server.on("/switch"..."
  3130. // définie dans le setup()
  3131. // la commande, en amont, provient de l'ESP n°3 qui gère les switches
  3132. {
  3133. flag_traiter_SW =0;
  3134. traitement_switches();
  3135. }
  3136.  
  3137.  
  3138. temps_ecoule = micros() - memo_micros;
  3139. if (temps_ecoule >= 1E6) // (>=) et pas strictement égal (==) parce qu'alors on rate l'instant en question
  3140. {
  3141. memo_micros = micros();
  3142. toutes_les_1s();
  3143. }
  3144.  
  3145. if (dV > acceleration) {acceleration++;}
  3146. if (dV < acceleration) {acceleration--;}
  3147.  
  3148.  
  3149. //---------------------------------------------------------------------------------------
  3150. if(TEST_AFFI==1) // pour test des affichages:
  3151. {
  3152.  
  3153. // les valeurs de ces paramètres peuvent être modifiées afin de tester tel ou tel point particulier
  3154.  
  3155. vitesse = 100 - 100*cos(t/200.0);
  3156. //vitesse = 100;
  3157.  
  3158. altitude_GPS = 1000 - 800*cos(t/400.0);
  3159. vspeed = 80*sin(t/100.0);
  3160.  
  3161. tangage =20.0*sin(t/87.0);
  3162. roulis = 35.0*sin(t/35.0);
  3163.  
  3164. data_ok=(1<<nb_acqui)-1;
  3165.  
  3166. affichages();
  3167.  
  3168. t++;
  3169.  
  3170. TFT480.setTextColor(JAUNE, BLEU);
  3171. TFT480.setFreeFont(FF1);
  3172. TFT480.drawString("mode TEST", 0, 0);
  3173.  
  3174.  
  3175. if (dV > acceleration) {acceleration++;}
  3176. if (dV < acceleration) {acceleration--;}
  3177.  
  3178. data_out(); // ** à commenter le cas échéant pour libérer le port série lors de la mise au point **
  3179.  
  3180. delay(20);
  3181.  
  3182. }
  3183.  
  3184. //---------------------------------------------------------------------------------------
  3185. else // FONCTIONNEMENT NORMAL
  3186. {
  3187. compteur1+=1;
  3188. if (compteur1 >= 100) // tous les 1000 passages dans la boucle
  3189. {
  3190. compteur1=0;
  3191. //affi_autopilot();
  3192. }
  3193.  
  3194. acquisitions();
  3195.  
  3196. if (flag_auto_rudder ==1)
  3197. {
  3198. calculs_GPS(); // ça, oui, on effectue à chaque passage
  3199. auto_rudder_au_decollage();
  3200. }
  3201.  
  3202.  
  3203. /** ----------------------------------------------
  3204.  pour tester si data_ok à bien la valeur attendue, c.a.d si toutes les acquisitions sont effectuées
  3205.  ce qui est le cas lorsque le dialogue avec le serveur de protocole FG est correctement programmé
  3206.  sachant que ce ne sont pas les pièges qui manquent !
  3207.  Les 6 lignes de code qui suivent peuvent faire gagner une journée...
  3208.  Il suffit d'analyser la valeur affichée en binaire pour voir les bits qui restent à 0 -> problèmes (à priori de libellés)
  3209.  Il doit y avoir une correspondance stricte entre les noms de variables ici et celles émises par le fichier hardware4.xml
  3210. */
  3211.  
  3212. ////TFT480.setFreeFont(FM9);
  3213. ////TFT480.setTextColor(VERT, NOIR);
  3214. ////s2= (String) data_ok;
  3215. ////TFT480.fillRect(150, 220, 80, 12, TFT_BLACK);
  3216. ////TFT480.drawString(s2, 150, 220);
  3217.  
  3218. /** ---------------------------------------------- **/
  3219.  
  3220. if (data_ok == ((1<<nb_acqui)-1) ) // si toutes les acquisitions faites
  3221. {
  3222. TFT480.fillCircle(435, 310, 5, VERT);
  3223. if (attente_data == 1)
  3224. {
  3225. attente_data=0;
  3226. TFT480.fillScreen(TFT_BLACK);
  3227. init_affi_HA();
  3228. //init_affi_autopilot();
  3229. }
  3230.  
  3231. affichages();
  3232. calculs_GPS();
  3233. data_ok=0;
  3234.  
  3235. data_out(); // ** à commenter le cas échéant pour libérer le port série lors de la mise au point **
  3236. }
  3237. else
  3238. {
  3239. TFT480.fillCircle(435, 310, 5, ROUGE);
  3240. if(attente_data==1)
  3241. {
  3242. affi_nop();
  3243. RAZ_variables();
  3244. affichages();
  3245. delay(100);
  3246. }
  3247.  
  3248. }
  3249.  
  3250. if ((vitesse > 100) && (altitude_GPS > 500)) {au_sol =0;}
  3251. if ((vitesse < 50) && (hauteur_AAL < 40)) {au_sol =1;}
  3252.  
  3253. ////v_test1 += 0.01;
  3254. ////if (v_test1 > 1.0) {v_test1 = -1.0;}
  3255. ////bargraph_float_test(v_test1);
  3256.  
  3257.  
  3258. }
  3259.  
  3260. //---------------------------------------------------------------------------------------
  3261.  
  3262.  
  3263.  
  3264. }
  3265.  
  3266.  
  3267. /** ***************************************************************************************
  3268. CLASS VOYANT // affiche un nombre ou un petit texte dans un rectangle
  3269. ainsi que (en plus petit) deux valeurs supplémentaires, par ex: les valeurs mini et maxi
  3270. ********************************************************************************************/
  3271.  
  3272. // Constructeur
  3273. VOYANT::VOYANT()
  3274. {
  3275.  
  3276. }
  3277.  
  3278.  
  3279.  
  3280. void VOYANT::init(uint16_t xi, uint16_t yi, uint16_t dxi, uint16_t dyi)
  3281. {
  3282. x0 = xi;
  3283. y0 = yi;
  3284. dx = dxi;
  3285. dy = dyi;
  3286. &nb