File content as of revision 88:8965483d35c9:

#include "mbed.h"
#include <string>
#include "Sensor_head_revB.h"
#include "HTU21D.h"
#include "PID.h"
#include "Faulhaber.h"
#include "Utils.h"
#include "beep.h"
#include "USBSerial.h"

//Commandes  des servos
#define PWM_SERVO_POUMON PB_15
#define nSleep_SERVO_POUMON PC_6
#define FWD_SERVO_POUMON PB_14
#define REV_SERVO_POUMON PB_13
#define Channel_A_SERVO_POUMON PB_1
#define Channel_B_SERVO_POUMON PB_2
#define HOME_SERVO_POUMON 0

#define PWM_SERVO_FUITE PB_10
#define nSleep_SERVO_FUITE PB_0
#define FWD_SERVO_FUITE PB_5
#define REV_SERVO_FUITE PB_4
#define Channel_A_SERVO_FUITE PC_9
#define Channel_B_SERVO_FUITE PA_8
#define HOME_SERVO_FUITE 90

//Pin de test des alims
#define PIN_V_PILES_ANALOG PA_1
#define PIN_V_PILES_DIGI PC_3
#define PIN_V_USB_DIGI PA_0

//#define OUTPUT(...) { NVIC_DisableIRQ(USART3_IRQn); serialMonit.printf(__VA_ARGS__); fflush(stdout);NVIC_EnableIRQ(USART3_IRQn);}
//#define OUTPUT(...) { NVIC_DisableIRQ(USART2_IRQn); serialMonit.printf(__VA_ARGS__); fflush(stdout);NVIC_EnableIRQ(USART2_IRQn);}
//#define OUTPUT(...) { NVIC_DisableIRQ(OTG_FS_IRQn);serialMonit.printf(__VA_ARGS__); fflush(stdout);NVIC_EnableIRQ(OTG_FS_IRQn);}
#define TRACEUR_DEBUG(...) { printf(__VA_ARGS__); fflush(stdout);}
//#define TRACEUR(...) { serialMonit.printf(__VA_ARGS__); fflush(stdout);}
#define OUTPUT(...) { serialMonit.printf(__VA_ARGS__); fflush(stdout);}
#define IHM(...) { NVIC_DisableIRQ(USART3_IRQn); display.printf(__VA_ARGS__); fflush(stdout);NVIC_EnableIRQ(USART3_IRQn);}

//Communication USB, OTG_FS
USBSerial serialMonit;
//USBSerial serialMonit;//Pour pilotes windows Olivier

//Moniteur série debug, Serial 2
Serial serial(PA_2, PA_3,115200); // UART2 à décommenter / commenter pour DEBUG et DEEP_DEBUG

//Communication avec l'IHM, Serial 3
Serial display(PC_4, PC_5, 115200);

//PinName pwm, PinName nSleep, PinName fwd, PinName rev, PinName channelA, PinName channelB, int pulsesPerRev, int Rapport, Encoding encoding = X2_ENCODING
//Angle 0 -> tuyau fermé, Angle 90 -> tuyau ouvert
Faulhaber Servo_Poumon(VOLET_POUMON_ADDR, PWM_SERVO_POUMON, nSleep_SERVO_POUMON, FWD_SERVO_POUMON, REV_SERVO_POUMON, Channel_A_SERVO_POUMON, Channel_B_SERVO_POUMON, 16, 1257, HOME_SERVO_POUMON, Faulhaber::X2_ENCODING);
Faulhaber Servo_Fuite(VOLET_FUITE_ADDR, PWM_SERVO_FUITE, nSleep_SERVO_FUITE, FWD_SERVO_FUITE, REV_SERVO_FUITE, Channel_A_SERVO_FUITE, Channel_B_SERVO_FUITE, 16, 207, HOME_SERVO_FUITE, Faulhaber::X2_ENCODING);

//Init de la lib ARNSRS;
SENSOR_HEAD_REV_B sensors;

//pour Param Venant du PV
const int sizeParam = 50;
char  param[sizeParam];
volatile int indexParam = 0;
bool newParamFlag = false;

//pour Param Venant de l'IHM
const int sizeIHM = 50;
char  IHM[sizeIHM];
volatile int indexIHM = 0;
bool newIHMFlag = false;

//Flag PID ON / OFF
int FLAG_PID = 0;

//Flag pour interrompre l'affichage si on veut...
bool FLAG_AFF = false;

//Flag pour interrompre le traceur  si on veut...
bool FLAG_TRACE = false;

//Flag pour envoyer à l'app windev...
bool FLAG_WINDEV = false;

//Flag pour envoyer à l'IHM...
bool FLAG_DISPLAY = true;

//Flag pour interrompre l'enregistrement si on veut...
bool FLAG_REC = true;

//Flag pour interrompre les demandes O2 en cours de calibration...
bool FLAG_O2 = true;

//Flag si on débranche l'USB
bool ALIM_UNPLUGGED = false;

//Variables de stockage des infos capteurs
int co2 = 0;
float pression = 0;
float Temp1 = 0;
int ppO2 = 0;
int CellO2_1 = 0;
int CellO2_2 = 0;

//Variables et constantes OTU
Ticker OTU_Ticker;
float OTU = 0;
float EXP_OTU = 0.83;

//Mesure du temps d'éxecution du loop
Timer REAL_RATE;
float RATE = 0;
float RATE_TRUE = 0;
float Ref_Time = 2.0; //La durée de la boucle désirée...

//HTU21D sur l'I2C
HTU21D temphumid(PB_9, PB_8); //Temp humid sensor || SDA, SCL
float Temp2;
int Humid;

//Data LOG
char to_store[50];
time_t seconds;
char Log_File_Name[] = "                           ";
int myCount = 0;
int Max_Log_Size = 1800;//1 heure, 1800 lignes donc à multiplier par la fréquence d'enregistrement, Ref_Time, pour voir combien de temps représente un Log...

