diff --git a/BaliseDGAC_GPS_Logger.ino b/BaliseDGAC_GPS_Logger.ino index 1dde583..ef17574 100644 --- a/BaliseDGAC_GPS_Logger.ino +++ b/BaliseDGAC_GPS_Logger.ino @@ -1,7 +1,8 @@ -// BaliseDGAC_GPS_Logger Balise avec enregistrement de traces -// 20/03/2021 v2 + +// BaliseDGAC_GPS_Logger Balise avec enregistrement de traces et récepteur balise +// 03/2022 v3 b1 // Choisir la configuration du logiciel balise dans le fichier fs_options.h -// (pin utilisées pour le GPS, option GPS, etc ...) +// (pins utilisées pour le GPS, option GPS, etc ...) /* This program is free software: you can redistribute it and/or modify @@ -18,51 +19,110 @@ along with this program. If not, see . */ /*------------------------------------------------------------------------------ - 02/2021 + 03/2022 02/2021 Author: FS - Platforms: ESP8266 + Platforms: ESP8266 / ESP32 / ESP32-C3 Language: C++/Arduino Basé sur : - https://github.com/dev-fred/GPS_Tracker_ESP8266/tree/main/GPS_Tracker_ESP8266V1 + https://github.com/dev-fred/GPS_Tracker_ESP8266 https://github.com/khancyr/droneID_FR https://github.com/f5soh/balise_esp32/blob/master/droneID_FR.h (version 1 https://discuss.ardupilot.org/t/open-source-french-drone-identification/56904/98 ) https://github.com/f5soh/balise_esp32 + https://www.tranquille-informatique.fr/modelisme/divers/balise-dgac-signalement-electronique-a-distance-drone-aeromodelisme.html ------------------------------------------------------------------------------*/ -#include +#ifdef ESP32 +#pragma message "Compilation pour ESP32 !" +#include +#include "fs_WebServer.h" +#elif defined(ESP8266) +#pragma message "Compilation pour ESP8266 !" #include -#include +#include +#else +#error "Il faut utiliser une carte ESP32 ou ESP8266" +#endif + +#include #include #include -#include #include #include "droneID_FR.h" #include "fsBalise.h" #include "fs_options.h" #include "fs_GPS.h" -#include "fs_filtreGpsAscii.h" +#include "fs_main.h" +//#ifdef fs_RECEPTEUR +#include "fs_recepteur.h" +//#endif +#include "fs_pagePROGMEM.h" + +#if defined(ESP32) +#include +#pragma message "Utilisation de HardwareSerial !" +#else +#include +#pragma message "Utilisation de SotfwareSerial !" +#endif extern pref preferences; -extern char statistics[] PROGMEM ; +//extern const char statistics[] PROGMEM ; extern File traceFile; const char prefixe_ssid[] = "BALISE"; // Enter SSID here // déclaration de la variable qui va contenir le ssid complet = prefixe + MAC adresse -char ssid[32]; +char ssid[33]; +//double codeinfo = 0; // FS : pourquoi double ???????? ++++++++++++++++++++++++++++++ +double codeinfo = 1; // FS : pourquoi double ???????? ++++++++++++++++++++++++++++++ + +/* + codeinfo + 1 GPS non détecté, aucune donnée GPS reçue + + 2 Positionnement du GPS non valide (fix en cours?) + 3 Fix du GPS perdu... + 4 La précision du GPS n'est pas bonne + Nb de satellites (doit être supérieur à 3): " + String(gps.satellites.value())); + 5 La précision du GPS n'est pas bonne + Précision 2D (doit être inférieure à 5.0): " + String(gps.hdop.hdop())); + 6 La précision du GPS n'est pas bonne + Altitude non récupérée: " + String(gps.altitude.meters())); + 7 stockage positionde départ + 9 ça roule. Pas d'erreur +*/ +// dès que la position home a été mise, on passe à true et on y reste +bool has_set_home = false; +double home_alt = 0; +//les positions précédentes: +double lat_prev = 0; +double lon_prev = 0; +int16_t alt_prev = 0; +int16_t dir_prev = 0; +double vit_prev = 0; // en m/s + +boolean saveLocationIsUpdated; /** Numero de serie donnée par l'dresse MAC ! Changer les XXX par vos references constructeurs, le trigramme est 000 pour les bricoleurs ... */ - -char drone_id[31] = "000XXX000000000000XXXXXXXXXXXX"; // les xxx seront remplacés par l'adresse MAC, sans les ":" +char drone_id[31] = "000FSB000000000000YYYYYYYYYYYY"; // les YY seront remplacés par l'adresse MAC, sans les ":" // 012345678901234567890123456789 // 11111111112222222222 +#ifdef ESP32 +extern "C" +{ +#include "esp_wifi.h" + esp_err_t esp_wifi_80211_tx(wifi_interface_t ifx, const void *buffer, int len, bool en_sys_seq); + esp_err_t esp_wifi_internal_set_fix_rate(wifi_interface_t ifx, bool en, wifi_phy_rate_t rate); +} +#else extern "C" { #include "user_interface.h" int wifi_send_pkt_freedom(uint8 *buf, int len, bool sys_seq); } +#endif droneIDFR drone_idfr; @@ -101,11 +161,17 @@ static_assert((sizeof(drone_id) / sizeof(*drone_id)) <= 31, "Drone ID should be /* Put IP Address details-> smartphone */ const byte DNS_PORT = 53; DNSServer dnsServer; +#ifdef ESP32 +//WebServer server(80); +fs_WebServer server(80); +#else ESP8266WebServer server(80); +#endif IPAddress local_ip; // +++++ FS https://github.com/espressif/arduino-esp32/issues/1037 IPAddress gateway; IPAddress subnet; +boolean modeRecepteur = false; // false: mode balise avec emmision de trames; true: mode reception de trames beacon boolean traceFileOpened = false; boolean fileSystemFull = false; boolean immobile = false; @@ -113,574 +179,542 @@ long T0Immobile; String messAlarm = ""; unsigned int timeLogError = 0; -unsigned int counter = 0; -unsigned int TRBcounter = 0; -unsigned int nb_sat = 0; -unsigned int SV = 0; -uint64_t gpsSec = 0; -uint64_t beaconSec = 0; -float VMAX = 0.0; -char CLonLat[32]; -char CHomeLonLat[32]; -char Cmd; -float altitude_ref = 0.0 ; -float Lat = 0.0 ; -float Lon = 0.0 ; -float HLng = 0.0; -float HLat = 0.0; -float latLastLog, lngLastLog ; // FS ++++++ pour calcul déplacement depuis le dernier log -static uint64_t gpsMap = 0; -String Messages = "init"; -unsigned long _heures = 0; -unsigned long _minutes = 0; -unsigned long _secondes = 0; -const unsigned int limite_sat = 5; -const float limite_hdop = 2.0; -float _hdop = 0.0; -unsigned int _sat = 0; +unsigned long TimeShutdown; +boolean countdownRunning = true; +boolean shutdown = false; + +unsigned int TRBcounter = 0; // nbr de trames nvoyées + +float vitesse = 0.0; // vitesse en m/s, qui sera lissée, pour décision fermer fichier et arret wifi +float VmaxCockpit = 0.0, AltmaxCockpit = 0.0; +float VmaxSegment = 0.0, AltMaxSegment = 0.0 ;// vitesse (kmh)/alt max vue entre 2 points de trace enregistré + +float latLastLog, lngLastLog ; // FS ++++++ pour calcul déplacement depuis le dernier log //=========== #ifdef fs_STAT // pour statistiques -unsigned long T0Beacon, T0Loop, T0SendPkt, T0Server; -int entreBeaconMin, entreBeaconMax, entreBeaconTotal, entreBeaconSousTotal; -int duree, dureeMinLog, dureeMaxLog, dureeTotaleLog, dureeSousTotalLog; -int segment, segmentMin, segmentMax, segmentTotal, segmentSousTotal; -int periodeLoop, perMinLoop, perMaxLoop, perTotaleLoop, perSousTotalLoop; -int dureeMinSendPkt, dureeMaxSendPkt, dureeTotaleSendPkt, dureeSousTotalSendPkt; -int dureeMinServer, dureeMaxServer, dureeTotaleServer, dureeSousTotalServer; -float entreBeaconMoyenneLocale, dureeMoyenneLocaleLog, segmentMoyenneLocale, perMoyenneLocaleLoop; -float dureeMoyenneLocaleSendPkt, dureeMoyenneLocaleServer; +unsigned long T0Beacon; +statBloc_t statBeacon = {"Période Beacon"}, statSendPkt = {"Durée Sendpkt"}, statServer = {"Durée Server handle"}, + statLog = {"Durée ecr trace"}, statSegment = {"Long segment"}, statLoop = {"Période loop"} ; +statBloc_t statReveiller = {"Durée reveiller"}, statEndormir = {"Durée endormir"}; +statBloc_t statNotFound = {" notFound" }, statCockpit = {"cockpit"}; + int n0Failed, n0Passed; -unsigned int countLoop, countSendPkt, countServer, nbrLogError = 0; +unsigned int nbrLogError = 0; #endif // ou debug unsigned int countLog = 0; -unsigned long T0Log, T1Log = 0; +unsigned long T1Log = 0; char cGPS; -char ringBuffer[500]; -int indexBuffer; //=========== -//buzz(4, 2500, 1000); // buzz sur pin 4 à 2500Hz -void buzz(int targetPin, long frequency, long length) { -#ifdef ENABLE_BUZZER - long delayValue = 1000000 / frequency / 2; - long numCycles = frequency * length / 1000; - for (long i = 0; i < numCycles; i++) - { - digitalWrite(targetPin, HIGH); - delayMicroseconds(delayValue); - digitalWrite(targetPin, LOW); - delayMicroseconds(delayValue); - } -#endif -} - -#define led_pin 2 //internal blue LED -int led; -void flip_Led() { -#ifdef EBABLE_LED - //Flip internal LED - if (led == HIGH) { - digitalWrite(led_pin, led); - led = LOW; - } else { - digitalWrite(led_pin, led); - led = HIGH; - } +#if defined(ESP32) +HardwareSerial serialGPS(1); +#else +SoftwareSerial serialGPS(GPS_RX_PIN, GPS_TX_PIN); #endif -} - - -SoftwareSerial softSerial(GPS_RX_PIN, GPS_TX_PIN); TinyGPSPlus gps; -#define USE_SERIAL Serial - -char buff[14][34]; // contient les valeurs envoyées àla page HTML cockpit -void handleReadValues() { // pour traiter la requette de mise à jour de la page HTML cockpit - String mes = ""; - for (int i = 0; i <= 13; i++) { - if (i != 0) mes += ";"; - mes += (String)i + "$" + buff[i] ; - } - // ecraser VMAX (case 7) si une alarme. - if (messAlarm != "") mes += ";7$" + messAlarm; - server.send(200, "text/plain", mes); -} #ifdef fs_STAT -void resetStatistics() { - entreBeaconMin = 99999999; entreBeaconMax = 0; entreBeaconTotal = 0; entreBeaconSousTotal = 0; entreBeaconMoyenneLocale = 0.0; - dureeMinLog = 99999999; dureeMaxLog = 0; dureeTotaleLog = 0; dureeSousTotalLog = 0; dureeMoyenneLocaleLog = 0.0; - segmentMin = 99999999; segmentMax = 0; segmentTotal = 0; segmentSousTotal = 0; segmentMoyenneLocale = 0.0; - perMinLoop = 99999999; perMaxLoop = 0; perTotaleLoop = 0; perSousTotalLoop = 0, perMoyenneLocaleLoop = 0.0; - - dureeMinSendPkt = 99999999; dureeMaxSendPkt = 0; dureeTotaleSendPkt = 0; dureeSousTotalSendPkt = 0; dureeMoyenneLocaleSendPkt = 0.0; - dureeMinServer = 99999999; dureeMaxServer = 0; dureeTotaleServer = 0; dureeSousTotalServer = 0; dureeMoyenneLocaleServer = 0.0; - - countLoop = 0; countSendPkt = 0; countServer = 0; - countLog = 0; nbrLogError = 0; TRBcounter = 0; T1Log = 0; T0Beacon = 0; +void razStatistics() { + razStatBloc(&statBeacon); razStatBloc(&statSendPkt); razStatBloc(&statServer); + razStatBloc(&statLog); razStatBloc(&statSegment); razStatBloc(&statLoop); + razStatBloc(&statReveiller); razStatBloc(&statEndormir); + razStatBloc(&statNotFound); razStatBloc(&statCockpit); + nbrLogError = 0; T1Log = 0; n0Passed = gps.passedChecksum(); n0Failed = gps.failedChecksum(); - handleStat(); } -// Calcul pour statistiques min/max etc ... -// Tout est passer par référence !! sauf T0 -void calculerStat (boolean calculerTemps, int T0, int &minimum, int &maximum, float &moyenneLocaleEchantillons, int &totalEchantillons, - int &sousTotalEchantillons, unsigned int &count) { - - // calculerTemps true: T0 représente le temps début de la periode de mesure - // (utile pour stat de timming) - // calculerTemps false: T0 represente la mesure elle même, dont on va calculer min/max etc .. - // (utile pour stat sur longeur de segment) - long T1; - int echantillon; - if (T0 == 0 && calculerTemps) return; // ignorer le premier car T0 est parfois mal initialisé - T1 = millis(); - if (calculerTemps) echantillon = T1 - T0; - else echantillon = T0; - totalEchantillons += echantillon; minimum = min(minimum, echantillon); maximum = max(maximum, echantillon); - sousTotalEchantillons += echantillon; - count++; - if (count % 20 == 0) { - moyenneLocaleEchantillons = float(sousTotalEchantillons) / 20.0; - sousTotalEchantillons = 0; - } +void handleResetStatistics() { + razStatistics(); // raz des compteurs & Co + handleStat(); // renvoyer le html } -// Generation reponse pour la page statistique + void handleReadStatistics() { - char buf[350]; // 260 car env utlisés + dbgHeap("deb"); + char buf[350]; // 290 car env utlisés int deb = 0; - deb += sprintf(&buf[deb], "0$%s Err GPS:%u
passedCheck:%u failedCheck:%u (%.0f%%);", messAlarm.c_str(), nbrLogError, - gps.passedChecksum() - n0Passed, gps.failedChecksum() - n0Failed , - 100 * float(gps.failedChecksum() - n0Failed) / float(gps.passedChecksum() - n0Passed)); // - deb += sprintf(&buf[deb], "1$Nbr entrées trace: %u ,Nbr Beacon: %u;", countLog, TRBcounter ); - deb += sprintf(&buf[deb], "2$%u;", dureeMinLog); - deb += sprintf(&buf[deb], "3$%u;", dureeMaxLog); - deb += sprintf(&buf[deb], "4$%.1f;", float(dureeTotaleLog) / countLog); - deb += sprintf(&buf[deb], "5$%.1f;", dureeMoyenneLocaleLog); - deb += sprintf(&buf[deb], "6$%u;", segmentMin); - deb += sprintf(&buf[deb], "7$%u;", segmentMax); - deb += sprintf(&buf[deb], "8$%.1f;", float(segmentTotal) / countLog); - deb += sprintf(&buf[deb], "9$%.1f;", segmentMoyenneLocale); - deb += sprintf(&buf[deb], "10$%u;", entreBeaconMin); - deb += sprintf(&buf[deb], "11$%u;", entreBeaconMax); - deb += sprintf(&buf[deb], "12$%.1f;", float(entreBeaconTotal) / TRBcounter); - deb += sprintf(&buf[deb], "13$%.1f;", entreBeaconMoyenneLocale); - deb += sprintf(&buf[deb], "14$%u;", perMinLoop); - deb += sprintf(&buf[deb], "15$%u;", perMaxLoop); - deb += sprintf(&buf[deb], "16$%.1f;", float(perTotaleLoop) / countLoop); - deb += sprintf(&buf[deb], "17$%.1f;", perMoyenneLocaleLoop); - deb += sprintf(&buf[deb], "18$%u;", dureeMinServer); - deb += sprintf(&buf[deb], "19$%u;", dureeMaxServer); - deb += sprintf(&buf[deb], "20$%.1f;", float(dureeTotaleServer) / countServer); - deb += sprintf(&buf[deb], "21$%.1f;", dureeMoyenneLocaleServer); - deb += sprintf(&buf[deb], "22$%u;", dureeMinSendPkt); - deb += sprintf(&buf[deb], "23$%u;", dureeMaxSendPkt); - deb += sprintf(&buf[deb], "24$%.1f;", float(dureeTotaleSendPkt) / countSendPkt); - deb += sprintf(&buf[deb], "25$%.1f", dureeMoyenneLocaleSendPkt); // pas de ; pour la dernière valeur - //Serial.println(deb); - // Serial.print("\t"); Serial.println(buf); + // Pour remplir la page HTML de stats. 2eme champs (délimité par les ;) vont dans les elements Value1 et Value2 + // Les autres remplissent la table construite dynamiquement + // libellé1$min$max$moyenne1$moyenne2;libellé2$min$max$moyenne1$moyenne2 + int gpsFailed = gps.failedChecksum(); + int gpsPass = gps.passedChecksum(); + deb += snprintf_P(&buf[deb], sizeof(buf) - deb, PSTR("%s Err GPS:%u
passedCheck:%u failedCheck:%u (%.1f%%);"), messAlarm.c_str(), nbrLogError, + gpsPass - n0Passed, gpsFailed - n0Failed , + 100 * float(gpsFailed - n0Failed) / float(gpsFailed - n0Failed + gpsPass - n0Passed)); + deb += snprintf_P(&buf[deb], sizeof(buf) - deb, PSTR("Nbr entrées trace: %u ,Nbr Beacon: %u;"), + countLog, TRBcounter); + deb += writeStatBloc(&buf[deb], sizeof(buf) - deb, &statLog); + deb += writeStatBloc(&buf[deb], sizeof(buf) - deb, &statSegment); + deb += writeStatBloc(&buf[deb], sizeof(buf) - deb, &statBeacon); + deb += writeStatBloc(&buf[deb], sizeof(buf) - deb, &statLoop); + deb += writeStatBloc(&buf[deb], sizeof(buf) - deb, &statServer); + // deb += writeStatBloc(&buf[deb], sizeof(buf) - deb, &statSendPkt); + deb += writeStatBloc(&buf[deb], sizeof(buf) - deb, &statNotFound); + // deb += writeStatBloc(&buf[deb], sizeof(buf) - deb, &statCockpit); + + // effacer le dernier ";". + deb--; + buf[deb] = 0; + // Serial.printf_P(PSTR("-----handleReadStatistics: %u %i/%i %s\n "), millis(), deb, sizeof(buf), buf); server.send(200, "text/plain", buf); + dbgHeap("fin"); } void handleStat() { + countdownRunning = false; sendChunkDebut ( true ); // debut HTML style, avec topMenu server.sendContent_P(statistics); server.chunkedResponseFinalize(); } #endif // partie spécifique pour statistiques +// Generation reponse requette pour mise à jour de la page HTML cockpit +// Format: suite de N°_de_champ$valeur; +void handleReadValues() { + const char *messageCodeinfo[] = {"GPS", "pos/date", "perdu", "sat", "hdop", "alt", "", "", ""}; + char buf[400]; // 350 car env utlisés + int deb = 0; + countdownRunning = true; //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + deb += sprintf_P(&buf[deb], PSTR("1$Balise v%s;"), versionSoft); + deb += sprintf_P(&buf[deb], PSTR("2$ID: %s;"), drone_id); + if (messAlarm != "") deb += sprintf_P(&buf[deb], PSTR("3$%s;"), messAlarm.c_str()); + else { + deb += sprintf_P(&buf[deb], PSTR("3$Mode basse consommation ")); + if (!preferences.arretWifi) + deb += sprintf_P(&buf[deb], PSTR("impossible.;")); + else + deb += sprintf_P(&buf[deb], PSTR("%s programmé.;"), preferences.basseConso ? "" : " non"); + } + if (!preferences.arretWifi) + deb += sprintf_P(&buf[deb], PSTR("5$Arrêt du Wifi non programmé.;")); // pas de ; pour la dernière valeur + else + deb += sprintf_P(&buf[deb], PSTR("5$;"), has_set_home ? "'b1'> Arrêt" : "'b3'> Après le fix GPS, arrêt" , int((int(TimeShutdown) - int(millis())) / 1000)); // pas de ; pour la dernière valeur + deb += sprintf_P(&buf[deb], PSTR("7$Lng:%.6f;"), lon_prev); + deb += sprintf_P(&buf[deb], PSTR("8$Lat:%.6f;"), lat_prev); + deb += sprintf_P(&buf[deb], PSTR("9$%02d:%02d:%02d.%02d;"), gps.time.hour(), gps.time.minute(), gps.time.second(), + gps.time.centisecond()); + deb += sprintf_P(&buf[deb], PSTR("10$H(Max):%i(%i);"), int(alt_prev - home_alt), int(AltmaxCockpit - home_alt)); // hauteur par rapport au sol + deb += sprintf_P(&buf[deb], PSTR("11$V(Max):%.2f(%.2f)
km/h;"), vit_prev * 3.6, VmaxCockpit); // vitesse en km/h + deb += sprintf_P(&buf[deb], PSTR("12$Cap:%i;"), dir_prev); + deb += sprintf_P(&buf[deb], PSTR("13$Trames:%u;"), TRBcounter); + deb += sprintf_P(&buf[deb], PSTR("14$Pts de trace:%u;"), countLog); + deb += sprintf_P(&buf[deb], PSTR("15$Code:%u;"), int(codeinfo)); + + deb += sprintf_P(&buf[deb], PSTR("16$;"), codeinfo <= 6 ? "'b2'>Att. Fix" : "'b1'>Fix OK" , + messageCodeinfo[int(codeinfo) - 1]); + deb += sprintf_P(&buf[deb], PSTR("17$Sat:%u;"), gps.satellites.value()); + deb += sprintf_P(&buf[deb], PSTR("18$Hdop:%.2f"), gps.hdop.hdop()); + + // Serial.printf_P(PSTR("-----handleReadValues. millis=%u TimeShutdown=%u deb=%i/%i\n"), millis(), TimeShutdown, deb, sizeof(buf)); + server.send(200, "text/plain", buf); +} +void handleRazVMaxHMAx() +{ + Serial.println(F("handleRazVMaxHMAx")); + VmaxCockpit = 0.0; + AltmaxCockpit = 0.0; + server.send(204); // OK, no content +} void beginServer() { server.begin(); - + Serial.println (F("HTTP server started")); + fs_initServerOn( ); // server.on(xxx Spécifiques au log, gestion fichier,OTA,option // if DNSServer is started with "*" for domain name, it will reply with // provided IP to all DNS request // dnsServer.setTTL(300); // va Serial.print(F("Starting dnsServer for captive portal ... ")); Serial.println(dnsServer.start(DNS_PORT, "*", local_ip) ? F("Ready") : F("Failed!")); Serial.print(F("Opening filesystem littleFS ... ")); - Serial.println(LittleFS.begin() ? F("Ready") : F("Failed!")); - fs_initServerOn( ); // server.on(xxx Spécifiques au log, gestion fichier,OTA,option -#ifdef fs_STAT - server.on("/stat", handleStat); - server.on("/readStatistics", handleReadStatistics); - server.on("/statReset", resetStatistics); + +#ifdef ESP32 + Serial.println(LittleFS.begin(true) ? F("Ready") : F("Failed!")); // true = format if fail +#else + Serial.println(LittleFS.begin() ? F("Ready") : F("Failed!")); // pour ESP8266 format if fail par defaut #endif - //+++++++++++++++++++++++++++++++++++++++++++++++++++++ - server.on("/readValues", handleReadValues); - Serial.println (F("HTTP server started")); + nettoyageTraces(); // ne garder qu'un nombre limité de traces } -#ifndef GPS_STANDARD -void SelectChannels() + +uint32_t sleep_time_in_ms = 10000; +void Reveiller_Balise(void) { - // CFG-GNSS packet GPS + Galileo + Glonas - byte packet[] = {0xB5, 0x62, 0x06, 0x3E, 0x3C, 0x00, 0x00, 0x00, 0x20, 0x07, 0x00, 0x08, 0x10, 0x00, - 0x01, 0x00, 0x01, 0x01, 0x01, 0x01, 0x03, 0x00, 0x00, 0x00, 0x01, 0x01, 0x02, 0x04, - 0x08, 0x00, 0x01, 0x00, 0x01, 0x01, 0x03, 0x08, 0x10, 0x00, 0x00, 0x00, 0x01, 0x01, - 0x04, 0x00, 0x08, 0x00, 0x00, 0x00, 0x01, 0x01, 0x05, 0x00, 0x03, 0x00, 0x00, 0x00, - 0x01, 0x01, 0x06, 0x08, 0x0E, 0x00, 0x01, 0x00, 0x01, 0x01, 0x2E, 0x75 - }; - sendPacket(packet, sizeof(packet)); + if (shutdown && preferences.basseConso) { +#ifndef ESP32 + WiFi.forceSleepWake(); // ESP8266 + WiFi.mode(WIFI_STA); + wifi_set_channel(6); +#else + WiFi.mode(WIFI_STA); + esp_wifi_set_channel(6, WIFI_SECOND_CHAN_NONE); // ESP32 +#endif + } + delay(0); + return; } -void BaudRate9600() +void Endormir_Balise() { - byte packet[] = {0xB5, 0x62, 0x06, 0x00, 0x14, 0x00, 0x01, 0x00, 0x00, 0x00, 0xD0, 0x08, 0x00, 0x00, 0x80, 0x25, 0x00, 0x00, 0x07, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0xA2, 0xB5}; - sendPacket(packet, sizeof(packet)); + if (shutdown && preferences.basseConso) { + WiFi.disconnect(); + WiFi.mode(WIFI_OFF); +#ifndef ESP32 + WiFi.forceSleepBegin(); +#endif + delay(0); + } + return; } -void Rate500() +void dumpTrame(uint8_t* beaconPacket, uint16_t to_send) { - byte packet[] = {0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xF4, 0x01, 0x01, 0x00, 0x01, 0x00, 0x0B, 0x77}; - sendPacket(packet, sizeof(packet)); + for (int i = 0; i <= to_send; i++) { + Serial.printf("%1i ", ( i / 100)); + } + Serial.println(); + for (int i = 0; i <= to_send; i++) { + Serial.printf("%1i ", ( i / 10) % 10); + } + Serial.println(); + for (int i = 0; i <= to_send; i++) { + Serial.printf("%1i ", i % 10); + } + Serial.println(); + for (int i = 0; i <= to_send; i++) { + Serial.printf_P(PSTR("%02X"), beaconPacket[i]); + } + Serial.println(); + for (int i = 0; i <= to_send; i++) { + Serial.print(isPrintable( beaconPacket[i]) ? char(beaconPacket[i]) : '.') ; Serial.print(" "); + } + Serial.println(); } -// Send the packet specified to the receiver. -void sendPacket(byte * packet, byte len) +static void EnvoiTrame(double lat_actu, double lon_actu, int16_t alt_actu, int16_t dir_actu, double vit_actu) { - for (byte i = 0; i < len; i++) + //on définit le code info dans la trame: + drone_idfr.set_codeinfo(codeinfo); + //on envoie les données à la biblio pour formattage: + drone_idfr.set_current_position(lat_actu, lon_actu, alt_actu); + drone_idfr.set_heading(dir_actu); + drone_idfr.set_ground_speed(vit_actu); + drone_idfr.set_heigth(alt_actu - home_alt); + + /** + On regarde s'il est temps d'envoyer la trame d'identification drone: soit toutes les 3s soit si le drones s'est déplacé de 30m en moins de 3s. + - soit toutes les 3s, + - soit si le drone s'est déplacé de 30m en moins de 30s soit 10m/s ou 36km/h, + - uniquement si la position Home est déjà définie, + - et dans le cas où les données GPS sont nouvelles. + */ + + // on envoie une trame dès qu'il est temps d'en envoyer une + if (drone_idfr.time_to_send()) { - softSerial.write(packet[i]); +#ifdef pinLed + // Tant que le fix n'est pas fait, flash du led avec la periode des trame. + // Quand le fix est fiat, un flash rapide pour chaque trame. + // Le flash est un peu plus long:brillant en mode économie d'énergie car il y a un certain delay pour endormir/reveiller le wifi + if (codeinfo == 9)digitalWrite(pinLed, HIGH); + else digitalWrite(pinLed, TRBcounter % 2); +#endif + //On commence par renseigner le ssid du wifi dans la trame + // write new SSID into beacon frame + // const size_t ssid_size = (sizeof(ssid) / sizeof(*ssid)) - 1; // remove trailling null termination + size_t ssid_size = strlen(ssid); + beaconPacket[40] = ssid_size; // set size + memcpy(&beaconPacket[41], ssid, ssid_size); // set ssid + const uint8_t header_size = 41 + ssid_size; //TODO: remove 41 for a marker + + //On génère la trame wifi avec l'identification + const uint8_t to_send = drone_idfr.generate_beacon_frame(beaconPacket, header_size); // override the null termination + //allume wifi +#ifdef fs_STAT + statReveiller.T0 = millis(); +#endif + Reveiller_Balise(); + //envoi trame + unsigned long TT = millis(); +#ifdef fs_STAT + calculerStat (true, &statReveiller); + statSendPkt.T0 = millis(); +#endif + //dump une seule fois d'une trame Beacon ... + if (millis() < 9000) { + Serial.println("Trame typique:"); + dumpTrame(beaconPacket, to_send); + } + +#ifdef ESP32 + if (shutdown) { + ESP_ERROR_CHECK(esp_wifi_80211_tx(WIFI_IF_STA, beaconPacket, to_send, true)); + } + else { + ESP_ERROR_CHECK(esp_wifi_80211_tx(WIFI_IF_AP, beaconPacket, to_send, true)); // interface mode AP dans mon cas + } +#else + // ESP_ERROR_CHECK( wifi_send_pkt_freedom(beaconPacket, to_send, true)); + wifi_send_pkt_freedom(beaconPacket, to_send, true); +#endif + +#ifdef fs_STAT + calculerStat (true, &statSendPkt); + statEndormir.T0 = millis(); +#endif + TRBcounter++; + //éteindre wifi + Endormir_Balise(); + //On reset la condition d'envoi + drone_idfr.set_last_send(); +#ifdef fs_STAT + calculerStat (true, &statEndormir); + calculerStat (true, &statBeacon); + statBeacon.T0 = millis(); + if (statSendPkt.count % 20 == 0) { + Serial.printf_P(PSTR("%i\tReveiller :%i %i %f"), statReveiller.count, statReveiller.min, statReveiller.max, statReveiller.moyenneLocale); + Serial.printf_P(PSTR("\tSendpkt :%i %i %f"), statSendPkt.min, statSendPkt.max, statSendPkt.moyenneLocale); + Serial.printf_P(PSTR("\tEndormir :%i %i %f\n"), statEndormir.min, statEndormir.max, statEndormir.moyenneLocale); + dbgHeap("fin"); + + } +#endif +#ifdef pinLed + if (codeinfo == 9) digitalWrite(pinLed, LOW); +#endif } } -#endif void setup() { - +#ifdef pinLed + pinMode(pinLed, OUTPUT); +#endif Serial.begin(115200); - Serial.println(); - Serial.println(); - Serial.println(); + //delay(2000); + Serial.print(F("\n\nBalise v")); Serial.print(versionSoft); + Serial.println(F(" " __DATE__ " " __TIME__)); + Serial.print(F("GPS: ")); +#ifdef GPS_quectel + Serial.println(F("Quectel")); +#elif defined(GPS_ublox) + Serial.println(F("Ublox")); +#else + Serial.println(F("non défini")); +#endif + Serial.printf_P(PSTR("Pin %i sur RX du GPS\nPin %i sur TX du GPS\n"), GPS_TX_PIN, GPS_RX_PIN); EEPROM.begin(512); checkFactoryReset(); readPreferences(); Serial.println(F("Prefs lues dans EEPROM:")); listPreferences(); + // init liaison GPS. Cette vitesse sera changée en fait lors de la configuration du GPS +#if defined(ESP32) + serialGPS.begin(9600, SERIAL_8N1, GPS_RX_PIN, GPS_TX_PIN ); +#else + serialGPS.begin(9600); + if (!serialGPS) { // If the object did not initialize, then its configuration is invalid + Serial.println(F("Invalid SoftwareSerial pin configuration, check config")); + while (1) { // Don't continue with invalid configuration + delay (1000); + } + } +#endif + local_ip.fromString(preferences.local_ip); gateway.fromString(preferences.gateway); subnet.fromString(preferences.subnet); - -#ifdef ENABLE_BUZZER - pinMode(BUZZER_PIN_G, OUTPUT); // set a pin for buzzer output - digitalWrite(BUZZER_PIN_G, LOW); - pinMode(BUZZER_PIN_P, OUTPUT); // set a pin for buzzer output - buzz(BUZZER_PIN_P, 2500, 100); -#endif - -#ifdef ENABLE_LED - //built in blue LED -> change d'état à chaque envoi de trame - pinMode(led_pin, OUTPUT); - digitalWrite(led_pin, LOW); -#endif - // for (uint8_t t = 4; t > 0; t--) { - // Serial.printf("[SETUP] BOOT WAIT %d...\r\n", t); - // Serial.flush(); - // delay(1000); - // } - //connection sur le terrain à un smartphone // start WiFi - WiFi.mode(WIFI_AP); // +++++++++++++++++++++++++++ FS + WiFi.mode(WIFI_AP); //conversion de l'adresse mac: String temp = WiFi.macAddress(); - temp.replace(":", ""); //on récupère les12 caractères - strcpy(&drone_id[18], temp.c_str()); // quel'onmet a la fin du drone_id + temp.replace(":", ""); //on récupère les 12 caractères + strcpy(&drone_id[18], temp.c_str()); // que on met à la fin du drone_id Serial.print(F("Drone_id:")), Serial.println(drone_id); temp = WiFi.macAddress(); //concat du prefixe et de l'adresse mac temp = String(prefixe_ssid) + "_" + temp; //transfert dans la variable globale ssid temp.toCharArray(ssid, 32); - + if (strlen(preferences.ssid_AP) != 0 ) strcpy (ssid, preferences.ssid_AP); + else strcpy (preferences.ssid_AP, ssid); Serial.print(F("Setting soft-AP configuration ... ")); Serial.println(WiFi.softAPConfig(local_ip, gateway, subnet) ? F("Ready") : F("Failed!")); Serial.print(F("Setting soft-AP ... ")); // ssid, pwd, channel, hidden, max_cnx - Serial.println(WiFi.softAP(ssid, preferences.password, 6, false, 4) ? F("Ready") : F("Failed!")); + Serial.println(WiFi.softAP(ssid, preferences.password, 6, false, 4) ? F("Ready") : F("Failed!")); // 4 clients peris + // Serial.println(WiFi.softAP(ssid, preferences.password, 6, false, 1) ? F("Ready") : F("Failed!")); // 1 client permis Serial.print(F("IP address for AP network ")); Serial.print(ssid); Serial.print(F(" : ")); Serial.println(WiFi.softAPIP()); - WiFi.setOutputPower(20.5); // max 20.5dBm - - softap_config current_config; - wifi_softap_get_config(¤t_config); - current_config.beacon_interval = 1000; - wifi_softap_set_config(¤t_config); - - beginServer(); //lancement du server WEB - -#ifdef GPS_STANDARD - fs_initGPS(); - // softSerial.begin(9600); // ++++++++++ FS : communication à 9600. -#else - //--------------------------------------------- 57600 ->BAUDRATE 9600 - softSerial.begin(GPS_57600); - delay(100); // Little delay before flushing. - softSerial.flush(); - Serial.println("GPS BAUDRATE 9600"); - BaudRate9600(); - delay(100); // Little delay before flushing. - softSerial.flush(); - softSerial.begin(GPS_9600); - delay(100); // Little delay before flushing. - softSerial.flush(); - //-------Config RATE = 500 ms - Serial.println("Configure GPS RATE = 500 ms"); - Rate500(); - delay(100); // Little delay before flushing. - softSerial.flush(); - //--------Config CHANNELS - Serial.println("Configure GPS CHANNELS = GPS + Galileo + Glonas"); - SelectChannels(); - delay(100); // Little delay before flushing. - softSerial.flush(); -#endif + beginServer(); //lancement du server WEB, DNS, file system + delay(1200); // le GPS QUECTEL met environ 1.1s pour être pret après un power up et ne reçoit pas les commandes avant. Ubloxd : environ 600ms + fs_initGPS(preferences.baud, preferences.hz); // init du GPS (vitesse, refresh) et serialGPS drone_idfr.set_drone_id(drone_id); - snprintf(buff[0], sizeof(buff[0]), "ID:%s", drone_id); // on aura tout de suite l'info + #ifdef fs_STAT - resetStatistics(); + razStatistics(); #endif - // pour debug points aberrants - for (int i = 0; i < sizeof(ringBuffer); i++ ) ringBuffer[i] = ' '; - indexBuffer = 0; Serial.println(F("Attente du fix & Co")); - delay(1000); } void loop() { - delay(1); +#ifdef fs_RECEPTEUR + if (modeRecepteur) + { + server.handleClient(); + dnsServer.processNextRequest(); + loopRecepteur(); + return;// doit relancer loop + } +#endif + // Gestion du shutdown du Point Acces Wifi + if (!has_set_home || !countdownRunning) TimeShutdown = millis() + preferences.timeoutWifi * 1000; + if ( preferences.arretWifi && !shutdown && (millis() > TimeShutdown && countdownRunning || vitesse > 2)) { // 2m/S = 7.2km/h + Serial.printf_P(PSTR("!!!!!!! Shutdown. millis:%u TimeShutdown:%u vitesse:%i\n"), millis(), TimeShutdown, vitesse); + // Serial.printf_P(PSTR("WiFi.softAPdisconnect: %s\n"), WiFi.softAPdisconnect(true) ? F("OK") : F("Failed!")); + Serial.printf_P(PSTR("WiFi.mode: %s\n"), WiFi.mode(WIFI_STA) ? F("OK") : F("Failed!")); + // la balise +#ifdef ESP32 + esp_wifi_set_channel(6, WIFI_SECOND_CHAN_NONE); // ESP32 +#else + wifi_set_channel(6); +#endif +#ifdef STAT + razStatistics(); +#endif + delay(100); + shutdown = true; + } else if (!shutdown) { + // AP toujours actif #ifdef fs_STAT - T0Server = millis(); + statServer.T0 = millis(); #endif - server.handleClient(); + server.handleClient(); #ifdef fs_STAT - calculerStat (true, T0Server, dureeMinServer, dureeMaxServer, dureeMoyenneLocaleServer, dureeTotaleServer, - dureeSousTotalServer, countServer); + calculerStat (true, &statServer); #endif - dnsServer.processNextRequest(); - + dnsServer.processNextRequest(); + } // Ici on lit les données qui arrivent du GPS et on les passe à la librairie TinyGPS++ pour les traiter - while (softSerial.available()) { - cGPS = softSerial.read(); - ringBuffer[indexBuffer++] = cGPS; - indexBuffer %= sizeof(ringBuffer); - // pour filtrer quelques erreurs et avoir une chance de faire un checksum error ..... Q&D !! - if ( cGPS > 0x57 || !asciiFilter[cGPS] ) { - // Serial.print(char(cGPS)); - continue; // ignorer à partir des minuscules a etc ... - } + while (serialGPS.available()) { + cGPS = serialGPS.read(); gps.encode(cGPS); - // gps.encode(softSerial.read()); } yield(); + saveLocationIsUpdated = false; + vit_prev = 0; // restera à 0 en cas de problème avec le GPS. Sionon sera mis à jour. + // et on commence par filtrer tous les cas possibles d'erreur // On traite le cas où le GPS a un problème - if (millis() > 5000 && gps.charsProcessed() < 10) { - Serial.println(F("NO GPS")); - strncpy(buff[8], "NO GPS", sizeof(buff[8])); - delay(2000); //pour ne pas encombrer Serial Monitor ... - return; - } - boolean saveLocationIsUpdated = gps.location.isUpdated(); + if (millis() > 5000 && gps.charsProcessed() < 10) + codeinfo = 1; // On traite le cas si la position GPS n'est pas valide - if (!gps.location.isValid()) { - if (millis() - gpsMap > 1000) { - SV = gps.satellites.value(); - Serial.print("Waiting... SAT="); Serial.println(SV); - snprintf(buff[8], sizeof(buff[8]), "ATT SAT %u", SV); - strncpy(buff[10], "ATTENTE", sizeof(buff[10])); - buzz(BUZZER_PIN_P, 2500, 10);//tick - //Flip internal LED - flip_Led(); - gpsMap = millis(); - - } - return; - // } else if (gps.location.age() > 3000) { - } else if (gps.location.age() > 5000) { - // Serial.println("NO SAT"); - strncpy(buff[8], "NO SAT", sizeof(buff[8])); - return; - // } else if (gps.location.isUpdated() && gps.altitude.isUpdated() && gps.course.isUpdated() && gps.speed.isUpdated()) { - // FS FS ++++++++++++++++++++++++++++++++ OU à la place de ET - } else if (gps.location.isUpdated() || gps.altitude.isUpdated() || gps.course.isUpdated() || gps.speed.isUpdated()) { // FS +++++++++++ + else if (!gps.location.isValid() || !gps.date.isValid() ) + codeinfo = 2; + else if (gps.location.age() > 3000) + codeinfo = 3; + // test si précision ok, sinon on remonte au début afin de ne pas encore fixer la home position + else if (gps.satellites.value() < 4) + // La précision du GPS n'est pas bonne + // Nb de satellites (doit être supérieur à 3): " + String(gps.satellites.value())); + codeinfo = 4; + else if (gps.hdop.hdop() > 5) + //La précision du GPS n'est pas bonne + //Précision 2D (doit être inférieure à 5.0): " + String(gps.hdop.hdop())); + codeinfo = 5; + else if (gps.altitude.meters() == 0) + //La précision du GPS n'est pas bonne + //Altitude non récupérée: " + String(gps.altitude.meters())); + codeinfo = 6; + else { // ici on n'est plus en erreur. location/date/age/satellites/hdop/altitude sont OK. saveLocationIsUpdated = gps.location.isUpdated(); - // On traite le cas où la position GPS est valide. - // On renseigne le point de démarrage quand la précision est satisfaisante - if ( gps.satellites.value() > nb_sat ) {//+ de satellites - nb_sat = gps.satellites.value(); - // Serial.print("NEW SAT="); Serial.println(nb_sat); - strncpy(buff[8], "+SAT", sizeof(buff[8])); - } - if ( gps.satellites.value() < nb_sat ) {//- de satellites - nb_sat = gps.satellites.value(); - // Serial.print("LOST SAT="); Serial.println(nb_sat); - strncpy(buff[8], "-SAT", sizeof(buff[8])); - } - - if (!drone_idfr.has_home_set() && gps.satellites.value() > limite_sat && gps.hdop.hdop() < limite_hdop) { - - Serial.println(F("Setting Home Position")); - HLat = gps.location.lat(); HLng = gps.location.lng(); - latLastLog = HLat; lngLastLog = HLng; // FS +++++++++++++++++++++++++++++++++++ - drone_idfr.set_home_position(HLat, HLng, gps.altitude.meters()); - altitude_ref = gps.altitude.meters(); - snprintf(buff[11], sizeof(buff[11]), "DLNG:%.4f", HLng); - snprintf(buff[12], sizeof(buff[12]), "DLAT:%.4f", HLat); - strncpy(buff[10], "DEPART", sizeof(buff[10])); - VMAX = 0; - buzz(BUZZER_PIN_P, 7000, 200); - delay (50); - buzz(BUZZER_PIN_P, 7000, 200); - delay (50); - buzz(BUZZER_PIN_P, 7000, 200); - } - - // On actualise les données GPS de la librairie d'identification drone. - drone_idfr.set_current_position(gps.location.lat(), gps.location.lng(), gps.altitude.meters()); - drone_idfr.set_heading(gps.course.deg()); - drone_idfr.set_ground_speed(gps.speed.mps()); - - //************************************************************************** - //Calcul VMAX et renseigne les datas de la page WEB - if (VMAX < gps.speed.mps()) { - VMAX = gps.speed.mps(); - } - snprintf(buff[1], sizeof(buff[1]), "UTC:%d:%d:%d", gps.time.hour(), gps.time.minute(), gps.time.second()); - _sat = gps.satellites.value(); if (_sat < limite_sat) { - snprintf(buff[2], sizeof(buff[2]), "--SAT:%u", _sat); - } else { - snprintf(buff[2], sizeof(buff[2]), "SAT:%u", _sat); - } - _hdop = gps.hdop.hdop(); if (_hdop > limite_hdop) { - snprintf(buff[3], sizeof(buff[3]), "++HDOP:%.2f", _hdop); - } else { - snprintf(buff[3], sizeof(buff[3]), "HDOP:%.2f", _hdop); - } - snprintf(buff[4], sizeof(buff[4]), "LNG:%.4f", gps.location.lng()); - snprintf(buff[5], sizeof(buff[5]), "LAT:%.4f", gps.location.lat()); - snprintf(buff[6], sizeof(buff[6]), "ALT:%.2f", gps.altitude.meters() - altitude_ref); - snprintf(buff[7], sizeof(buff[7]), "%s VMAX(km/h):%.2f", messAlarm.c_str(), float (VMAX * 3.6)); - - //************************************************************* - + if (!has_set_home) + // On renseigne une première et unique fois le point de démarrage dès que la précision est satisfaisante + { + Serial.println(F("Stockage de la position de départ")); + home_alt = gps.altitude.meters(); + //on fixe la position home: + drone_idfr.set_home_position(gps.location.lat(), gps.location.lng(), home_alt); + has_set_home = true; + latLastLog = gps.location.lat(); + lngLastLog = gps.location.lng(); + //on envoie une trame + codeinfo = 7; + } else + codeinfo = 9; + + // si ici, pas d'erreur donc: + // on a un codeinfo 7 si on vient de definir la home position, ou 9 en fonctionnement normal + //on stocke les dernières positions + lat_prev = gps.location.lat(); + lon_prev = gps.location.lng(); + alt_prev = gps.altitude.meters(); + dir_prev = gps.course.deg(); + vit_prev = gps.speed.mps(); // metre /sec + vitesse = vitesse * 0.8 + vit_prev * 0.2; // un peu de lissage + //vitesse = vit_prev; + VmaxCockpit = max(VmaxCockpit, float(vit_prev * 3.7)); + AltmaxCockpit = max(AltmaxCockpit, float(alt_prev)); + VmaxSegment = max(VmaxSegment, float(gps.speed.kmph())); + // AltMaxSegment = max(AltMaxSegment, float(alt_prev)); + AltMaxSegment = max(AltMaxSegment, float(gps.altitude.meters())); // alt_prev est un entier double... } - - - /** - On regarde s'il est temps d'envoyer la trame d'identification drone : - - soit toutes les 3s, - - soit si le drone s'est déplacé de 30m, - - uniquement si la position Home est déjà définie, - - et dans le cas où les données GPS sont nouvelles. - */ - - // ATTENTION: ne pas appeler 2 fois drone_idfr.time_to_send !! - if (drone_idfr.has_home_set() && drone_idfr.time_to_send()) { - float time_elapsed = (float(millis() - beaconSec) / 1000); - beaconSec = millis(); - /* - Serial.print(time_elapsed,1); Serial.print("s Send beacon: "); Serial.print(drone_idfr.has_pass_distance() ? "Distance" : "Time"); - Serial.print(" with "); Serial.print(drone_idfr.get_distance_from_last_position_sent()); Serial.print("m Speed="); Serial.println(drone_idfr.get_ground_speed_kmh()); - */ - /** - On commence par renseigner le ssid du wifi dans la trame - */ - // write new SSID into beacon frame - const size_t ssid_size = (sizeof(ssid) / sizeof(*ssid)) - 1; // remove trailling null termination - beaconPacket[40] = ssid_size; // set size - memcpy(&beaconPacket[41], ssid, ssid_size); // set ssid - const uint8_t header_size = 41 + ssid_size; //TODO: remove 41 for a marker - /** - On génère la trame wifi avec l'identification - */ - const uint8_t to_send = drone_idfr.generate_beacon_frame(beaconPacket, header_size); // override the null termination - - /** - On envoie la trame - */ -#ifdef fs_STAT - T0SendPkt = millis(); -#endif - if (wifi_send_pkt_freedom(beaconPacket, to_send, 0) != 0) // 0 sucess, -1 erreur - Serial.println(F("--Err emission trame beacon")); - -#ifdef fs_STAT - calculerStat (true, T0SendPkt, dureeMinSendPkt, dureeMaxSendPkt, dureeMoyenneLocaleSendPkt, dureeTotaleSendPkt, - dureeSousTotalSendPkt, countSendPkt); - //calcul de stat. Attention la fonction incrmente déjà le compteur - calculerStat (true, T0Beacon, entreBeaconMin, entreBeaconMax, entreBeaconMoyenneLocale, entreBeaconTotal, entreBeaconSousTotal, TRBcounter); - TRBcounter--; - T0Beacon = millis(); -#endif - //Flip internal LED - flip_Led(); - //incrementation compteur de trame de balise envoyé - TRBcounter++; - snprintf(buff[9], sizeof(buff[9]), "TRAME:%u", TRBcounter); - - _secondes = millis() / 1000; //convect millisecondes en secondes - _minutes = _secondes / 60; //convertir secondes en minutes - _heures = _minutes / 60; //convertir minutes en heures - _secondes = _secondes - (_minutes * 60); // soustraire les secondes converties afin d'afficher 59 secondes max - _minutes = _minutes - (_heures * 60); //soustraire les minutes converties afin d'afficher 59 minutes max - - snprintf(buff[13], sizeof(buff[13]), " %dmn:%ds", _minutes, _secondes ); - - /** - On reset la condition d'envoi - */ - drone_idfr.set_last_send(); - } - + // Tout est bon si codeinfo =9 ou 7. Sinon envoyer une trame avec les coordonnées &Co précédentes + // (sauf pour la vitesse qui a été mise à 0 au début, pour ne pas avoir de problème avec l'estimation de distance parcourue + // dans has_pass_distance() ) + // + EnvoiTrame(lat_prev, lon_prev, alt_prev, dir_prev, vit_prev); // envoie éventuel. Autres conditions testées dans la fonction // on regarde si on doit enregistrer un point dans la trace - if (drone_idfr.has_home_set() && !preferences.logNo && (saveLocationIsUpdated || preferences.logAfter < 0) && !fileSystemFull ) { + // IL faut au moins avoir eu un fix, des nouvelles données. + + if ( preferences.logOn && drone_idfr.has_home_set() && !fileSystemFull ) { float segmentf = distanceSimple(gps.location.lat(), gps.location.lng(), latLastLog, lngLastLog); - // rejet de points abberrants (problème de communication avec le GPS ...) - if ( gps.location.lat() == 0 || gps.location.lng() == 0 || (preferences.logAfter > 0) && segmentf > 30 * preferences.logAfter) { - // point GPS abberrant: enregistrer l'erreur et l'ignorer ... - // N'enregistrer que une fois le message, en début de rafale . - // if ( gps.time.value() != timeLogError) { // on suppose que gps.time.value() n'est pas aussi faux que gps.location()... - // logRingBuffer( ringBuffer, sizeof(ringBuffer), indexBuffer); - // logError(gps, latLastLog, lngLastLog, segmentf); - // timeLogError = gps.time.value(); - // } + // Si le fix existe :rejet de points abberrants (problème de communication avec le GPS ...). Surtout pour ESP8266 + if (drone_idfr.has_home_set() && ( gps.location.lat() == 0 || gps.location.lng() == 0 || (preferences.logAfter > 0) && segmentf > 30 * preferences.logAfter)) { + // point GPS aberrant: enregistrer l'erreur et l'ignorer ... #ifdef fs_STAT nbrLogError++; + // Serial.printf_P(PSTR("%f %f %f %f %f\n"), segmentf, gps.location.lat(), gps.location.lng(), latLastLog, lngLastLog); #endif } else { // point GPS OK // fermer le fichier trace si on ne bouge plus: avion posé ?, on risque la coupure de courant - if (gps.speed.kmph() <= 0.01) { // baddddddddddddddddddddddddd ?????????????????? +++++++++++++++++ - if ( immobile && millis() - T0Immobile > 2000 && preferences.logAfter > 0 && traceFileOpened ) { - //a l'arret depuis plus de 2000s.: fermer le fichier trace. Sauf si en mode test avec preferences.logAfter <= 0) + if (vitesse <= 0.1) { // ??? 0.36km/h . En fait Default speed threshold: 0.4 m/s + // if ( immobile && millis() - T0Immobile > 2000 && preferences.logAfter > 0 && traceFileOpened ) { // fermer si mode distance uniquement + if ( immobile && millis() - T0Immobile > 2000 && traceFileOpened ) { // fermer dans tous les cas + //a l'arret depuis plus de 2000s.: fermer le fichier trace. traceFile.close(); traceFileOpened = false; - // Serial.println(F("-----------close main")); - } else if (!immobile) { + // Serial.print(F("-----------close tracefile vitesse:")); Serial.println(vitesse); + } else if (!immobile) { // début immobilité immobile = true; T0Immobile = millis(); } - } else { + } else { // fin immobilité immobile = false; } // Si preferences.logAfter > 0 :On stocke un point de trace si le nouveau segment et plus long que preferences.logAfter - // Si preferences.logAfter < 0 : pour des tests. On ne tient plus compte de la longeur du segment, mais on stocke + // Si preferences.logAfter < 0 :On ne tient plus compte de la longeur du segment, mais on stocke // un point de trace toutes les abs(preferences.logAfter) ms ( il faut au moins 70ms ??) - if (((preferences.logAfter > 0) && (segmentf > preferences.logAfter)) || ((preferences.logAfter < 0) && (millis() - T1Log > abs (preferences.logAfter)))) { - T0Log = millis(); + // et uniquementsi on s'est un peu déplacé depuis le dernier log ou si l'option logToujours est true. + if (((preferences.logAfter > 0) && (segmentf > preferences.logAfter)) + || ((preferences.logAfter < 0) && (millis() - T1Log > abs (preferences.logAfter) ) + && (segmentf > 1.0 || preferences.logToujours) ) ) { + // Serial.print(F("Demande ecr trace. segmentf: ")); Serial.println(segmentf); + T1Log = millis(); +#ifdef fs_STAT + statLog.T0 = millis(); +#endif if ( !fs_ecrireTrace (gps)) { Serial.println(F("File system full ? ")); fileSystemFull = true; @@ -690,31 +724,24 @@ void loop() fileSystemFull = false; messAlarm = ""; #ifdef fs_STAT - calculerStat (true, T0Log, dureeMinLog, dureeMaxLog, dureeMoyenneLocaleLog, dureeTotaleLog, - dureeSousTotalLog, countLog); - countLog--; // même compteur pour durée d'un log et nbr de segment - calculerStat (false, segment, segmentMin, segmentMax, segmentMoyenneLocale, segmentTotal, - segmentSousTotal, countLog); - countLog--; + calculerStat (true, &statLog); + statSegment.T0 = segmentf; + calculerStat (false, &statSegment); #endif countLog++; + if (countLog % 300 == 0) { - // Serial.println(F("-- Flush")); - traceFile.flush(); // forcer la mise a jour tous les 100 points + traceFile.flush(); // forcer la mise a jour tous les n points } - latLastLog = gps.location.lat(); - lngLastLog = gps.location.lng(); - // stats sur une tour complet de la boucle -#ifdef fs_STAT - calculerStat (true, T0Loop, perMinLoop, perMaxLoop, perMoyenneLocaleLoop, perTotaleLoop, - perSousTotalLoop, countLoop); -#endif } - T1Log = millis(); + latLastLog = gps.location.lat(); + lngLastLog = gps.location.lng(); } } } // test si il faut ecrire un point #ifdef fs_STAT - T0Loop = millis(); + // stats sur une tour complet de la boucle + calculerStat (true, &statLoop); + statLoop.T0 = millis(); #endif } diff --git a/README.md b/README.md index 6b9b337..7f6fc71 100644 --- a/README.md +++ b/README.md @@ -1,98 +1,183 @@ -# BaliseDGAC_GPS_Logger -Version d'une balise de signalisation style DGAC pour drone et aéromodélisme avec enregistrement des traces. -[Voir ici les principales modifications par rapport à la version 1](#principales-modifications-par-rapport-à-la-version-1) -# Balise avec enregistrement de traces -Cette version de la balise DGAC de signalement électronique à distance pour drone et aéromodélisme est basée sur la version **GPS_Tracker_ESP8266V1_WEB** de "dev-fred" disponible à https://github.com/dev-fred/GPS_Tracker_ESP8266 . - -La réalisation a été faite avec un ESP01 (ESP8266) et un GPS QUECTEL L80 ce qui donne une réalisation très compacte, mais bien d'autres possibilités existent avec par exemple un ESP8266 D1, un GSP BN220 etc... -La consommation de cette configuration varie de 80 à 90mA. - -## Principales caractéristiques: -- **interface Web** -- Ajout d'une fonction d'**enregistrement des traces** format **CSV/GPX** dans le système de fichiers de l'ESP avec interface Web de gestion (effacer / télécharger / choix des champs / conditions d'enregistrement). -- Modification de l'**identificateur de la balise**: l'adresse MAC est utilisée comme numéro de série. -- Ajout d'une fonction de mise à jour du logiciel à travers la liaison WiFI (**OTA** Over The Air) -- Ajout d'un **portail captif**: lors de la connexion au réseau créé par la balise le navigateur est lancé et on se retrouve directement dans l'interface utilisateur, sans besoin de donner une adresse IP -- Interface utilisateur pour la **gestion des préférences**, la gestion "système" etc … - -| ![](/img/cockpit_LI.jpg) | ![](/img/traces.png) | +Ceci est une version beta. Les remarques sont les bienvenues ! + +# **BaliseDGAC\_GPS\_Logger V3 Emetteur/Récepteur** +Version d'une balise de signalisation style DGAC pour [signalisation de drones et aéromodèles](https://www.ecologie.gouv.fr/sites/default/files/notice_signalement_electronique.pdf) avec possibilité d'enregistrement des traces GPS. +La balise a deux modes de fonctionnement: +- Mode émetteur +- Mode récepteur pour contrôler le fonctionnement des balises du voisinage + +| | | +| ------------ | ------------ | +|Balise réalisée avec un module ESP32-C3 T-01C3. Poids: 11g| [Quelques photos de la réalisations](/realisation.md)| + +## **Crédit:** +Le cœur du logiciel qui transmet la trame spécifique d’identification à distance pour drones et aéromodèles est basé sur la version [GPS\_Tracker\_ESP8266V1\_WEB](https://github.com/dev-fred/GPS_Tracker_ESP8266) de "dev-fred" ainsi que sur les travaux de ["Tr@nquille"](https://www.tranquille-informatique.fr/modelisme/divers/balise-dgac-signalement-electronique-a-distance-drone-aeromodelisme.html) +Les parties interface WEB et enregistrement de traces ont été rajoutées. + + +## **Principales caractéristiques:** +- Génération des signaux de signalisation électronique pour les aéromodèles, suivant les prescriptions de l'[arrêté du 27 décembre 2019](https://www.legifrance.gouv.fr/jorf/id/JORFTEXT000039685188) (loi drone …). +- Mode émetteur ou récepteur. +- Code compatible ESP32/ ESP32-C3 / ESP8266. +- Interface Web accessible sur un point d'accès (AP) créé par la balise. Gestion et contrôle du bon fonctionnement de la balise. Gestion des préférences … +- Portail captif: lors de la connexion au réseau créé par la balise le navigateur est lancé et on se retrouve directement dans l’interface utilisateur, sans besoin de donner une adresse. +- Possibilité de coupure du point d’accès pour ne pas interférer avec les signaux radio de télécommande et limiter fortement la consommation de la balise. +- Fonction d’enregistrement des traces GPS dans le système de fichiers de l’ESP avec interface Web de gestion. Téléchargement de traces en format CSV et/ou GPX. +- Fonction de mise à jour du logiciel à travers la liaison Wi-Fi (OTA Over The Air). + +Cette balise peut être utilisée en dehors du contexte signalisation d'aéromodèles pour faire par exemple des tests de vitesse lors de la mise au point de mobiles, de bateaux du type racers/offshore, de modèles de voitures RC etc …[Exemple ici](#scenario) + +## **Matériel supporté** +**Microcontrôleurs supportés:** +- ESP8266 (par exemple module ESP01) +- ESP32 +- ESP32-C3 (par exemple module TTGO T-01C3 ESP32-C3) + +**Modules GPS supportés:** +- Quectel L80 (et GPS style base chipset:MediaTek MT3339 ??) +- Beitian BN-220, BN-180, BN-880 (et GPS style base chipset: u-blox M8030-KT ??) + +Le choix d'un module **LILYGO® TTGO T-01C3 ESP32-C3** ayant les mêmes dimensions et brochage qu'un ESP01 mais basé sur un ESP32-C3 permet une réalisation compacte et performante. Par rapport à un ESP01 classique, ce module dispose de plus de mémoire (4MB), de plus de puissance de traitement, d'un LED indépendant, d'une entrée/sortie supplémentaire, d'un connecteur pour une antenne externe optionnelle, etc.… Il semble aussi moins sensible aux problèmes d'alimentation que le module ESP01/ESP8266. +Bien d’autres possibilités existent avec par exemple un ESP8266 D1, un GPS BN220 etc. + +## **Message d'identification** +Dès que la balise est sous tension, même en l’absence d’un fix GPS, la balise émet des messages d'identification conforme à l'arrêté du 27 décembre 2019. +Le format de l'identifiant diffusé est le suivant: +- Un code sur 3 caractères, sensé représenter le trigramme constructeur. + Il doit être **obligatoirement** 000 pour une construction amateur "DIY" +- Un code sur 3 caractères, sensé représenter le modèle de la balise +- Un code sur 24 caractères, sensé représenter le numéro de série de la balise. + +Il est donc du genre: "000FSB000000000000YYYYYYYYYYYY" +Le logiciel remplace les 12 derniers caractères par l'adresse MAC de la balise assurant l'unicité de l'identifiant. +L'interface utilisateur affiche l'identifiant de la balise qui devra être enregistré sur le site AlphaTango. + + +## **Environnement logiciel. Compilation** +Les tests ont été faits dans l'environnement IDE Arduino 18.19. +Il est impératif d'avoir les environnements les plus récents pour ESP8266 et ESP32. (Février 2022: ESP32 2.0.2, ESP8266 3.0.2) +Seule la librairie TinyGPS++ ne fait pas partie des packages standards ESP32/ESP8266 et doit être installée. + +Avant de compiler il faut choisir quelques options dans le fichier **fs\_option.h** (choix du GPS, choix des ports de communication pour le GPS, choix d’inclure ou non la mise à jour par OTA, la disponibilté d'un LED accessible dans le montage, etc. …). Voir les commentaires. +Le mode "récepteur" n'est pas supporté pour l'ESP8266. +Le choix du type de processeur est fait lors de la compilation en sélectionnant le bon type de carte dans l'IDE Arduino + +### **Modules GPS** + +Choisir dans le fichier fs\_options.h une des lignes : + +\#define GPS\_quectel // style Quectel L80 et GPS style base chipset MediaTek MT3339 +ou +\#define GPS\_ublox // pour Beitian BN-220, BN-180, BN-880 et GPS style base chipset u-blox M8030-KT. + +Le logiciel a été testé avec un GPS QUECTEL L80 et un Beitian BN-880 (dont la partie GPS est compatible avec un BN-180,BN-220, BN-280) +Les GPS qui utilisent les commandes style $PMTK251, $PMTK220, $PMTK314 (cas de Quectel, GlobalTop/Sierra Wireless, …) peuvent sûrement être utilisés. + +### **Utilisation d'un LED** +Si un LED est donné dans la configuration par */#define pinLed xx* (voir fichier fs\_option.h) son clignotement est rythmé par l'émission des trames d'identification. +- En absence de fix GPS: clignotement lent, période de 6 secondes. +- Après un fix GPS: flash très rapide lors de l'envoi d'une trame. (le flash est un peu plus long si la balise est en mode économie d'énergie/mise en sommeil) + +# **Mode Emetteur** +Cest le mode par défaut de la balise lors de sa mise sous tension. Les trames d’identification sont émises. Une interface Web permet son contrôle après connexion au point d’accès créé par la balise (système de portail captif) + +## **Fenêtre « Cockpit »** + + +Elle affiche l’état général de la balise et permet de contrôler son bon fonctionnement. +La vitesse maximale ainsi que la hauteur maximale atteintes depuis la mise sous tension sont aussi affichées ainsi que le nombre de trames d'identification émises et le nombre de points GPS enregistrés depuis le dernier démarrage de la balise. +
+ +## **Fenêtre « Trace »** + +| | | +| ------------ | ------------ | + +La fenêtre « Trace » permet la gestion des traces GPS enregistrées. +Des couleurs mettent en évidence le fichier le plus ancien, le plus gros et le plus récent. +Un click sur le nom d’un fichier ouvre une fenêtre donnant les caractéristiques principales de la trace (Nombre de points, heure de début/fin, vitesse maximum, hauteur maximum …) +Un fichier peut être téléchargé localement pour analyse fine. + +**Attention :** dans certains cas (sous Android ?), le navigateur ouvert automatiquement lors de la connexion au portail captif ne permet pas de faire des téléchargements de fichiers. Il faut alors utiliser explicitement le navigateur standard. Ceci ne se produit pas sur un PC Windows. + +## **Fenêtre « Préférences »** + +Cette fenêtre permet de choisir le format des traces GPS, la configuration du GPS (vitesse/rafraîchissement), la gestion du point d’accès Wi-Fi. +
+ +### **Format de la trace GPS** +On peut choisir d'enregistrer ou non une trace GPS. +La création d’une trace ne commence qu’après l’obtention d’un fix GPS et l'enregistrement d'un nouveau point de trace est déclenché (Choix exclusif): +- Soit par une distance parcourue supérieure à X mètres +- Soit par un temps écoulé de T millisecondes depuis le dernier point enregistré. + +Les traces sont enregistrées dans un format neutre et le choix du format GPX/CSV n'est pris en compte que lors du téléchargement: le même fichier peut donc être chargé une fois en format GPX, une fois en format CSV. + +Les traces CSV sont plus faciles à analyser dans Excel par exemple et un site comme [GEO JAVAWA](https://www.geo.javawa.nl/trackanalyse/index.php?lang=en) permet une analyse fine, segment par segment de traces GPX. On peut importer ces fichiers CSV dans [Google Maps](https://www.google.fr/maps) pour visualisation: Google Maps/Menu/Vos adresses/Cartes/Créer une carte/Importer ou les transformer en fichier GPX, KML,… avec par exemple [GPSVisualizer](https://www.gpsvisualizer.com/)., etc ... + +### **Configuration du GPS** +La vitesse de transmission du GPS et sa fréquence de rafraîchissement sont configurables (9600/19200/38400/57600 bds, 1/3/5/7/10 Hz). + +La vitesse de transmission doit être augmentée si une fréquence de rafraîchissement importante est choisie. + +Si on veut enregistrer une trace fine, avec des points proches, il est nécessaire de choisir une fréquence de rafraîchissement élevée (par exemple si on utilise la balise pour une recherche de vitesse maximum sur de courtes périodes de temps). Le choix 19200Bds et 10Hz est un bon compromis. + +A noter que si on utilise un ESP8266, la liaison série avec le GPS est entièrement émulée par le logiciel avec parfois des problèmes de qualité de transmission pour des vitesses élevées. Avec un ESP32, la liaison est gérée bien plus efficacement par le matériel + +### **Gestion du point d'accès Wi-Fi** +A la mise sous tension, la balise crée un point d'accès Wi-Fi ouvert dont le nom par défaut est BALISE\_adresse-Mac (genre BALISE\_60:55:F9:71:59:5C) + +Dès la connexion au réseau réalisée, un navigateur est ouvert et donne accès à l'interface utilisateur de gestion de la balise. Ce portail captif fonctionne très bien sous Windows mais est parfois un peu plus capricieux sous Android et il peut être nécessaire de donner au navigateur une adresse quelconque du genre http://xx.fr pour accéder à l'interface utilisateur !!. Voir aussi les problèmes potentiels lors du [téléchargement](#warning1) de traces GPS) + +Il est possible de modifier le nom du réseau et de le protéger par un mot de passe. Si après modification du nom du réseau on veut rétablir son nom par défaut il suffit d'effacer le champ "Nom du réseau" et de soumettre cette modification. Il n'est pas nécessaire de se souvenir de l'adresse MAC de la balise. + +Il est possible de couper le point d'accès en fonctionnement. +Si la coupure est autorisée elle interviendra après un délai suivant l'obtention d'un fix GPS ou si la vitesse de déplacement de la balise est supérieure à 2m/s (7.2km/h) +Si l'interface utilisateur est utilisée, ce délai commencera à courir uniquement lorsque la page "Cockpit" de l'interface sera affichée. + +Il est possible de couper complètement l'activité Wi-Fi de la balise entre deux émissions de la trame d'identification. Ceci permet éventuellement une réduction des interactions entre les équipements de télécommande et la balise et **permet surtout d'abaisser fortement la consommation du dispositif**. + +Il est à noter que la mise en sommeil de la balise entraîne un retard de l'ordre de 20ms entre le moment où la trame doit être émise et son émission réelle. + +**Remarque :** Dans le cas d’une balise construite avec un ESP8266, il est important d’utiliser une version du package de base ESP8266 supérieure à 3.0.0 car sinon ce retard atteint 300ms ou plus, ce qui peut être rédhibitoire si on veut en même temps enregistrer des traces GPS avec une haute résolution … + + +### **Bouton « Reset » :** +Redémarre la balise +### **Bouton « Formatage » :** +Réinitialise le système de gestion de fichiers +### **Bouton « Maj OTA »** +Permet une mise à jour très aisée du logiciel par la liaison Wi-Fi (au détriment de la mémoire réservée pour les fichiers traces, ce qui peut éventuellement être un problème avec un module style ESP8266 avec peu de mémoire …) +Pour une mise à jour, il suffit de sélectionner le fichier résultat de compilation qui est en général dans + C:\Users\XXXXX\AppData\Local\Temp\arduino\_build\_YYYYYY\le\_projet.ino.bin +(Sélectionner le répertoire arduino\_build\_... qui a la date de modification la plus récente) + + +# Mode Récepteur +| | | | ------------ | ------------ | -Si il y a plus de 4 fichiers de traces, le logiciel efface automatiquement les fichiers les plus anciens quand la place manque dans la mémoire. Le fichier le plus ancien, le plus récent et le plus volumineux sont mis en valeur. -La page "**Péférences**" permet de choisir le format de téléchargement des traces **CSV** et/ou **GPX** et la même trace peut être téléchargée dans les 2 formats. Les traces CSV sont plus faciles à analyser dans Excel par exemple, alors qu'un site comme [GEO JAVAWA](https://www.geo.javawa.nl/trackanalyse/index.php?lang=en) permet une analyse fine, segment par segment de traces GPX. -On peut importer ces fichiers CSV dans Google Maps pour visualisation: - Google Maps/Menu/Vos adresses/Cartes/Créer une carte/Importer -ou les transformer en fichier GPX, KML,... avec par exemple [GPSVisualizer](https://www.gpsvisualizer.com/) - -L'enregistrement des points de trace ne se fait que lorsque la balise est en mouvement et est totalement décorrélé de l'émission des trames d'identification. -La page "**Préférences**" permet de choisir la distance minimale qui provoque l'enregistrement, dès que le fix GPS est fait. -La vitesse de transmission du GPS et sa fréquence de rafraichissement sont aussi sélectionnées sur cette page (38400Bds et 10Hz conseillés) - -… - -![](/img/preferences.png) - -Pour l'aspect WiFi, par défaut le réseau est ouvert (pas de mot de passe) et l'adresse IP est 192.168.4.1 -Le portail captif permet une connexion aisée, sans le besoin de donner l'adresse IP (Fonctionne très bien sous Windows avec Firefox, Chrome, Edge. Est un peu plus capricieux sous Android où il suffit de donner une adresse pouvant être valide comme xx.fr !!) -Le bouton ***Reset*** redémarre la balise et ***Reset Usine*** restaure les préférences à leurs valeurs par défaut. -***Format*** réinitialise le système de gestion de fichiers. -***OTA*** permet une mise à jour du logiciel par la liaison WiFi. -![](/img/OTA.png) -## Gestion de la trace -Chaque point de trace occupe 20 octets en mémoire, ce qui permet d'enregistrer environ 12000 points dans une partition filesystem de 256Ko. Avec un modèle volant à 20m/s (soit environ 70km/h) et en choisissant d'enregistrer 1 point pour chaque déplacement de 10m on pourra donc enregistrer environ 1h30 de vol. -Le GPS met à jour ses mesures au maximum à 10hz soit toutes les 0.1s ce qui donne un enregistrement de 20mn seulement dans les mêmes conditions de vitesse si on choisit d'enregistrer la trace tous les 2m. -Pour un modèle volant au dessus de 50km/h (14 m/s) il est illusoire de vouloir enregistrer une trace avec des points distants de 1m ..... - -Si besoin est, pour quelques dizaines de centimes il est possible d'augmenter la mémoire d'un ESP8266 jusqu'à 16mb ;-) - - ## Compilation -- Avant de compiler il faut choisir quelques options dans le fichier fs_option.h(choix des pins IO pour le GPS, choix d'inclure ou non la mise à jour par OTA, vitesse/gestion GPS, génération d'une page [statistiques](#génération-de-statistiques) etc ...) -- Le fichier compilé avec option OTA occupe environ 373Kb et si on veut maximiser la place laissée au système de gestion de fichiers on peut choisir dans l'IDE Arduino, pour une module ESP01 une map mémoire FS 256Kb/OTA 375kb -Avec les options OTA et STA (statistiques) le fichier compilé occupe environ 378Kb et il faut choisir une map mémoire FS 192kb/OTA 406kb -Outil/Type de Carte "Generic ESP8266 Module" Flash Size 1MB(FS:256KB OTA ¨375KB) ce qui permet d'enregistrer plusieurs heures de vols. -Sans OTA on peut choisir un filesystem de 512KB -- Pour un premier chargement du programme il est conseillé d'utiliser l'option Outils/Erase Flash: "All Flash Contents" -- Les librairies LittleFS, DNSServer, EEPROM sont installées en même temps que le SDK ESP8266. -## Modules GPS -Le logiciel a été testé avec un GPS QUECTEL L80 dont la gestion est principalement assurée dans le fichier fs_GPS.cpp. Des GPS qui utilisent les commandes style $PMTK251, $PMTK220, $PMTK314 (cas de Quectel, GlobalTop/Sierra Wireless, ...) peuvent être utilisés directement. Les modules Beitian demandent sûrement une adaptation plus complexe. -Si le GPS a une configuration connue et satisfaisante lors d'un cold start, il suffit de valider les 2 premières lignes dans la fonction fs_initGPS(). On perdra alors la possibilité de choisir dynamiquement la vitesse et la fréquence de rafraichissement. - -## Problèmes connus - - - Il peut arriver (rarement) quue les trames d'identification ne soient plus émises, alors que - le fix GPS est normal: le programme semble attendre (parfois - plusieurs dizaines de secondes) quelque part dans le code des - librairies spécifiques de l'ESP.... A approfondir ... - - La communication entre le GPS et le processeur via SoftwareSerial - n'est pas très robuste et parfois des erreurs de transmissions ne - sont pas détectées, ce qui donne des points GPS aberrants conduisant - à des calculs de longueur de segment parcourus faux et donc à des - points de trace faux. [Voir TinyGPSPlus issue#87](https://github.com/mikalhart/TinyGPSPlus/issues/87) - -## Principales modifications par rapport à la version 1 - - - téléchargement de traces en format CSV et/ou GPX. - - enregistrement de la trace décorrélé de l'émission des trames d'identification. - - enregistrement des points de traces uniquement dicté par le déplacement de la balise. - - choix possible de la vitesse de transmission du GPS et de sa fréquence de mise à jour (34500bds, 10hz recommandés). - - au moins 4 fichiers traces sont gardés en mémoire. - - option pour générer une page de statistiques pour analyser le comportement du logiciel. -## Génération de statistiques -Surtout intéressante dans un phase de développemnt/validation, l'option **fs_STAT** disponible dans le fichier **fs_options.h** permet de générer une page Web supplémentaire et de suivre en temps réel des statistiques sur le fonctionnement de la balise: - - nombre de trames GPS correctes/incorrectes - - nombre de points traces enregistrés - - durée / période d'exécution de certaines parties du logiciel. - - ... - -En phase de tests, il est possible de choisir une longueur de segment maximum négative à partir de la page de **Préférences**. Cette valeur devient alors en fait la période d'enregistrement de la trace: -200 donnera un point de trace toutes les 200ms. - Dans tous les cas le fix GPS doit être validé. Ceci permet d'explorer rapidement "sur la table" des choix vitesse / rafraichissement GPS, de voir l'évolution du taux d'erreurs, des temps d’exécution etc ... - -# Enjoy ! Les commentaires sont les bienvenus. -#### Idées de développements futurs. - -- nettoyage du code. Limiter l'utilisation de "string". -- revoir la partie condition de génération de la trame d'identification. -- Arrêt du webserver /AP après N minutes ?? pour ne pas interférer avec le 2.4G de la télécommande ?? -- portage sur ESP32 quand la librairie littleFS sera disponible. -- ??? +Le mode récepteur permet de contrôler le fonctionnement des balises actives du voisinage. Une page liste les identifiants des balises les plus actives. + Un click sur un identifiant de balise ouvre une page contenant des détails sur les valeurs émises. + Le retour en mode « Emetteur » doit obligatoirement se faire en utilisant le bouton « Retour en mode émetteur » ou par un redémarrage complet de la balise (mise hors tension /en tension). Ne pas utiliser le bouton « retour » du navigateur Web. + +
+ + # Scénario d'utilisation de la balise + +Cette balise peut être utilisée en dehors du contexte signalisation d'aéromodèles pour faire par exemple des tests de vitesse lors de la mise au point de mobiles, de bateaux du type racers/offshore, de modèle de voitures RC etc … +Scénario dutilisation: +- Ne pas permettre la coupure du point d’accès Wi-Fi +- Choisir une fréquence de rafraîchissement GPS élevée ( 19200bds / 10Hz) +- Choisir éventuellement de créer un fichier trace en prenant des points de mesure rapprochés (Un délai de 100ms entre points correspondants à la fréquence max de rafraîchissement du GPS) +- Faire un essai et faire revenir le mobile +- Se reconnecter au point d’accès créé pas la balise. +- Analyser la vitesse max rapportée dans la page « Cockpit » +- Remettre à zéro ces valeurs +- Changer les réglages et refaire un essai. +- Etc … + +Les traces GPS enregistrées permettent de retrouver un historique des essais. + +Enjoy !:blush: diff --git a/droneID_FR.h b/droneID_FR.h index a9e109d..9d27e2b 100644 --- a/droneID_FR.h +++ b/droneID_FR.h @@ -11,11 +11,12 @@ You should have received a copy of the GNU General Public License along with this program. If not, see . - */ +*/ /* - * version 1 https://discuss.ardupilot.org/t/open-source-french-drone-identification/56904/98 - * - */ + Initial version by Pierre Kancir on May 2020 on funding by Airbot Systems + yéyé Tr@nquille xav a ajouté ses trucs aussi, notamment le codeinfo + version 4.0 +*/ #pragma once @@ -23,351 +24,459 @@ #include /** - * Cette class implemente le système d'identification numérique des drones français conformément à - * This class implement the french drone identification frame as stated in - * https://www.legifrance.gouv.fr/eli/arrete/2019/12/27/ECOI1934044A/jo/texte - */ -class droneIDFR { -public: - /** - * Constructeur de la librairie. Utilise une valeur par default pour l'ID du drone - */ + Cette class implemente le système d'identification numérique des drones français conformément à + This class implement the french drone identification frame as stated in + https://www.legifrance.gouv.fr/eli/arrete/2019/12/27/ECOI1934044A/jo/texte +*/ +class droneIDFR +{ + public: + /** + Constructeur de la librairie. Utilise une valeur par default pour l'ID du drone + */ //droneIDFR(): _droneID("ILLEGAL_DRONE_APPELEZ_POLICE17") {}; /** - * - * Taille maximale de la frame - * - */ + + Taille maximale de la frame + + */ static constexpr uint8_t FRAME_PAYLOAD_LEN_MAX = 251; - - /** - * Setter position avec les coordonnées GPS en entier en centidegrees + altitude - * @param lat - * @param lon - * @param alt - */ - void set_current_position(int32_t lat, int32_t lon, int16_t alt) { - _latitude = lat * 1e-2; - _longitude = lon * 1e-2; - _altitude = alt; - - _new_GPS_data = true; - std::chrono::duration time_period_GPS = std::chrono::high_resolution_clock::now() - _last_data_rcv; - _last_data_rcv = std::chrono::high_resolution_clock::now(); - - _data_period_GPS = (_data_period_GPS * 0.8) + (time_period_GPS.count() * 0.2); - - if(_home_set){ - // Update height above Home - _height = _altitude - _home_altitude; - - // 2D distance from previous position sent (meters, deg) - _2D_distance = distanceBetween(static_cast(lat) * 1e-7, static_cast(lon) * 1e-7, static_cast(_last_latitude_sent) * 1e-5, static_cast(_last_longitude_sent) * 1e-5); - - // 3D distance from last position sent (meters) - _3D_distance_from_last_point_sent = sqrt((_2D_distance * _2D_distance) + ((_last_altitude_sent - _altitude) * (_last_altitude_sent - _altitude))); - } + + /** + Setter position avec les coordonnées GPS en entier en centidegrees + altitude + @param lat + @param lon + @param alt + */ + void set_current_position(int32_t lat, int32_t lon, int16_t alt) + { + _latitude = lat * 1e-2; + _longitude = lon * 1e-2; + _altitude = alt; + + _new_GPS_data = true; + std::chrono::duration time_period_GPS = std::chrono::high_resolution_clock::now() - _last_data_rcv; + _last_data_rcv = std::chrono::high_resolution_clock::now(); + + _data_period_GPS = (_data_period_GPS * 0.8) + (time_period_GPS.count() * 0.2); + + if (_home_set) + { + // Update height above Home + _height = _altitude - _home_altitude; + + // 2D distance from previous position sent (meters, deg) + _2D_distance = distanceBetween(static_cast(lat) * 1e-7, static_cast(lon) * 1e-7, static_cast(_last_latitude_sent) * 1e-5, static_cast(_last_longitude_sent) * 1e-5); + + // 3D distance from last position sent (meters) + _3D_distance_from_last_point_sent = sqrt((_2D_distance * _2D_distance) + ((_last_altitude_sent - _altitude) * (_last_altitude_sent - _altitude))); + } + } + /** + Setter position avec les coordonnées GPS en double en centidegrees + altitude + Converti les valeurs en entiers. + @param lat + @param lon + @param alt + */ + void set_current_position(double lat, double lon, int16_t alt) + { + _latitude = lat * 1e5; + _longitude = lon * 1e5; + _altitude = alt; + + _new_GPS_data = true; + std::chrono::duration time_period_GPS = std::chrono::high_resolution_clock::now() - _last_data_rcv; + _last_data_rcv = std::chrono::high_resolution_clock::now(); + + _data_period_GPS = (_data_period_GPS * 0.8) + (time_period_GPS.count() * 0.2); + + if (_home_set) + { + // Update height above Home + _height = _altitude - _home_altitude; + + // 2D distance from previous position sent (meters, deg) + _2D_distance = distanceBetween(lat, lon, static_cast(_last_latitude_sent) * 1e-5, static_cast(_last_longitude_sent) * 1e-5); + + // 3D distance from last position sent (meters) + _3D_distance_from_last_point_sent = sqrt((_2D_distance * _2D_distance) + ((_last_altitude_sent - _altitude) * (_last_altitude_sent - _altitude))); + } + } + /** + Setter position Home avec les coordonnées GPS en entiers en centidegrees + altitude + @param lat + @param lon + @param alt + */ + void set_home_position(int32_t lat, int32_t lon, int16_t alt) + { + _home_latitude = lat * 1e-2; + _home_longitude = lon * 1e-2; + _home_altitude = alt; + + // Init + _last_latitude_sent = _home_latitude; + _last_longitude_sent = _home_longitude; + _last_altitude_sent = _home_altitude; + + _home_set = true; } /** - * Setter position avec les coordonnées GPS en double en centidegrees + altitude - * Converti les valeurs en entiers. - * @param lat - * @param lon - * @param alt - */ - void set_current_position(double lat, double lon, int16_t alt) { - _latitude = lat * 1e5; - _longitude = lon * 1e5; - _altitude = alt; - - _new_GPS_data = true; - std::chrono::duration time_period_GPS = std::chrono::high_resolution_clock::now() - _last_data_rcv; - _last_data_rcv = std::chrono::high_resolution_clock::now(); - - _data_period_GPS = (_data_period_GPS * 0.8) + (time_period_GPS.count() * 0.2); - - if(_home_set){ - // Update height above Home - _height = _altitude - _home_altitude; - - // 2D distance from previous position sent (meters, deg) - _2D_distance = distanceBetween(lat,lon, static_cast(_last_latitude_sent) * 1e-5, static_cast(_last_longitude_sent) * 1e-5); - - // 3D distance from last position sent (meters) - _3D_distance_from_last_point_sent = sqrt((_2D_distance * _2D_distance) + ((_last_altitude_sent - _altitude) * (_last_altitude_sent - _altitude))); - } + Setter position Home avec les coordonnées GPS en double en centidegrees + altitude + Converti les valeurs en entiers. + @param lat + @param lon + @param alt + */ + void set_home_position(double lat, double lon, int16_t alt) + { + _home_latitude = lat * 1e5; + _home_longitude = lon * 1e5; + _home_altitude = alt; + + // Init + _last_latitude_sent = _home_latitude; + _last_longitude_sent = _home_longitude; + _last_altitude_sent = _home_altitude; + + _home_set = true; + } + + /*récup position home*/ + String get_home_position() + { + return ("Latitude: " + String(_home_latitude / 1e5, 4) + " - Longitude: " + String(_home_longitude / 1e5, 4) + " - Altitude: " + String(_home_altitude)); + } + + /*récup position actuelle*/ + String get_position_actuelle() + //_3D_distance_from_last_point_sent + //_2D_distance + { + return ("Latitude: " + String(_latitude / 1e5, 4) + " - Longitude: " + String(_longitude / 1e5, 4) + " - Altitude: " + String(_altitude) + " - Hauteur: " + String(_height) + " - 2D Distance: " + String(_2D_distance) + " - 3D Distance: " + String(_3D_distance_from_last_point_sent) + " - Cap: " + String(_heading)); + } + + /** + Setter pour l'altitude en MSL (Mean Sea Level)/ Niveau au dessus de la mer en m + @param alt + */ + + + void set_altitude(int16_t alt) + { + _altitude = alt; + } + /** + Setter pour la hauteur au sol par rapport au point de décollage en m + @param height + */ + void set_heigth(int16_t height) + { + _height = height; + } + /** + Setter pour les coordonnées GPS en entiers en centidegrees + @param lat + @param lon + */ + void set_home_lat_lon(int32_t lat, int32_t lon) + { + _home_latitude = lat * 1e-2; + _home_longitude = lon * 1e-2; + } + /** + Setter pour les coordonnées GPS en entiers en centidegrees + @param lat + @param lon + */ + void set_home_lat_lon(double lat, double lon) + { + _home_latitude = lat * 1e5; + _home_longitude = lon * 1e5; } + /** - * Setter position Home avec les coordonnées GPS en entiers en centidegrees + altitude - * @param lat - * @param lon - * @param alt - */ - void set_home_position(int32_t lat, int32_t lon, int16_t alt) { - _home_latitude = lat * 1e-2; - _home_longitude = lon * 1e-2; - _home_altitude = alt; - - // Init - _last_latitude_sent = _home_latitude; - _last_longitude_sent = _home_longitude; - _last_altitude_sent = _home_altitude; - - _home_set = true; + Setter pour la vitesse au sol en m/s + @param ground_speed + */ + void set_ground_speed(double ground_speed) + { + _ground_speed = ground_speed; } /** - * Setter position Home avec les coordonnées GPS en double en centidegrees + altitude - * Converti les valeurs en entiers. - * @param lat - * @param lon - * @param alt - */ - void set_home_position(double lat, double lon, int16_t alt) { - _home_latitude = lat * 1e5; - _home_longitude = lon * 1e5; - _home_altitude = alt; - - // Init - _last_latitude_sent = _home_latitude; - _last_longitude_sent = _home_longitude; - _last_altitude_sent = _home_altitude; - - _home_set = true; + Setter pour le code info + @param codeinfo + */ + void set_codeinfo(double codeinfo) + { + _codeinfo = codeinfo; } - /** - * Setter pour la vitesse au sol en m/s - * @param ground_speed - */ - void set_ground_speed(double ground_speed) { - _ground_speed = ground_speed; + Cap du drone en degree par rapport au nord + @param heading + */ + void set_heading(uint16_t heading) + { + _heading = heading; } /** - * Cap du drone en degree par rapport au nord - * @param heading - */ - void set_heading(uint16_t heading) { - _heading = heading; + Setter pour l'id du drone. + Utiliser cette fonction pour changer l'id par défault + @param id_value + */ + void set_drone_id(const char* id_value) + { + // don't use std::copy as it isn't support on all targets like espressif32 sdk ! + // TODO : if size(id_value) < TLV_LENGTH[ID_FR], fill with 0 + memcpy(_droneID, id_value, TLV_LENGTH[ID_FR]); } /** - * Setter pour l'id du drone. - * Utiliser cette fonction pour changer l'id par défault - * @param id_value - */ - void set_drone_id(const char* id_value) { - // don't use std::copy as it isn't support on all targets like espressif32 sdk ! - // TODO : if size(id_value) < TLV_LENGTH[ID_FR], fill with 0 - memcpy(_droneID, id_value, TLV_LENGTH[ID_FR]); + Renvoie la distance (3D) par rapport à la dernière position envoyée. + + */ + double get_distance_from_last_position_sent() + { + return _3D_distance_from_last_point_sent; } /** - * Renvoie la distance (3D) par rapport à la dernière position envoyée. - * - */ - double get_distance_from_last_position_sent(){ - return _3D_distance_from_last_point_sent; + Renvoie la vitesse GPS au sol. + @return ground speed in km/h + */ + double get_ground_speed_kmh() + { + return _ground_speed * 3.6; } /** - * Renvoie la vitesse GPS au sol. - * @return ground speed in km/h - */ - double get_ground_speed_kmh(){ - return _ground_speed * 3.6; + Renvoie le code info. + */ + double get_codeinfo() + { + return _codeinfo; } /** - * Genère la frame 802.11 beacon complète - * @param full_frame beacon frame buffer - * @param start_from starting offset on the buffer - * @return buffer space used - */ + Genère la frame 802.11 beacon complète + @param full_frame beacon frame buffer + @param start_from starting offset on the buffer + @return buffer space used + */ uint8_t generate_beacon_frame(uint8_t* full_frame, uint16_t start_from) { - // Vendor specific 802.11 beacon frame - full_frame[start_from] = FRAME_VS; - start_from++; - const uint16_t payload_marker = start_from; - start_from++; + // Vendor specific 802.11 beacon frame + full_frame[start_from] = FRAME_VS; + start_from++; + const uint16_t payload_marker = start_from; + start_from++; - for (auto i = 0; i<3; i++) { - full_frame[start_from] = FRAME_OUI[i]; - start_from++; - } - full_frame[start_from] = FRAME_VS_TYPE; + for (auto i = 0; i < 3; i++) + { + full_frame[start_from] = FRAME_OUI[i]; start_from++; - const uint8_t payload_size = generate_drone_frame(full_frame, start_from); // remove payload - full_frame[payload_marker] = payload_size + 4; // +OUI + VSTYPE - return start_from + payload_size; + } + full_frame[start_from] = FRAME_VS_TYPE; + start_from++; + const uint8_t payload_size = generate_drone_frame(full_frame, start_from); // remove payload + full_frame[payload_marker] = payload_size + 4; // +OUI + VSTYPE + return start_from + payload_size; } /** - * Genère le contenu de l'identification drone - * @param full_frame beacon frame buffer - * @param start_from starting offset on the buffer - * @return buffer space used - */ - uint8_t generate_drone_frame(uint8_t* full_frame, uint16_t start_from) { - uint8_t count = 0; - // Protocol version - full_frame[start_from + count] = PROTOCOL_VERSION; - count++; - full_frame[start_from + count] = TLV_LENGTH[PROTOCOL_VERSION]; - count++; - full_frame[start_from + count] = FRAME_VS_TYPE; - count++; - // Drone ID FR - full_frame[start_from + count] = ID_FR; - count++; - full_frame[start_from + count] = TLV_LENGTH[ID_FR]; - count++; - for (auto i = 0; i < TLV_LENGTH[ID_FR]; i++) { - full_frame[start_from + count] = _droneID[i]; - count++; - } - // LATITUDE - full_frame[start_from + count] = LATITUDE; - count++; - full_frame[start_from + count] = TLV_LENGTH[LATITUDE]; - count++; - for (auto i = TLV_LENGTH[LATITUDE] - 1; i >= 0; i--) { - full_frame[start_from + count] = (get_2_complement(_latitude) >> (8 * i)) & 0xFF; - count++; - } - // LONGITUDE - full_frame[start_from + count] = LONGITUDE; - count++; - full_frame[start_from + count] = TLV_LENGTH[LONGITUDE]; - count++; - for (auto i = TLV_LENGTH[LONGITUDE] - 1; i >= 0; i--) { - full_frame[start_from + count] = (get_2_complement(_longitude) >> (8 * i)) & 0xFF; - count++; - } - // ALTITUDE - full_frame[start_from + count] = ALTITUDE; - count++; - full_frame[start_from + count] = TLV_LENGTH[ALTITUDE]; - count++; - for (auto i = TLV_LENGTH[ALTITUDE] - 1; i >= 0; i--) { - full_frame[start_from + count] = (get_2_complement(_altitude) >> (8 * i)) & 0xFF; - count++; - } - // HEIGHT - full_frame[start_from + count] = HEIGTH; - count++; - full_frame[start_from + count] = TLV_LENGTH[HEIGTH]; - count++; - for (auto i = TLV_LENGTH[HEIGTH] - 1; i >= 0; i--) { - full_frame[start_from + count] = (get_2_complement(_height) >> (8 * i)) & 0xFF; - count++; - } - // HOME LATITUDE - full_frame[start_from + count] = HOME_LATITUDE; - count++; - full_frame[start_from + count] = TLV_LENGTH[HOME_LATITUDE]; + Genère le contenu de l'identification drone + @param full_frame beacon frame buffer + @param start_from starting offset on the buffer + @return buffer space used + */ + uint8_t generate_drone_frame(uint8_t* full_frame, uint16_t start_from) + { + uint8_t count = 0; + // Protocol version + full_frame[start_from + count] = PROTOCOL_VERSION; + count++; + full_frame[start_from + count] = TLV_LENGTH[PROTOCOL_VERSION]; + count++; + full_frame[start_from + count] = FRAME_VS_TYPE; + count++; + // Drone ID FR + full_frame[start_from + count] = ID_FR; + count++; + full_frame[start_from + count] = TLV_LENGTH[ID_FR]; + count++; + for (auto i = 0; i < TLV_LENGTH[ID_FR]; i++) + { + full_frame[start_from + count] = _droneID[i]; count++; - for (auto i = TLV_LENGTH[HOME_LATITUDE] - 1; i >= 0; i--) { - full_frame[start_from + count] = (get_2_complement(_home_latitude) >> (8 * i)) & 0xFF; - count++; - } - // HOME LONGITUDE - full_frame[start_from + count] = HOME_LONGITUDE; + } + // LATITUDE + full_frame[start_from + count] = LATITUDE; + count++; + full_frame[start_from + count] = TLV_LENGTH[LATITUDE]; + count++; + for (auto i = TLV_LENGTH[LATITUDE] - 1; i >= 0; i--) + { + full_frame[start_from + count] = (get_2_complement(_latitude) >> (8 * i)) & 0xFF; count++; - full_frame[start_from + count] = TLV_LENGTH[HOME_LONGITUDE]; + } + // LONGITUDE + full_frame[start_from + count] = LONGITUDE; + count++; + full_frame[start_from + count] = TLV_LENGTH[LONGITUDE]; + count++; + for (auto i = TLV_LENGTH[LONGITUDE] - 1; i >= 0; i--) + { + full_frame[start_from + count] = (get_2_complement(_longitude) >> (8 * i)) & 0xFF; count++; - for (auto i = TLV_LENGTH[HOME_LONGITUDE] - 1; i >= 0; i--) { - full_frame[start_from + count] = (get_2_complement(_home_longitude) >> (8 * i)) & 0xFF; - count++; - } - // GROUND SPEED - full_frame[start_from + count] = GROUND_SPEED; + } + // ALTITUDE + full_frame[start_from + count] = ALTITUDE; + count++; + full_frame[start_from + count] = TLV_LENGTH[ALTITUDE]; + count++; + for (auto i = TLV_LENGTH[ALTITUDE] - 1; i >= 0; i--) + { + full_frame[start_from + count] = (get_2_complement(_altitude) >> (8 * i)) & 0xFF; count++; - full_frame[start_from + count] = TLV_LENGTH[GROUND_SPEED]; + } + // HEIGHT + full_frame[start_from + count] = HEIGTH; + count++; + full_frame[start_from + count] = TLV_LENGTH[HEIGTH]; + count++; + for (auto i = TLV_LENGTH[HEIGTH] - 1; i >= 0; i--) + { + full_frame[start_from + count] = (get_2_complement(_height) >> (8 * i)) & 0xFF; count++; - full_frame[start_from + count] = round(_ground_speed); + } + // HOME LATITUDE + full_frame[start_from + count] = HOME_LATITUDE; + count++; + full_frame[start_from + count] = TLV_LENGTH[HOME_LATITUDE]; + count++; + for (auto i = TLV_LENGTH[HOME_LATITUDE] - 1; i >= 0; i--) + { + full_frame[start_from + count] = (get_2_complement(_home_latitude) >> (8 * i)) & 0xFF; count++; - // HEADING - full_frame[start_from + count] = HEADING; + } + // HOME LONGITUDE + full_frame[start_from + count] = HOME_LONGITUDE; + count++; + full_frame[start_from + count] = TLV_LENGTH[HOME_LONGITUDE]; + count++; + for (auto i = TLV_LENGTH[HOME_LONGITUDE] - 1; i >= 0; i--) + { + full_frame[start_from + count] = (get_2_complement(_home_longitude) >> (8 * i)) & 0xFF; count++; - full_frame[start_from + count] = TLV_LENGTH[HEADING]; + } + // GROUND SPEED + full_frame[start_from + count] = GROUND_SPEED; + count++; + full_frame[start_from + count] = TLV_LENGTH[GROUND_SPEED]; + count++; + full_frame[start_from + count] = round(_ground_speed); + count++; + // HEADING + full_frame[start_from + count] = HEADING; + count++; + full_frame[start_from + count] = TLV_LENGTH[HEADING]; + count++; + for (auto i = TLV_LENGTH[HEADING] - 1; i >= 0; i--) + { + // donne une erreur avec esp32-c3 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ FS + full_frame[start_from + count] = (get_2_complement(_heading) >> (8 * i)) & 0xFF; count++; - for (auto i = TLV_LENGTH[HEADING] - 1; i >= 0; i--) { - full_frame[start_from + count] = (get_2_complement(_heading) >> (8 * i)) & 0xFF; - count++; - } - return count; - // TODO: check lenght + } + + // CODEINFO + full_frame[start_from + count] = CODEINFO; + count++; + full_frame[start_from + count] = TLV_LENGTH[CODEINFO]; + count++; + full_frame[start_from + count] = round(_codeinfo); + count++; + + return count; + // TODO: check lenght + } + + /** + Sauvegarde les données de la dernière trame envoyée : Position et Temps + */ + void set_last_send() + { + // Temps + _last_send = std::chrono::high_resolution_clock::now(); + + // Position + _last_latitude_sent = _latitude; + _last_longitude_sent = _longitude; + _last_altitude_sent = _altitude; } /** - * Sauvegarde les données de la dernière trame envoyée : Position et Temps - */ - void set_last_send() { - // Temps - _last_send = std::chrono::high_resolution_clock::now(); - - // Position - _last_latitude_sent = _latitude; - _last_longitude_sent = _longitude; - _last_altitude_sent = _altitude; + Notifie si la position Home est définie + @return true if Home is set + */ + bool has_home_set() const + { + return _home_set == true; } - - /** - * Notifie si la position Home est définie - * @return true if Home is set - */ - bool has_home_set() const { - return _home_set == true; - } - /** - * Notifie quand 3s sont écoulés pour envoyer une nouvelle trame. - * @return true if elapse time is > 3s - */ - bool has_pass_time() { - std::chrono::duration elapsed = std::chrono::high_resolution_clock::now() - _last_send; - return elapsed.count() + _data_period_GPS >= FRAME_TIME_LIMIT; + /** + Notifie quand 3s sont écoulés pour envoyer une nouvelle trame. + @return true if elapse time is > 3s + */ + bool has_pass_time() + { + std::chrono::duration elapsed = std::chrono::high_resolution_clock::now() - _last_send; + return elapsed.count() + _data_period_GPS >= FRAME_TIME_LIMIT; } /** - * Notifie si le drone a bougé de plus de 30m depuis le dernier envoi, - * avec une trajectoire rectiligne et une vitesse "rapide" l'envoi est anticipé - * pour éviter de dépasser les 30m. - * @return true if distance from last point sent will be > 30m - */ - bool has_pass_distance() { - _predicted_distance = 0; - - // If enough speed, look forward for next point with estimated distance increased by 10% - if(_ground_speed >= (FRAME_DISTANCE_LIMIT/FRAME_TIME_LIMIT)){ - _predicted_distance = (_data_period_GPS * _ground_speed * 1.1); - } - - return (_3D_distance_from_last_point_sent +_predicted_distance) >= FRAME_DISTANCE_LIMIT; + Notifie si le drone a bougé de plus de 30m depuis le dernier envoi, + avec une trajectoire rectiligne et une vitesse "rapide" l'envoi est anticipé + pour éviter de dépasser les 30m. + @return true if distance from last point sent will be > 30m + */ + bool has_pass_distance() + { + _predicted_distance = 0; + + // If enough speed, look forward for next point with estimated distance increased by 10% + if (_ground_speed >= (FRAME_DISTANCE_LIMIT / FRAME_TIME_LIMIT)) + { + _predicted_distance = (_data_period_GPS * _ground_speed * 1.1); + } + + return (_3D_distance_from_last_point_sent + _predicted_distance) >= FRAME_DISTANCE_LIMIT; } /** - * Notifie si la condition de distance ou de temps est dépassée pour envoyer une nouvelle trame, - * et si de nouvelles données GPS sont présentes. - * @return - */ - bool time_to_send() { - bool current_status = _new_GPS_data; - - if(current_status){ - // Reset - _new_GPS_data = false; - } - - return current_status && (has_pass_time() || has_pass_distance()); + Notifie si la condition de distance ou de temps est dépassée pour envoyer une nouvelle trame, + et si de nouvelles données GPS sont présentes. + @return + */ + bool time_to_send() + { + bool current_status = _new_GPS_data; + + if (current_status) + { + // Reset + _new_GPS_data = false; + } + + return current_status && (has_pass_time() || has_pass_distance()); } -private: + private: /** - * Temps limite entre deux trames en s - */ + Temps limite entre deux trames en s + */ static constexpr uint8_t FRAME_TIME_LIMIT = 3; // in s /** - * Distance limite entre deux trames en m - */ + Distance limite entre deux trames en m + */ static constexpr double FRAME_DISTANCE_LIMIT = 30.0; // in m /** - * Enumeration des types de données à envoyer - */ - enum DATA_TYPE: uint8_t { + Enumeration des types de données à envoyer + */ + enum DATA_TYPE : uint8_t + { RESERVED = 0, PROTOCOL_VERSION = 1, ID_FR = 2, @@ -380,41 +489,44 @@ class droneIDFR { HOME_LONGITUDE = 9, // In WS84 in degree * 1e5 GROUND_SPEED = 10, // In m/s HEADING = 11, // Heading in degree from north 0 to 359. - NOT_DEFINED_END = 12, + CODEINFO = 12, // message info + NOT_DEFINED_END = 13, }; /** - * Tableau TLV (TYPE, LENGTH, VALUE) avec les tailles attendu des différents type données. - */ - static constexpr uint8_t TLV_LENGTH[] { - 0, // [DATA_TYPE::RESERVED] - 1, // [DATA_TYPE::PROTOCOL_VERSION] - 30, // [DATA_TYPE::ID_FR] - 0, // [DATA_TYPE::ID_ANSI_CTA] - 4, // [DATA_TYPE::LATITUDE] - 4, // [DATA_TYPE::LONGITUDE] - 2, // [DATA_TYPE::ALTITUDE] - 2, // [DATA_TYPE::HEIGTH] - 4, // [DATA_TYPE::HOME_LATITUDE] - 4, // [DATA_TYPE::HOME_LONGITUDE] - 1, // [DATA_TYPE::GROUND_SPEED] - 2, // [DATA_TYPE::HEADING] + Tableau TLV (TYPE, LENGTH, VALUE) avec les tailles attendu des différents type données. + */ + static constexpr uint8_t TLV_LENGTH[] + { + 0, // [DATA_TYPE::RESERVED] + 1, // [DATA_TYPE::PROTOCOL_VERSION] + 30, // [DATA_TYPE::ID_FR] + 0, // [DATA_TYPE::ID_ANSI_CTA] + 4, // [DATA_TYPE::LATITUDE] + 4, // [DATA_TYPE::LONGITUDE] + 2, // [DATA_TYPE::ALTITUDE] + 2, // [DATA_TYPE::HEIGTH] + 4, // [DATA_TYPE::HOME_LATITUDE] + 4, // [DATA_TYPE::HOME_LONGITUDE] + 1, // [DATA_TYPE::GROUND_SPEED] + 2, // [DATA_TYPE::HEADING] + 1, // [DATA_TYPE::CODEINFO] }; static constexpr uint8_t FRAME_COPTER_ID = 3; static constexpr uint8_t FRAME_PLANE_ID = 4; /** - * Beacon frame VS: - */ + Beacon frame VS: + */ static constexpr uint8_t FRAME_VS = 0XDD; /** - * Beacon frame OUI - */ + Beacon frame OUI + */ const uint8_t FRAME_OUI[3] = {0x6A, 0x5C, 0x35}; /** - * Beacon frame VS TYPE - */ + Beacon frame VS TYPE + */ static constexpr uint8_t FRAME_VS_TYPE = 1; int32_t _latitude; @@ -426,7 +538,8 @@ class droneIDFR { int16_t _home_altitude; double _ground_speed; uint16_t _heading; - uint8_t _droneID[TLV_LENGTH[ID_FR]+1]; // +1 for null termination + double _codeinfo; + uint8_t _droneID[TLV_LENGTH[ID_FR] + 1]; // +1 for null termination std::chrono::system_clock::time_point _last_send = std::chrono::system_clock::now(); std::chrono::system_clock::time_point _last_data_rcv = std::chrono::system_clock::now(); // for travelled distance calculation @@ -440,44 +553,50 @@ class droneIDFR { bool _home_set = false; bool _new_GPS_data = false; - static inline uint32_t get_2_complement(int32_t value) { - return value & 0xFFFFFFFF; + static inline uint32_t get_2_complement(int32_t value) + { + return value & 0xFFFFFFFF; } - static inline uint16_t get_2_complement(int16_t value) { - return value & 0xFFFF; + static inline uint16_t get_2_complement(int16_t value) + { + return value & 0xFFFF; + } + static inline uint16_t get_2_complement(uint16_t value) // rajout par FS pour éviter erreur compil avec ESP32-C3 + { + return value & 0xFFFF; } // Taken from TinyGPS++ /** - * Calcule une approximation de la distance entre deux coordonnées WS84 (GPS) - * @param lat1 - * @param long1 - * @param lat2 - * @param long2 - * @return distance en m - */ + Calcule une approximation de la distance entre deux coordonnées WS84 (GPS) + @param lat1 + @param long1 + @param lat2 + @param long2 + @return distance en m + */ static double distanceBetween(double lat1, double long1, double lat2, double long2) { - // returns distance in meters between two positions, both specified - // as signed decimal-degrees latitude and longitude. Uses great-circle - // distance computation for hypothetical sphere of radius 6372795 meters. - // Because Earth is no exact sphere, rounding errors may be up to 0.5%. - // Courtesy of Maarten Lamers - double delta = radians(long1-long2); - const double sdlong = sin(delta); - const double cdlong = cos(delta); - lat1 = radians(lat1); - lat2 = radians(lat2); - const double slat1 = sin(lat1); - const double clat1 = cos(lat1); - const double slat2 = sin(lat2); - const double clat2 = cos(lat2); - delta = (clat1 * slat2) - (slat1 * clat2 * cdlong); - delta = sq(delta); - delta += sq(clat2 * sdlong); - delta = sqrt(delta); - const double denom = (slat1 * slat2) + (clat1 * clat2 * cdlong); - delta = atan2(delta, denom); - return abs(delta * 6372795); + // returns distance in meters between two positions, both specified + // as signed decimal-degrees latitude and longitude. Uses great-circle + // distance computation for hypothetical sphere of radius 6372795 meters. + // Because Earth is no exact sphere, rounding errors may be up to 0.5%. + // Courtesy of Maarten Lamers + double delta = radians(long1 - long2); + const double sdlong = sin(delta); + const double cdlong = cos(delta); + lat1 = radians(lat1); + lat2 = radians(lat2); + const double slat1 = sin(lat1); + const double clat1 = cos(lat1); + const double slat2 = sin(lat2); + const double clat2 = cos(lat2); + delta = (clat1 * slat2) - (slat1 * clat2 * cdlong); + delta = sq(delta); + delta += sq(clat2 * sdlong); + delta = sqrt(delta); + const double denom = (slat1 * slat2) + (clat1 * clat2 * cdlong); + delta = atan2(delta, denom); + return abs(delta * 6372795); } }; diff --git a/fsBalise.cpp b/fsBalise.cpp index fcc16e2..0553b7b 100644 --- a/fsBalise.cpp +++ b/fsBalise.cpp @@ -17,14 +17,29 @@ #include "fsBalise.h" #include "fs_GPS.h" #include "fs_options.h" +#include "fs_main.h" +//#ifdef fs_RECEPTEUR +#include "fs_recepteur.h" +//#endif #include "fs_pagePROGMEM.h" #include #include +#ifdef ESP32 +#include +extern fs_WebServer server; +#else extern ESP8266WebServer server; +#endif extern boolean traceFileOpened; extern bool fileSystemFull; extern String messAlarm; +extern float VmaxSegment, AltMaxSegment; +extern unsigned long TimeShutdown; +extern boolean countdownRunning; +#ifdef fs_STAT +extern statBloc_t statNotFound , statCockpit ; +#endif extern TinyGPSPlus gps; struct pref preferences; @@ -36,28 +51,45 @@ void sendChunkDebut ( bool avecTopMenu ) { if (avecTopMenu)server.sendContent_P(topMenu); } void handleCockpit() { + // reset du timeout pour arreter le wifi + TimeShutdown = millis() + preferences.timeoutWifi * 1000; + countdownRunning = true; +#ifdef fs_STAT + statCockpit.T0 = millis(); + handleCockpitNotFound(); + calculerStat(true, &statCockpit); +#else + handleCockpitNotFound(); +#endif +} +void handleCockpitNotFound() { + //Serial.print("handleCockpitNotFound "); Serial.print (server.hostHeader()); Serial.print(" ");Serial.println (server.uri()); sendChunkDebut ( true ); // debut HTML style, avec topMenu server.sendContent_P(cockpit); + server.sendContent_P(cockpit_fin); server.chunkedResponseFinalize(); } - -// Ecriture d'une ligne csv ou GPX dans le fichier de log. -// Attention: le fichier GPX a une trace non fermer. Le compléter lors d'un download -// Si le file system est plein, on efface les fichiers les plus anciens. -// Dans ce cas on perdra une ligne de log -// Return: true ecriture OK; false sinon -// (le file system est surement plein, avec un seul fichier -// +void handleNotFound() { + // Serial.printf_P(PSTR("------------handleNotFound:%s milli:%u\n"), server.uri().c_str(), millis()); + if (loadFromSPIFFS(server.uri())) return; // permet de telecharger un fichier par appel direct +#ifdef fs_STAT + statNotFound.T0 = millis(); + handleCockpitNotFound(); // sinon: acces au portail général + calculerStat(true, &statNotFound); +#else + handleCockpitNotFound(); // sinon: acces au portail général +#endif +} void logRingBuffer(char ringBuffer[], int sizeBuffer, int indexBuffer) { // char logFileName[] = "logRingBuf.txt"; - char logFileName[] = "errorlog.txt"; + char logFileName[] = "/errorlog.txt"; File fileToAppend = LittleFS.open(logFileName, "a"); - fileToAppend.println("\n====================="); - fileToAppend.printf("%u bauds,%uHz\n", preferences.baud, preferences.hz); + fileToAppend.println(F("\n=====================")); + fileToAppend.printf_P(PSTR("%u bauds,%uHz\n"), preferences.baud, preferences.hz); fileToAppend.println(indexBuffer); - fileToAppend.write(ringBuffer + indexBuffer, sizeBuffer - indexBuffer); - fileToAppend.write(ringBuffer, indexBuffer); + fileToAppend.write((uint8_t*)ringBuffer[indexBuffer], sizeBuffer - indexBuffer); + fileToAppend.write((uint8_t*)ringBuffer, indexBuffer); fileToAppend.println(); fileToAppend.close(); yield(); @@ -65,36 +97,41 @@ void logRingBuffer(char ringBuffer[], int sizeBuffer, int indexBuffer) { // Problème détecté avec la lecture du GPS. On enregistre des infos dans un log d'erreur. void logError(TinyGPSPlus &gps, float latLastLog, float lngLastLog , float segment) { - char logFileName[] = "errorlog.txt"; + char logFileName[] = "/errorlog.txt"; bool isFileExist = LittleFS.exists(logFileName); File fileToAppend = LittleFS.open(logFileName, "a"); // if (!isFileExist) { //Header - fileToAppend.println("\nlocation.isValid,lat,lng,latLastLog,lngLastLog,segment,speed_km,alt_m," - "sat,hdop,year,month,day,hour,min,sec,centi,failedChecksum,passedChecksum\n"); + fileToAppend.println(F("\nlocation.isValid,lat,lng,latLastLog,lngLastLog,segment,speed_km,alt_m," + "sat,hdop,year,month,day,hour,min,sec,centi,failedChecksum,passedChecksum\n")); // } - fileToAppend.printf("%s", gps.location.isValid() ? "T" : "F"); - fileToAppend.printf(",%.6f,%.6f,%.6f,%.6f", gps.location.lat(), gps.location.lng(), latLastLog, lngLastLog); - fileToAppend.printf(",%.0f,%.2f,%.2f", segment, gps.speed.kmph(), gps.altitude.meters()); - fileToAppend.printf(",%u,%i,%u,%u,%u", gps.satellites.value(), gps.hdop.value(), gps.date.year(), gps.date.month(), gps.date.day()); - fileToAppend.printf(",%u,%u,%u,%u,%u,%u\n", gps.time.hour(), gps.time.minute(), gps.time.second(), gps.time.centisecond(), - gps.failedChecksum(), gps.passedChecksum()); + fileToAppend.printf_P(PSTR("%s"), gps.location.isValid() ? "T" : "F"); + fileToAppend.printf_P(PSTR(",%.6f,%.6f,%.6f,%.6f"), gps.location.lat(), gps.location.lng(), latLastLog, lngLastLog); + fileToAppend.printf_P(PSTR(",%.0f,%.2f,%.2f"), segment, gps.speed.kmph(), gps.altitude.meters()); + fileToAppend.printf_P(PSTR(",%u,%i,%u,%u,%u"), gps.satellites.value(), gps.hdop.value(), gps.date.year(), gps.date.month(), gps.date.day()); + fileToAppend.printf_P(PSTR(",%u,%u,%u,%u,%u,%u\n"), gps.time.hour(), gps.time.minute(), gps.time.second(), gps.time.centisecond(), + gps.failedChecksum(), gps.passedChecksum()); fileToAppend.close(); } // Ecriture d'une ligne dans le trace file. En général le fichier est deja ouvert en mode append +// Return: true ecriture OK; false sinon +// (le file system est surement plein, avec un seul fichier +// char traceFileName[25] = ""; // en global , car utilisé aussi dans telechargement. -File traceFile; // en global. Besoin dans le main et pour diwnload +File traceFile; // en global. Besoin dans le main et pour download bool fs_ecrireTrace(TinyGPSPlus &gps) { - char timeBuffer[16]; - String logMessage; + int longueurEcriture; if (!traceFileOpened) { - if (traceFileName[0] == 0) // on cree une seule fois le fichier dans un run - sprintf(traceFileName, "%04u-%02u-%02u_%02u-%02u", gps.date.year(), gps.date.month(), gps.date.day(), - gps.time.hour(), gps.time.minute()); + if (traceFileName[0] == 0) { // on cree une seule fois le nom fichier dans un run + sprintf_P(traceFileName, PSTR("/%04u-%02u-%02u_%02u-%02u"), gps.date.year(), gps.date.month(), gps.date.day(), + gps.time.hour(), gps.time.minute()); + // Serial.printf_P(PSTR("date.isValid: %s location.isValid: %s\n"), gps.date.isValid() ? "yes" : "no", gps.location.isValid() ? "yes" : "no"); + } if (!LittleFS.exists(traceFileName)) { Serial.print(F("Création du fichier trace ")); Serial.println(traceFileName); traceFile = LittleFS.open(traceFileName, "a"); + Serial.printf_P(PSTR(" open append:%s\n"), traceFile ? "OK" : "FAIL"); //+++++++++++++++++++++++++++++++++++++++++ traceFile.close(); // le creer et forcer son enregistrement } traceFile = LittleFS.open(traceFileName, "a"); @@ -115,6 +152,10 @@ bool fs_ecrireTrace(TinyGPSPlus &gps) { trackLigne.centisecond = gps.time.centisecond(); // 100ths of a second (0-99) (u8) trackLigne.altitude = gps.altitude.meters(); trackLigne.speed = gps.speed.kmph(); + trackLigne.VmaxSegment = VmaxSegment; // vitesse max vue entre ce point et le point précédent en km/h + trackLigne.AltMaxSegment = AltMaxSegment; + VmaxSegment = 0.0; + AltMaxSegment = 0.0; longueurEcriture = traceFile.write((uint8_t *)&trackLigne, sizeof(trackLigne)); if (longueurEcriture != sizeof(trackLigne)) { Serial.println(F("Erreur longueur écriture.")); @@ -131,10 +172,25 @@ bool fs_ecrireTrace(TinyGPSPlus &gps) { bool removeOldest() { int nbrFiles = 0; String oldest = "zzzzz", youngest = ""; +#ifdef ESP32 + File dir = LittleFS.open("/"); +#else Dir dir = LittleFS.openDir("/"); +#endif int tailleTotale = 0; + +#ifdef ESP32 + File f = dir.openNextFile(); + while (f) { + // String fileName = f.name(); +#else while (dir.next()) { - String fileName = dir.fileName(); + File f = dir.openFile("r"); + // String fileName = String("/") + f.name(); +#endif + String fileName = f.name(); + if (fileName.charAt(0) != '/') fileName = "/" + fileName; + // Ici le fileName est /xyz... (ESP32 ou 8266) nbrFiles++; if (fileName < oldest ) { oldest = fileName; @@ -142,7 +198,16 @@ bool removeOldest() { if (fileName > youngest ) { youngest = fileName; } +#ifdef ESP32 + f = dir.openNextFile(); +#else + f.close(); +#endif } +#ifdef ESP32 + dir.close(); +#endif + Serial.printf_P(PSTR("oldest:%s\n"), oldest.c_str()); if ( nbrFiles > 4) { messAlarm = ""; // on va faire de la place dans la mémoire (?) .Enlever message alarme fileSystemFull = false; @@ -156,21 +221,38 @@ void listSPIFFS(String message) { int nbrFiles = 0, longest = 0; String oldest = "zzzzz", youngest = ""; int idOldest = 0, idYoungest = 0, idLongest; - String response = ""; // refresh 30 seconds. - response += "
"; + String response = "\n"; // refresh 30 seconds. + response += "
" + messAlarm + " " + message + "
\n"; server.sendContent(response); + // Adaptation ESP8266 / ESP32 pour liste directory vu à partir ligne 185 dans + //https://github.com/lorol/ESPAsyncWebServer/blob/master/src/SPIFFSEditor.cpp#L185 + traceFile.flush(); // mettre à jour les infos sur fichir et fs +#ifdef ESP32 + File dir = LittleFS.open("/"); +#else Dir dir = LittleFS.openDir("/"); +#endif + +#ifdef ESP32 + File f = dir.openNextFile(); + while (f) { + // String fileName = f.name(); +#else while (dir.next()) { - String fileName = dir.fileName(); - //fs: File f = dir.openFile("r"); + // String fileName = dir.fileName(); File f = dir.openFile("r"); + // String fileName = String("/") + f.name(); +#endif + String fileName = f.name(); + if (fileName.charAt(0) != '/') fileName = "/" + fileName; + // Ici le fileName est /xyz... (ESP32 ou 8266) if (!f) { response = "\n"; + + // on génère explicitement un download="kkjhjk.zzz" (sans le / de tête) car sinon cela ne marche pas bien sur certains systèmes + response = ""; + response += "\n"; response += "\n"; - response += "\n"; + response += ""; } server.sendContent(response); +#ifdef ESP32 + f = dir.openNextFile(); +#else + f.close(); +#endif } +#ifdef ESP32 + dir.close(); +#endif response = "
" + messAlarm + " " + message + "
" + fileName + "Open error !! <\td><\td>\n"; } else { totalSize += f.size(); nbrFiles++; - if (fileName.substring(0, 1) == "2" ) { // ne prendre en compte que les fichier de trace pour oldet/youger + if (fileName.substring(0, 2) == "/2" ) { // ne prendre en compte que les fichiers de trace pour oldest/younger if (fileName < oldest ) { oldest = fileName; idOldest = nbrFiles; @@ -184,15 +266,29 @@ void listSPIFFS(String message) { longest = f.size(); } } - // on génère explicitement un download="kkjhjk.zzz" car sinon cela ne marche pas bien sur certains systèmes - response = "
" + fileName + "" + (String)f.size() + "
" + (String)f.size() + "
"; if (nbrFiles != 0) { response += ""; } +#ifdef ESP32 + response += "
Nombre de traces: " + String(nbrFiles) + "
Taille totale: " + String(totalSize); + response += "
Espace disponible: " + String(LittleFS.totalBytes()) + "
Espace utilisé: " + String(LittleFS.usedBytes()) ; +#else FSInfo fs_info; if (LittleFS.info(fs_info)) { size_t totalBytes; size_t usedBytes; response += "
Nombre de traces: " + String(nbrFiles) + "
Taille totale: " + String(totalSize); - response += "
totalBytes: " + String(fs_info.totalBytes) + "
usedBytes: " + String(fs_info.usedBytes) ; + response += "
Espace disponible: " + String(fs_info.totalBytes) + "
Espace utilisé: " + String(fs_info.usedBytes) ; } else { response += "
Erreurs dans le système de fichier "; } +#endif response += "
"; server.sendContent(response); return ; } +// Garder uniquement les preferences.nbrMaxTraces dernieres traces +void nettoyageTraces() { + int nbrFiles = 0; +#ifdef ESP32 + File dir = LittleFS.open("/"); + File f = dir.openNextFile(); + while (f) { + nbrFiles++; + f = dir.openNextFile(); + } + dir.close(); +#else + Dir dir = LittleFS.openDir("/"); + while (dir.next()) { + nbrFiles++; + } +#endif + for (int i = nbrFiles; i > preferences.nbrMaxTraces; i--) { + removeOldest(); + } +} + bool loadFromSPIFFS(String path) { // Path est du type server.uri(), donc commence par / - // Les fichier de trace commence tous par l'année /2xxx et font l'objet d'un traitement particuliers. + // Les fichiers de trace commencent tous par l'année /2xxx et font l'objet d'un traitement particuliers. // Un autre fichier peut exister errorlog.txt qui lui sera directement téléchargé. - // Les autre demande sont a tous les coup des "notFound" + // Les autres demandes sont a tous les coups des "notFound" // Les autres (fichiers log erreur sont simplement directement téléchargés. if (path.substring(0, 2) == "/e" ) { File dataFile = LittleFS.open(path.c_str(), "r"); @@ -239,12 +362,8 @@ bool loadFromSPIFFS(String path) { // 01234678901 // dateDuFichier va servir à construire le champ date/heure du fichier GPX String dateDuFichier = path.substring(1, 11); // on oublit le / du debut de l'uri yyyy-mm-dd - // Serial.print("Date | ");Serial.print(dateDuFichier);Serial.println(" | "); - // Si on veut charger le fichier trace en cours, le fermer - //Serial.printf("; % s; % s; \n" , traceFileName, dateDuFichier.c_str()); - String ss = path.substring(1); - if (strcmp(traceFileName, ss.c_str()) == 0) { - Serial.println("Fermeture fichier trace en cours"); + if (strcmp(traceFileName, path.c_str()) == 0) { + Serial.println(F("Fermeture fichier trace en cours")); traceFile.close(); traceFileOpened = false; } @@ -256,10 +375,10 @@ bool loadFromSPIFFS(String path) { // creer le header if (strcmp(preferences.formatTrace, "csv") == 0) { String logMessage; - logMessage = "Latitude, Longitude"; - if (preferences.logHeure) logMessage += ", Heure"; - if (preferences.logVitesse) logMessage += ", Vitesse"; - if (preferences.logAltitude) logMessage += ", Altitude"; + if (preferences.logHeure) logMessage += "Heure,"; + logMessage += "Latitude,Longitude"; + if (preferences.logVitesse) logMessage += ",Vitesse,Vitesse_max_segment"; + if (preferences.logAltitude) logMessage += ",Altitude,Altitude_max_segment"; logMessage += "\n"; server.sendContent(logMessage); } @@ -272,44 +391,52 @@ bool loadFromSPIFFS(String path) { while (dataFile.available()) { dataFile.read((uint8_t *)&trackLigne, sizeof(trackLigne)); if (strcmp(preferences.formatTrace, "csv") == 0) { //generation CSV - // -90.00000,-180.00000,12:30:31,1000.00,99999.99 - // 12345678901234567890123456789012345678901234567890 - // Soit un total de 46 + 2 (cr/lf) + 1 = 49 caractères par point CSV max - if (sizeof(buf) - deb < 51 ) { + // 12:30:31,-90.00000,-180.00000,1000.00,1000.00,99999.99,99999.99 + // 123456789012345678901234567890123456789012345678901234567890123 + // Soit un total de 63 + 2 (cr/lf) + 1 = 65 caractères par point CSV max + if (sizeof(buf) - deb < 70 ) { + // Serial.printf("Ecriture buffer download %i\n", deb); server.sendContent((const char*)buf, deb); deb = 0; buf[0] = 0; // chaine vide } - deb += sprintf(&buf[deb], " % .6f, % .6f", trackLigne.lat, trackLigne.lng); // - if (preferences.logHeure) deb += sprintf(&buf[deb], ", % 02u: % 02u: % 02u. % 02u", trackLigne.hour, trackLigne.minute, trackLigne.second, trackLigne.centisecond); - if (preferences.logVitesse) deb += sprintf(&buf[deb], ", % .2f", trackLigne.speed); - if (preferences.logAltitude) deb += sprintf(&buf[deb], ", % .2f", trackLigne.altitude); - deb += sprintf(&buf[deb], "\r\n"); + if (preferences.logHeure) deb += sprintf_P(&buf[deb], PSTR("%02u:%02u:%02u.%02u,"), trackLigne.hour, trackLigne.minute, trackLigne.second, trackLigne.centisecond); + deb += sprintf_P(&buf[deb], PSTR("%.6f,%.6f"), trackLigne.lat, trackLigne.lng); + if (preferences.logVitesse) { + deb += sprintf_P(&buf[deb], PSTR(",%.2f"), trackLigne.speed); + deb += sprintf_P(&buf[deb], PSTR(",%.2f"), trackLigne.VmaxSegment); + } + if (preferences.logAltitude) { + deb += sprintf_P(&buf[deb], PSTR(",%.2f"), trackLigne.altitude); + deb += sprintf_P(&buf[deb], PSTR(",%.2f"), trackLigne.AltMaxSegment); + } + deb += sprintf_P(&buf[deb], PSTR("\r\n")); } else { // generation GPX // 99999.40 // 123456789012345678901234567890123456789012345678901234567890123456789012345678901234567890123456789012345 // 103 + 2(cr/lf) + 1 106 caractères max pour une entrée - if (sizeof(buf) - deb < 108 ) { + if (sizeof(buf) - deb < 120 ) { + // Serial.printf_P(PSTR("Ecriture buffer download %i\n"), deb); server.sendContent((const char*)buf, deb); deb = 0; buf[0] = 0; } // speed n'existe pas en GPX 1.1: géré par les boutons de sélection du format dans les preferences. //