//Contrôle des servos
float Consigne_poumon = HOME_SERVO_POUMON;
float volet_poumon_Position;

float Consigne_fuite = HOME_SERVO_FUITE;
float volet_fuite_Position;

float Volets_Speed = 1;
float Volet_DeadBand = 0;

//Paramètre du PID
float Kc = 40;
float Ti = 1;
float Td = 0;
float RATE_PID = 1.0;
float Commande_PID;
int consigne = 210;
float Max_Input = 1260;
float Min_Input = 0;
float Borne = 1; //ce sont des pourcentages
float Max_Output = 100 - Borne;//on laisse en pourcentage et on calcule l'angle après le compute
float Min_Output = 0 + Borne;

//Init PID
PID control_Servo(Kc, Ti, Td, RATE_PID);

//Boolean du status de l'appareil, en mode SECU ou nominal
bool EN_MODE_SECU = false;

//Test voltage piles
AnalogIn V_PILES_ANALOG(PIN_V_PILES_ANALOG);

int Vusb = 1;
int VPiles = 1;
float VPiles_val = 1;

bool flag_USB = false;
bool flag_Piles = false;

//Commande à envoyer à l'IHM
//0 -> Marche normale
//1 -> Mise en veille
//2 -> Perte alim
//3 -> Big problème
//4 -> Reset
int Commande_IHM = 0;

//Pin connexion USB et Piles
//DigitalIn vusb(PIN_V_USB_DIGI);
//DigitalIn vpiles(PIN_V_PILES_DIGI);

//Interruption pin
InterruptIn vpiles_off_on(PIN_V_PILES_DIGI);
InterruptIn vusb_off_on(PIN_V_USB_DIGI);

//Pin enable du régulateur 5 v
DigitalOut E5V(PA_4);

//Pin enable du régulateur 3.3 v
DigitalOut E3V(PB_11, PullDown);

//Pin du Buzzer
Beep buzzer(PC_8);

//Déclaration des Threads
Thread thread_Secu;
Thread thread_Head;
Thread thread_Volets_POUMON;
Thread thread_Volets_FUITE;
Thread thread_PID;

//Récap des fonctions
void Save_Pos();//Sauvegarde des positions moteurs
void Mode_SECU();//Mise en secu des moteurs
void Sleep_IHM();//Mise en veille IHM, envoie de la consigne sur son port série
void Sleep_HIGH_WakeUp();//Mise en veille MCU avec réveille sur front haut
void Sleep_LOW_WakeUp();//Mise en veille MCU avec réveille sur front bas. BABEFACE
void ALIM_is_unplugged();//Appelée par l'intéruption. Passe juste un FLAG à false
void ALIM_unplugged();//Appelée par le sécu THREAD si le FLAG ci-dessus passe a false
void Stop_Sequence();//Enchainement des fonction amenant à l'arrêt de l'appareil
void Affichage();//Affichage complet du Debug dans le Terminal...ne pas utiliser car trop lent...préférer le même sur l'IHM
void Traceur_Arduino();//Envoies des Data pour être interprétées par le traceur Arduino
int Power_Test(DigitalIn& pin);//Test une pin Digital..Renvoie 1 ou 0. Pas utilisée, on utilise les intérruptions
float Power_Test(AnalogIn& pin);//Lecture d'une pin analogique. Pas encore utilisée
bool Check();//A remplir de paramètres qui renvoie true s'ils sont bon, false s'ils sont hors limites
void Calcul_OTU();//Calcul des OTU, commandée par un Tcker toutes les 60 s
void SENSORS_thread();//Thread d'intérogation des capteurs. Appelé tous les ref_time
void GO_TO_thread_POUMON();//Thread d'asservissement en position du volet Poumon. Appelé en permanence, priorité haute
void GO_TO_thread_FUITE();//Thread d'asservissement en position du volet Fuite. Appelé en permanence, priorité haute
void SECU_thread();//Thread de contrôle des paramètres devant entrainer une réaction de sécurité. Pour le moment capture du Flag de l'alim débranchée
void PID_thread();//Thread de calcul du PID
void callbackParam();//Callback de l'intéruption des com sur le port USD
void callbackIHM();//Callback de l'intéruption des com sur le port IHM
void Decoding_Message(char message []);//Décodage des message reçu par le port USB
void Decoding_IHM(char message []);//Décodage des message reçu par le port IHM. Uniquement le transfert sur clef USB pour le moment
void Create_File_Name_Date();//Création d'un nom de fichier par date
void Create_File_Name_Index();//Création d'un nom de fichier par Index

void Save_Pos()
{

    //Enregistrement position des servos sans mettre en sécu
    UTILS::EffacePage(2, PAGE_POUMON_ADDR, 1);

    int Pos_P = Servo_Poumon.getPulses();
    UTILS::Write(VOLET_POUMON_ADDR, Pos_P);
    DEBUG("  position volet poumon sauvegardée = %d pulse(s)\r\n", Pos_P);

    UTILS::EffacePage(2, PAGE_FUITE_ADDR, 1);

    int Pos_F = Servo_Fuite.getPulses();
    UTILS::Write(VOLET_FUITE_ADDR, Pos_F);
    DEBUG("  position volet fuite sauvegardée = %d pulse(s)\r\n", Pos_F);

}

//Passage en mode SECU
void Mode_SECU()
{

    //Mise du PID en mode manuel (desactivation...)
    FLAG_PID = 0;

    //On coupe les thread car on va utiliser une autre fonction
    thread_Volets_POUMON.terminate();
    thread_Volets_FUITE.terminate();

    //A fond et avec une erreur tolérée de +- 5 deg autour du Home. Cohérent avec le future init qui recherche une erreur > 5 deg
    Volets_Speed = 1;
    Volet_DeadBand = 1;

    Consigne_poumon = HOME_SERVO_POUMON;
    Consigne_fuite = HOME_SERVO_FUITE;

    Servo_Fuite.Go_To(Consigne_fuite, Volets_Speed, Volet_DeadBand);

    Servo_Poumon.Go_To(Consigne_poumon, Volets_Speed, Volet_DeadBand);

    Save_Pos();

    OUTPUT("  Volet poumon en sécu\r\n");
    OUTPUT("  Volet fuite en sécu\r\n");

    Servo_Poumon.Sleep();
    Servo_Fuite.Sleep();
}

void Sleep_IHM(int commande)
{
    //Mise en veille de l'IHM
    IHM("<0/0/0 0-0-0 ;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;%d>\r\n", commande);

    wait(0.5);

    HAL_GPIO_DeInit(GPIOC, GPIO_PIN_4|GPIO_PIN_5);

    // mise à zero UART IHM pour éviter son redémarrage intempestif (le tx suffirait)
    HAL_PWREx_EnablePullUpPullDownConfig();
    HAL_PWREx_EnableGPIOPullDown(PWR_GPIO_C, PWR_GPIO_BIT_4);
    HAL_PWREx_EnableGPIOPullDown(PWR_GPIO_C, PWR_GPIO_BIT_5);

}

void Alarm_IHM()
{
    IHM("<0/0/0 0-0-0 ;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;%d>\r\n", 6);
    }
    
void Sleep_HIGH_WakeUp()
{

    E5V = 0;

    UTILS::UnMount_Flash();

    DEBUG("  Mise en veille...\r\n");

    //Mise en veille
    HAL_PWREx_EnablePullUpPullDownConfig() ;
    HAL_PWREx_EnableGPIOPullDown(PWR_GPIO_A, PWR_GPIO_BIT_0);
    HAL_PWR_DisableWakeUpPin(PWR_WAKEUP_PIN1); // pour PA_0
    // Clear wake up Flag
    __HAL_PWR_CLEAR_FLAG(PWR_FLAG_WUF1);
    // Enable wakeup pin WKUP2
    HAL_PWR_EnableWakeUpPin(PWR_WAKEUP_PIN1); // high est la valeur par défaut
    // Set RTC back-up register RTC_BKP31R to indicate
    //later on that system has entered shutdown mode
    WRITE_REG( RTC->BKP31R, 0x1 );

    //Enter shutdown mode
    E3V=0;
    //n’est pas exécuté à cause du E3V=0 précédent
    HAL_PWREx_EnterSHUTDOWNMode();

}

void Sleep_LOW_WakeUp()
{
    E5V = 0;

    UTILS::UnMount_Flash();

    DEBUG("  Mise en veille...\r\n");

    UTILS::EffacePage(2, PAGE_FLAG_ADDR, 1);

    UTILS::Write(FLAG_ADDR, 0xBABEFACE);

    //Mise en veille
    HAL_PWREx_EnablePullUpPullDownConfig();
    HAL_PWREx_EnableGPIOPullDown(PWR_GPIO_A, PWR_GPIO_BIT_0);
    HAL_PWR_DisableWakeUpPin(PWR_WAKEUP_PIN1_LOW); // pour PA_0
    // Clear wake up Flag
    __HAL_PWR_CLEAR_FLAG(PWR_FLAG_WUF1);
    // Enable wakeup pin WKUP2
    HAL_PWR_EnableWakeUpPin(PWR_WAKEUP_PIN1_LOW); // high est la valeur par défaut
    // Set RTC back-up register RTC_BKP31R to indicate
    //later on that system has entered shutdown mode
    WRITE_REG( RTC->BKP31R, 0x1 );

    //Enter shutdown mode
    E3V=0;
    //Enter shutdown mode
    HAL_PWREx_EnterSHUTDOWNMode();
}

void ALIM_is_unplugged()
{
    ALIM_UNPLUGGED = true;
}

void ALIM_unplugged()
{
    DEBUG("  L'alimentation a été débranchée.\n");

    buzzer.beep(1000,0.1);

    Sleep_IHM(2);

    Mode_SECU();

    Sleep_HIGH_WakeUp();

}

//Sequence d'arrêt demandé par commande "sleep"
void Stop_Sequence()
{
    OUTPUT("  Mise en veille de l'appareil.\r\n");
    
    buzzer.beep(1000,0.1);

    Sleep_IHM(1);

    Mode_SECU();

    Sleep_LOW_WakeUp();
}

void Affichage()
{
    //serialMonit.printf("\r\n");
    serialMonit.printf("  CO2             = %d ppm\r\n"  , co2);
    serialMonit.printf("  PPO2            = %d mb\r\n", ppO2);
    serialMonit.printf("  OTU             = %d \r\n", (int)OTU);
    serialMonit.printf("  Pression        = %f msw\r\n", pression);
    serialMonit.printf("  Temp MS5837     = %f C\r\n", Temp1);
    serialMonit.printf("  Temp HTU21D     = %f C\n\r", Temp2);
    serialMonit.printf("  Humidity        = %d %%\n\r", Humid);
    //serialMonit.printf("\n\r");
    serialMonit.printf("  Cell O2 n 1     = %d\r\n"  , CellO2_1);
    serialMonit.printf("  Cell O2 n 2     = %d\r\n"  , CellO2_2);
    //serialMonit.printf("\r\n");
    serialMonit.printf("  Volet Poumon    = %f\r\n"  , volet_poumon_Position);
    serialMonit.printf("  Volet Fuite     = %f\r\n"  , volet_fuite_Position);
    //serialMonit.printf("\r\n");
    serialMonit.printf("  Temps d execution de la boucle = %f seconde(s)\r\n", (RATE + RATE_TRUE / 1000));
    //serialMonit.printf("\r\n");
    if (FLAG_REC) serialMonit.printf("  Chaine enregistrée = %s\r\n", to_store);
    else serialMonit.printf("  Pas d'enregistrement en cours.");
    //serialMonit.printf("\r\n");
    serialMonit.printf("  V_USB = %f\r\n", Vusb);
    //serialMonit.printf("\r\n\r\n");
    fflush(stdout);
}

void Traceur_Arduino()
{
    /*
        //Important pour PID
        TRACEUR("%d, ", ppO2);
        TRACEUR("%d, ", consigne);

        //TODO à commenter je pense...
        TRACEUR("%d, "  , co2);
        TRACEUR("%d, ", (int)OTU);
        TRACEUR("%f, ", pression);
        TRACEUR("%f, ", Temp1);
        TRACEUR("%f, ", Temp2);
        TRACEUR("%d, ", Humid);
        TRACEUR("%d, "  , CellO2_1);
        TRACEUR("%d, "  , CellO2_2);
        TRACEUR("%f, "  , volet_poumon_Position);
        TRACEUR("%f, "  , volet_fuite_Position);


        //Toujours finir par \r\n
        TRACEUR("\r\n");
    */
    //Important pour PID
    TRACEUR_DEBUG("%d, ", ppO2);
    TRACEUR_DEBUG("%d, ", consigne);
    TRACEUR_DEBUG("%f, "  , volet_poumon_Position);

    /*
    //TODO à commenter je pense...
    TRACEUR_DEBUG("%d, "  , co2);
    TRACEUR_DEBUG("%d, ", (int)OTU);
    TRACEUR_DEBUG("%f, ", pression);
    TRACEUR_DEBUG("%f, ", Temp1);
    TRACEUR_DEBUG("%f, ", Temp2);
    TRACEUR_DEBUG("%d, ", Humid);
    TRACEUR_DEBUG("%d, "  , CellO2_1);
    TRACEUR_DEBUG("%d, "  , CellO2_2);
    TRACEUR_DEBUG("%f, "  , volet_poumon_Position);
    TRACEUR_DEBUG("%f, "  , volet_fuite_Position);
    */

    //Toujours finir par \r\n
    TRACEUR_DEBUG("\r\n");
}


//Fonction test de valeur d'entrée digitale
int Power_Test(DigitalIn& pin)
{
    pin.mode(PullDown);

    if(!pin) {
        //Stop_Sequence();
        return 0;
    } else {
        return 1;
    }
}

//Fonction test de valeur d'entrée analogique
float Power_Test(AnalogIn& pin)
{
    float Val = pin.read();
    return Val;
}

//Contrôle du status de l'appareil / des constantes
bool Check()
{
    if (ppO2 > 100)
        return true;//Situation OK
    else
        return false;//Situation dégradée
}

//Calcul des OTU
void Calcul_OTU()
{
    /*
    La formule suivante permet de calculer la quantité d' OTU accumulée
    OTU = T * (2* PpO2 -1)0,83
    avec :
    T = temps de plongée en minutes
    PpO2 = pression partielle d’ oxygène en bars
    */

    if (ppO2 > 500) {
        float val = (2 * (float)ppO2/1000 - 1);//je divise par 1000 car la PP est en mb...
        OTU += pow(val, EXP_OTU);//T = 1 car le ticker est executé toutes les 60 secondes, 1 minute....
    }
}

//Thread d'intérogation des capteurs, positions servo
void SENSORS_thread()
{
    while (true) {
        //DEEP_DEBUG("  SENSORS_thread\r\n");

        //CO2 sur Cozir
        co2 = sensors.requestCO2();
        //P / T sur MS5837
        pression = sensors.requestPress();
        Temp1 =  sensors.requestTemp();
        //PPO2 sur ADS1015
        if (FLAG_O2) ppO2 = sensors.requestPpO2();
        //Cell O2 en mV
        if (FLAG_O2) CellO2_1 = sensors.requestCellO2_1();
        if (FLAG_O2) CellO2_2 = sensors.requestCellO2_2();

        //HTU21D
        Temp2 = temphumid.sample_ctemp();
        Humid = temphumid.sample_humid();

        //Retour position des servos
        volet_poumon_Position = Servo_Poumon.Get_Position();
        volet_fuite_Position = Servo_Fuite.Get_Position();

        //Calcul des OTU
        //Calcul_OTU();
        wait(Ref_Time);
    }
}

void GO_TO_thread_POUMON()
{
    while (true) {
        //DEEP_DEBUG("  GO_TO_Thread_POUMON\r\n");

        Servo_Poumon.Go_To_Prop(&serialMonit, Consigne_poumon);
        //Servo_Poumon.Go_To_PID(Consigne_poumon, Volet_DeadBand);//Nécessite de déclarer un dernier terme true dans l'init du moteur
        //Servo_Poumon.Go_To(Consigne_poumon, Volets_Speed, Volet_DeadBand);

    }
}

void GO_TO_thread_FUITE()
{
    while (true) {
        //DEEP_DEBUG("  GO_TO_Thread_FUITE\r\n");

        Servo_Fuite.Go_To_Prop(&serialMonit, Consigne_fuite);
        //Servo_Fuite.Go_To_PID(Consigne_fuite, Volet_DeadBand);
        //Servo_Fuite.Go_To(Consigne_fuite, Volets_Speed, Volet_DeadBand);

    }
}

void SECU_thread()
{
    while (true) {
        //Mettre toutes les vérifs de sécu....
        if (ALIM_UNPLUGGED) ALIM_unplugged();
    }
}

void PID_thread()
{
    while (true) {
        if (FLAG_PID == 1 && EN_MODE_SECU == false) {
            control_Servo.setProcessValue(ppO2);
            float pid_output = control_Servo.compute();
            // on détermine le output comme un débit entre 0 et 100% et on fait le calcul d'angle ensuite
            // on suppose pour simplifier que le débit est proportionnel à la surface projeté du papillon sur un plan perpendiculaire à la section du tuyau
            // exemple pour 50% de débit, l'angle à ouvrir est de Acos(0.5)=60degrés 
            float angle = 90 - acos(1-pid_output/100)*180/3.14159265f;
            //DEBUG("\r\n  pid_ouput = %f  \r\n", pid_output);
            //Nouvelle sortie servo poumon si on est pas en mode SECU
            Consigne_poumon = angle;//;
            // mode volets asservis (simule un seul moteur)
            Consigne_fuite = 90 - Consigne_poumon; //control_Servo.compute();
            wait(RATE_PID);
        }
    }
}

//Callback de l'intérruption des envois de commandes depuis le terminal / WINDEV
void callbackParam()
{

    while(serialMonit.readable()) {
        if ((indexParam  == sizeParam) || newParamFlag  == true) { //éviter la saturation du buffer
            char char_flush = serialMonit.getc();
        } else {
            param [indexParam ++] = serialMonit.getc();//chargement du buffer dans le message
            if ((indexParam == sizeParam) || (param[indexParam - 1] == '\n')) {//le message est complet ou nouvelle ligne ou autre si on veut...
                param[indexParam] = 0;
                newParamFlag  = true;
            }
        }
    }
}

//Callback de l'intérruption des envois de commandes depuis l'IHM
void callbackIHM()
{

    while(display.readable()) {
        if ((indexIHM  == sizeIHM) || newIHMFlag  == true) { //éviter la saturation du buffer
            NVIC_DisableIRQ(USART3_IRQn);
            char char_flush = display.getc();
            NVIC_EnableIRQ(USART3_IRQn);
        } else {
            NVIC_DisableIRQ(USART3_IRQn);
            IHM [indexIHM ++] = display.getc();//chargement du buffer dans le message
            if ((indexIHM == sizeIHM) || (IHM[indexIHM - 1] == '\n')) {//le message est complet ou nouvelle ligne ou autre si on veut...
                IHM[indexIHM] = 0;
                newIHMFlag  = true;
            }
            NVIC_EnableIRQ(USART3_IRQn);
        }
    }
}

void Decoding_Message(char message [])
{

    char com[20] = "";
    char numb[30] = "";

    sscanf(message,"%s %s",&com , &numb);

    DEBUG("\r\n  Commande = %s       Valeur = %s \r\n\r\n", com, numb);

    OUTPUT("?\r\n");

    if (0 == strcmp(com, "secu")) {
        Mode_SECU();
    } else if (0 == strcmp(com, "save_pos")) {
        Save_Pos();
    } else if (0 == strcmp(com, "trace")) {
        if (FLAG_TRACE) {
            FLAG_TRACE = false;
            OUTPUT("  Arrêt du tracé Arduino.\r\n");
        } else {
            FLAG_TRACE = true;
            OUTPUT("  Démarrage du tracé Arduino.\r\n");
        }
    } else if (0 == strcmp(com, "ping")) {
        E3V = 0;
    } else if (0 == strcmp(com, "res_ihm")) {
        IHM("<0/0/0 0-0-0 ;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;%d>\r\n", 4);
    } else if (0 == strcmp(com, "page_ihm")) {
        IHM("<0/0/0 0-0-0 ;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;%d>\r\n", 5);
    } else if (0 == strcmp(com, "alarm")) {
        Commande_IHM = 6;
    } else if (0 == strcmp(com, "alarm_fat")) {
        Commande_IHM = 7;    
    } else if (0 == strcmp(com, "ack_alarm")) {
        Commande_IHM = 8;               
    } else if (0 == strcmp(com, "arnsrs_id")) {
        OUTPUT("  Changement de l'ID de l'appareil pour le N°: %s\r\n", numb);
        //UTILS::Store_A_Val(atoi(numb), "ARNSRS_ID");
        UTILS::EffacePage(2, PAGE_ID_ADDR, 1);
        UTILS::Write(ID_ARNSRS_ADDR, (uint64_t) atoi(numb));
    } else if (0 == strcmp(com, "monit")) {
        FLAG_AFF = false;
        FLAG_WINDEV = true;
    } else if (0 == strcmp(com, "debug")) {
        FLAG_AFF = false;
        FLAG_WINDEV = false;
    } else if (0 == strcmp(com, "head_id")) {
        //On l'enregistre dans l'eeprom
        OUTPUT("Changement de l'ID de la tête capteur pour le N°: %s\r\n", numb);
        UTILS::write_EEPROM(numb, HEAD_ID);
    } else if (0 == strcmp(com, "o2_1_id")) {
        //On l'enregistre dans l'eeprom
        UTILS::write_EEPROM(numb, CELL_O2_1_ID);
    } else if (0 == strcmp(com, "o2_2_id")) {
        //On l'enregistre dans l'eeprom
        UTILS::write_EEPROM(numb, CELL_O2_2_ID);
    } else if (0 == strcmp(com, "co2_id")) {
        //On l'enregistre dans l'eeprom
        UTILS::write_EEPROM(numb, CO2_ID);
    } else if (0 == strcmp(com, "calib_o2")) {
        FLAG_O2 = false;
        wait_ms(100);
        float Val =  sensors.Calibrate_O2(atoi(numb));
        OUTPUT("  Calibration O2 dans l'air = %f\r\n", Val);
        wait_ms(100);
        FLAG_O2 = true;
    } else if (0 == strcmp(com, "calib_co2")) {
        int Val = sensors.Calibrate_CO2();
        OUTPUT("  Calibration CO2 dans l'air = %d\r\n", Val);
    } else if (0 == strcmp(com, "flash_i")) {
        UTILS::Flash_Infos(&serialMonit);
    } else if (0 == strcmp(com, "flash_u")) {
        OUTPUT("  Démontage de la Flash.\r\n");
        FLAG_REC = false;
        UTILS::UnMount_Flash();
    } else if (0 == strcmp(com, "flash_m")) {
        OUTPUT("  Montage de la Flash.\r\n");
        UTILS::Mount_Flash();
    } else if (0 == strcmp(com, "check_f")) {
        OUTPUT("  ARNSRS ID   = %d\r\n", (int) UTILS::Read(ID_ARNSRS_ADDR));
        OUTPUT("  Dernière Position volet poumon enregistrée  = %d\r\n", (int) UTILS::Read(VOLET_POUMON_ADDR));
        OUTPUT("  Dernière Position volet fuite enregistrée   = %d\r\n", (int) UTILS::Read(VOLET_FUITE_ADDR));
    } else if (0 == strcmp(com, "check_E")) {
        sensors.Sensor_head_check(&serialMonit);
    } else if (0 == strcmp(com, "rec")) {
        if (FLAG_REC) {
            FLAG_REC = false;
            OUTPUT("  Arrêt du Data Logging.\r\n");
        } else {
            FLAG_REC = true;
            OUTPUT("  Démarrage Data Logging dans %s\r\n", Log_File_Name);
        }
    } else if (0 == strcmp(com, "help")) {
        FLAG_AFF = false;
        UTILS::Help(&serialMonit);
    } else if (0 == strcmp(com, "start")) {
        FLAG_AFF = true;
    } else if (0 == strcmp(com, "stop")) {
        FLAG_AFF = false;
        //UTILS::Help();
    } else if (0 == strcmp(com, "flash_c")) {
        FLAG_REC = false;
        UTILS::Clean_Flash();
    } else if (0 == strcmp(com, "flash_f")) {
        FLAG_REC = false;
        UTILS::Clean_Flash_All();
    } else if (0 == strcmp(com, "dir")) {
        FLAG_WINDEV = false;
        wait(0.5);
        OUTPUT("$\r\n");
        wait(0.5);
        UTILS::Dir_Flash(&serialMonit);
    } else if (0 == strcmp(com, "get")) {
        wait_ms(100);
        char filename[20];
        UTILS::Read_Flash_File(&serialMonit, numb);
        wait_ms(100);
    } else if (0 == strcmp(com, "del")) {
        FLAG_REC = false;
        char filename[20];
        UTILS::Delete_Flash_File(numb);
        OUTPUT("$\r\n");
        wait(0.5);
        UTILS::Dir_Flash(&serialMonit);
        FLAG_REC = true;
    } else if (0 == strcmp(com, "file_s")) {
        char filename[20];
        UTILS::Get_File_Size(&serialMonit, numb);
    } else if (0 == strcmp(com, "calib_pou")) {
        volet_poumon_Position = 0;
        Servo_Poumon.reset();
        Consigne_poumon = HOME_SERVO_POUMON;
        OUTPUT("  Volet poumon Calibré.\r\n");
    } else if (0 == strcmp(com, "calib_fui")) {
        volet_fuite_Position = 0;
        Servo_Fuite.reset();
        Consigne_fuite = HOME_SERVO_FUITE;
        OUTPUT("  Volet fuite Calibré.\r\n");
    } else if (0 == strcmp(com, "sleep")) {
        Stop_Sequence();
    } else if (0 == strcmp(com, "time")) {//Depuis terminal MAC taper : " date +%s "
        set_time(atoi(numb));
        OUTPUT("  La RTC a été mise à l'heure.\r\n");
    } else if (0 == strcmp(com, "c_pou")) {
        Consigne_poumon = atof(numb);
        DEBUG("  Servo Poumon    = %f\r\n", Consigne_poumon);
    } else if (0 == strcmp(com, "c_fui")) {
        Consigne_fuite = atof(numb);
        DEBUG("  Servo Fuite     =  %f\r\n", Consigne_fuite);
    } else if (0 == strcmp(com, "mp_pou")) {
        Consigne_poumon += atof(numb);
        DEBUG("  Servo Poumon    = %f\r\n", Consigne_poumon);
    } else if (0 == strcmp(com, "mp_fui")) {
        Consigne_fuite += atof(numb);
        DEBUG("  Servo Fuite     =  %f\r\n", Consigne_fuite);
    } else if (0 == strcmp(com, "reset")) {
        FLAG_REC = false;
        //Mode_SECU();
        UTILS::UnMount_Flash();
        OUTPUT("  Reset de l'appareil.\r\n");
        wait(1);
        NVIC_SystemReset();
    } else if (0 == strcmp(com, "pid_r")) {
        control_Servo.reset();
        OUTPUT("  RESET PID\r\n");    
    } else if (0 == strcmp(com, "kc")) {
        Kc = atof(numb);
        //control_Servo.reset();
        control_Servo.setTunings(Kc, Ti, Td);
        OUTPUT("  MAJ PID --  Kc = %f   Ti = %f   Td = %f\r\n", Kc, Ti, Td);
    } else if (0 == strcmp(com, "ti")) {
        Ti = atof(numb);
        //control_Servo.reset();
        control_Servo.setTunings(Kc, Ti, Td);
        OUTPUT("  MAJ PID --  Kc = %f   Ti = %f   Td = %f\r\n", Kc, Ti, Td);
    } else if (0 == strcmp(com, "td")) {
        Td = atof(numb);
        //control_Servo.reset();
        control_Servo.setTunings(Kc, Ti, Td);
        OUTPUT("  MAJ PID --  Kc = %f   Ti = %f   Td = %f\r\n", Kc, Ti, Td);
    } else if (0 == strcmp(com, "cons")) {
        consigne = atoi(numb);
        control_Servo.setSetPoint(consigne);
        OUTPUT("  MAJ CONSIGNE PID --  Consigne = %d\r\n", consigne);
    } else if (0 == strcmp(com, "rate")) {
        RATE_PID = atof(numb);
        //control_Servo.reset();
        control_Servo.setInterval(RATE_PID);
        OUTPUT("  MAJ RATE PID --  Rate = %f\r\n", RATE_PID);
    } else if (0 == strcmp(com, "bornes")) {
        Borne = atof(numb);
        Min_Output = 0 + Borne;
        Max_Output = 90 - Borne;
        control_Servo.setOutputLimits(Min_Output, Max_Output);
        OUTPUT("  MAJ LIMITE OUTPUT PID --  Min_Output = %f   Max_Output = %f\r\n", Min_Output, Max_Output);    
    } else if (0 == strcmp(com, "dfu")) {
        OUTPUT("  Passage en DFU...\r\n");
        FLAG_REC = false;
        Mode_SECU();
        UTILS::UnMount_Flash();
        UTILS::EffacePage(2, PAGE_FLAG_ADDR, 1);
        UTILS::Write(FLAG_ADDR, 0xDEADBEEF);
        wait(1);
        NVIC_SystemReset();
    } else if (0 == strcmp(com, "pid")) {
        if (FLAG_PID == 1) {
            OUTPUT("  PID OFF\r\n");
            FLAG_PID = 0;
        } else if (FLAG_PID == 0) {
            OUTPUT("  PID ON\r\n");
            FLAG_PID = 1;
        }
    } else {
        OUTPUT("  Commande COZIR, commande érronée....\r\n");
        sensors.cozirSend(message);
    }

    strcpy(param," ");
    indexParam = 0;
    newParamFlag = false;
}

void Decoding_IHM(char message [])
{

    char com[20] = "";
    char numb[30] = "";

    sscanf(message,"%s %s",&com , &numb);

    DEEP_DEBUG("\r\n  Commande = %s       Valeur = %s \r\n\r\n", com, numb);

   // OUTPUT("?\r\n");

    if (0 == strcmp(com, "USB")) {
        OUTPUT("  Passage en Mode Transfert Flash To USB...\r\n");
        FLAG_REC = false;
        Mode_SECU();
        UTILS::UnMount_Flash();
        UTILS::FLASH_set_boot_bank(2);
        NVIC_SystemReset();
    } else if (0 == strcmp(com, "res_otu")) {
        OTU = 0;
    } else if (0 == strcmp(com, "cons")) {
        consigne = atoi(numb);
        control_Servo.setSetPoint(consigne);
        OUTPUT("  MAJ CONSIGNE PID --  Consigne = %d\r\n", consigne);    
    } else if (0 == strcmp(com, "reset")) {
        NVIC_SystemReset();
    } else if (0 == strcmp(com, "sleep")) {
        Stop_Sequence();
    }

    strcpy(IHM," ");
    indexIHM = 0;
    newIHMFlag = false;
}

void Create_File_Name_Date()
{
    //Du nom du fichier Date / heure
    seconds = time(NULL);
    char Time[40];
    strftime(Time, 40, "%a_%d_%m_%Y_%H%M", localtime(&seconds));
    sprintf(Log_File_Name, "%s_LOG.csv", Time);
    DEBUG("  Nouveau fichier LOG = %s \r\n", Log_File_Name);
}

void Create_File_Name_Index()
{
    //Du nom du fichier par Index
    sprintf(Log_File_Name, "LOG_%d.csv", UTILS::File_Index());
    DEBUG("  Nouveau fichier LOG = %s \r\n", Log_File_Name);
}


int main()
{

    HAL_Init();
    __HAL_RCC_PWR_CLK_ENABLE();
    HAL_PWR_EnableBkUpAccess();

    //Vérifications Flag de démarrage
    UTILS::START_FLAG();

    E5V = 1;
    E3V = 1;

    if (PIN_V_USB_DIGI) flag_USB = true;
    if (PIN_V_PILES_DIGI) flag_Piles = true;

    //UTILS::PVD_Config();

    buzzer.beep(1000,0.5);

    //Ci-dessous commande pour formater une nouvelle carte
    //UTILS::Format_Flash();

    //Montage Flash
    UTILS::Mount_Flash();

    /*
        if (UTILS::File_Exist("ARNSRS_ID.sys") == false) {
            UTILS::Store_A_Val(000, "ARNSRS_ID.sys");
            DEBUG("ARNSRS ID forcée à 000\r\n");
        }
    */

    //pour le premier lancement quand l'appareil n'a pas d'ID.
    //int ID = 0;
    //UTILS::Write(ADRESSE_ID, ID);

    DEBUG("\r\n\r\n  Démarrage de l'appareil, veuillez patienter...\r\n\r\n");


    //Vérification RTC, si on est le 01/01/70, c'est qu'il y a un problème...
    seconds = time(NULL);
    char YEAR[10];
    strftime(YEAR, 10, "%D", localtime(&seconds));
    if (0 == strcmp(YEAR, " 01/01/70 "))
        DEBUG("  Vous devez régler la RTC...\r\n\r\n");

    bool calib_O2 = false;
    bool calib_CO2 = false;

    sensors.Sensors_INIT(calib_O2, calib_CO2);

    wait(1);

    //Création du nouveau fichier LOG par index / par date.
    Create_File_Name_Index();
    //Create_File_Name_Date();

    //Création et écriture du header du fichier LOG
    sensors.Create_Header(Log_File_Name);

    /*
    Servo_Poumon.Init("Servo_Poumon.sys");
    Servo_Fuite.Init("Servo_Fuite.sys");

    Servo_Poumon.Init(VOLET_POUMON_ADDR);
    Servo_Fuite.Init(VOLET_FUITE_ADDR);
    */

    DEBUG("  Demarrage des threads...\r\n\r\n");

    /*

    Pour mémoire, les réglage de priorité des thread

      osPriorityIdle          = -3,          ///< priority: idle (lowest)
      osPriorityLow           = -2,          ///< priority: low
      osPriorityBelowNormal   = -1,          ///< priority: below normal
      osPriorityNormal        =  0,          ///< priority: normal (default)
      osPriorityAboveNormal   = +1,          ///< priority: above normal
      osPriorityHigh          = +2,          ///< priority: high
      osPriorityRealtime      = +3,          ///< priority: realtime (highest)
      osPriorityError         =  0x84        ///< system cannot determine priority or thread has illegal priority
    */

    thread_Volets_POUMON.set_priority(osPriorityHigh);

    thread_Volets_POUMON.start(callback(GO_TO_thread_POUMON));

    DEBUG("  Contrôle volet Poumon démarré\r\n\r\n");

    wait(1);

    thread_Volets_FUITE.set_priority(osPriorityHigh);

    thread_Volets_FUITE.start(callback(GO_TO_thread_FUITE));

    DEBUG("  Contrôle volet Fuite démarré\r\n\r\n");

    wait(1);

    thread_Secu.set_priority(osPriorityNormal);

    thread_Secu.start(callback(SECU_thread));

    DEBUG("  Contrôle des paramètres de sécu démarré\r\n\r\n");

    wait(1);

    thread_Head.set_priority(osPriorityNormal);

    thread_Head.start(callback(SENSORS_thread));

    DEBUG("  Tête capteurs démarrée\r\n\r\n");

    wait(1);

    //Init PID
    //Entrée PPO2 entre 100 et 1000 mb
    control_Servo.setInputLimits(Min_Input, Max_Input);
    //Sortie servo entre 0 et 100 %
    control_Servo.setOutputLimits(Min_Output, Max_Output);
    //Reglage de l'interval du PID
    control_Servo.setInterval(RATE_PID);
    //Consigne à x mb
    control_Servo.setSetPoint(consigne);
    //Mode auto au démarrage
    control_Servo.setMode(AUTO_MODE);

    thread_PID.set_priority(osPriorityNormal);

    thread_PID.start(callback(PID_thread));

    DEBUG("  PID thread démarré\r\n\r\n");

    wait(1);

    DEBUG("  Cliquez sur le bouton help pour voir la liste des \r\n  commandes administrateur disponibles.\r\n");

    OUTPUT("$\r\n");
    wait(0.5);
    UTILS::Dir_Flash(&serialMonit);

    serialMonit.attach(&callbackParam);

    display.attach(&callbackIHM);

    OTU_Ticker.attach(&Calcul_OTU, 60);

    vusb_off_on.fall(&ALIM_is_unplugged);

    vpiles_off_on.fall(&ALIM_is_unplugged);
    
    IHM("<0/0/0 0-0-0 ;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;%d>\r\n", 4);
    
    buzzer.beep(1000,0.3);
    wait_ms(500);
    buzzer.beep(1000,0.3);

    while (true) {

        //Démarrage du Timer mesurant le temps d'éxecution du code
        REAL_RATE.start();

        if (newParamFlag) {
            DEEP_DEBUG("  From PC = %s\r\n", param);
            Decoding_Message(param);
        }

        if (newIHMFlag) {
            DEEP_DEBUG("  From IHM = %s\r\n", IHM);
            Decoding_IHM(IHM);
        }

        //Fabrication de la chaine Date / heure
        seconds = time(NULL);
        char Time_buf[32];
        strftime(Time_buf, 32, "%D %I-%M-%S ", localtime(&seconds));

        //Fabrication de la chaine de com
        sprintf(to_store,"<%s;%d;%d;%.1f;%d;%.2f;%.2f;%d;%d;%d;%.2f;%.2f;%d;%.3f;%.3f;%.3f;%d;%d>",
                Time_buf,
                co2,
                ppO2,
                pression,
                (int)OTU,
                Temp1,
                Temp2,
                Humid,
                CellO2_1,
                CellO2_2,
                volet_poumon_Position,
                volet_fuite_Position,
                FLAG_PID,
                Kc,
                Ti,
                Td,
                consigne,
                Commande_IHM
               );


        //Pour windev
        if (FLAG_WINDEV) {
            OUTPUT("%s\r\n", to_store);
        }

        //Pour l'IHM
        if (FLAG_DISPLAY) {
            IHM("%s\r\n", to_store);
        }

        //Vers le moniteur série
        if (FLAG_AFF) {
            Affichage();
        }

        //Vers le traceur série
        if (FLAG_TRACE) {
            Traceur_Arduino();
        }

        //Enregistrement de la chaine
        if (FLAG_REC) {
            UTILS::Write_Flash_File(to_store, Log_File_Name);
            myCount ++;
            if (myCount > Max_Log_Size) {
                Create_File_Name_Index();
                //Create_File_Name_Date();
                sensors.Create_Header(Log_File_Name);
                myCount = 0;
                OUTPUT("$\r\n");
                wait(0.5);
                UTILS::Dir_Flash(&serialMonit);
            }
        }

        /*
                //Update du PID
                if (FLAG_PID == 1 && EN_MODE_SECU == false) {
                    control_Servo.setProcessValue(ppO2);
                    // il faut sortir le output comme un débit entre 0 et 100% et faire le calcul d'angle ensuite
                    // on suppose pour simplifier que le débit est proportionnel à la surface projeté du papillon sur un plan perpendiculaire à al section du tuyau
                    // exemple pour 50% de débit, l'angle à ouvrir est de Acos(0.5)=60degrés 
                    float angle = 90 + acos(1-pid_output/100)*180/3.14159265f;
                    //Nouvelle sortie servo poumon si on est pas en mode SECU
                    Consigne_poumon = 90 - angle;
                    // mode volets asservis (simule un seul moteur)
                    Consigne_fuite = 90 - Consigne_poumon;
                }
        */

        //Arrêt du Timer mesurant le temps d'éxecution du code
        REAL_RATE.stop();
        //Définition de la nouvelle valeur du temps d'échantillonage du PID.
        RATE = REAL_RATE.read();
        //Reset du Timer
        REAL_RATE.reset();

        //Pour ralentir le code à Ref_Time seconde fixe quelque soit les intéruptions du loop....
        if (Ref_Time > RATE) {
            RATE_TRUE = (Ref_Time - RATE) * 1000;
        } else {
            RATE_TRUE = 0;

            DEEP_DEBUG("Pour ralentir le code, Ref_Time doit être supérieur à %f seconde(s)\r\n\n", RATE);
        }

        wait_ms(RATE_TRUE);
    }
}