Projet d'interfaçage - LSM6DS3 : Test de Stabilité Quentin NOIRET LP MECSE 2020 -------------------------------------------------- L’application réalisée permet de tester la stabilité d’un système (ex : drone) en affichant différents seuils d’inclinaison en fonction des valeurs de l’accéléromètre.
Dependencies: Interfacage_MECSE_LSM6DS3 mbed LSM6DS3
main.cpp
- Committer:
- raketitch
- Date:
- 2020-06-24
- Revision:
- 8:99a94555abb4
- Parent:
- 7:e5f8936d11af
File content as of revision 8:99a94555abb4:
// Projet interfaçage : LSM6DS3 Demo //Quentin NOIRET //LP MECSE - IUT de CACHAN //2020 //Bibliothèques #include "mbed.h" #include "LSM6DS3.h" //sensor #include "LCD_DISCO_F746NG.h" //LCD Screen #include "TS_DISCO_F746NG.h" //Touch Screen #include "logo.h" //logo IUT #include "math.h" //nécessaire aux calculs d'angle #include "F746_GUI.hpp" //bibliothèques d'interface //Constantes #define REFRESH_TIME_MS 50 #define M_PI 3.141593 #define SEUIL_1 5 #define SEUIL_2 20 #define SEUIL_3 45 #define UP_PADDING 15 #define LEFT_PADDING 15 LSM6DS3 sensor(PB_9, PB_8); //instance du capteur : (PIN_SCL, PIN_SDA) LCD_DISCO_F746NG lcd; //écran LCD TS_DISCO_F746NG ts; //écran tactile Serial pc(USBTX, USBRX); //liaison série //Prototypes des fonctions void setup(void); //initialisation du capteur void readSensor(void); //lecture du capteur void printSensor(void); //affichage des valeurs du capteur void drawImageLogo(int offsetX, int offsetY); //affichage du logo de l'IUT //Variables globales char text[30]; float accelX, accelY, accelZ, // units m/s/s i.e. accelZ if often 9.8 (gravity) gyroX, gyroY, gyroZ, // units dps (degrees per second) accRoll=-1, accPitch=-1; // units degrees (roll and pitch noisy, yaw not possible) int etat1=0, etat2=0; //variable d'etat using namespace std; //--------------------------------------------------------------------// //--------------------------- MAIN -----------------------------------// //--------------------------------------------------------------------// int main() { //Variables locales TS_StateTypeDef TS_State; //Etat de l'écran tactile uint16_t touch, oldTouch; //pour détecter les front montant d'un double appui bool touchFlag = false; //flag qui s'active quand on réalise un double appui //------------------------------------------------------// //------------------ INITIALISATION --------------------// //------------------------------------------------------// setup(); //------------------------------------------------------// //--------------- DEMARRAGE INTERFACE ------------------// //------------------------------------------------------// //------------------------------------------------------// //---------------------- ECRAN 1 -----------------------// //------------------------------------------------------// BSP_LCD_Clear(LCD_COLOR_WHITE); BSP_LCD_SetFont(&LCD_DEFAULT_FONT); BSP_LCD_SetBackColor(LCD_COLOR_WHITE); BSP_LCD_SetTextColor(LCD_COLOR_BLACK); BSP_LCD_DisplayStringAt(0, 1, (uint8_t *)"PROJET INTERFACAGE", CENTER_MODE); drawImageLogo(150,150); HAL_Delay(2000); //------------------------------------------------------// //---------------------- ECRAN 2 -----------------------// //------------------------------------------------------// BSP_LCD_Clear(LCD_COLOR_BLACK); BSP_LCD_DisplayStringAt(0, 1, (uint8_t *)"Toucher pour commencer", CENTER_MODE); //Attend que l'utilisateur touche l'écra, do { ts.GetState(&TS_State); }while(!TS_State.touchDetected); HAL_Delay(50); //------------------------------------------------------// //----------- DECLARATIONS OBJETS INTERFACE -----------// //------------------------------------------------------// Button btnStart(LEFT_PADDING, UP_PADDING+35, 100, 75, "Start"); Button btnStop(LEFT_PADDING+100+5, UP_PADDING+35, 100, 75, "Stop"); Button btnDebug_On(LEFT_PADDING+200+10, UP_PADDING+35, 100, 75, "Afficher Val"); Button btnDebug_Off(LEFT_PADDING+300+15, UP_PADDING+35, 100, 75, "Masquer Val"); Button btnReset(480-LEFT_PADDING-100, 272-UP_PADDING-50, 100, 50, "Reset"); Label lbTitre(240, 2, "Test de Stabilite", Label::CENTER, Font16); Label lbInfo(240, 20, "Poser 2 doigts sur l'ecran pour afficher/masquer le menu", Label::CENTER); Label lbSystem(260, 180, "", Label::CENTER, Font24); NumericLabel<float> lbl_ax(LEFT_PADDING, 150, "ax : %4.2f", accelX, Label::LEFT, Font12, LCD_COLOR_WHITE, LCD_COLOR_BLACK);//, Font12, Label::LCD_COLOR_BLACK, Label::LCD_COLOR_BLACK); NumericLabel<float> lbl_ay(LEFT_PADDING, 160, "ay : %4.2f", accelY, Label::LEFT, Font12, LCD_COLOR_WHITE, LCD_COLOR_BLACK); NumericLabel<float> lbl_az(LEFT_PADDING, 170, "az : %4.2f", accelZ, Label::LEFT, Font12, LCD_COLOR_WHITE, LCD_COLOR_BLACK); NumericLabel<float> lbl_roll(LEFT_PADDING, 180, "Roll : %4.2f", accRoll, Label::LEFT, Font12, LCD_COLOR_WHITE, LCD_COLOR_BLACK); NumericLabel<float> lbl_pitch(LEFT_PADDING, 190, "Pitch: %4.2f", accPitch, Label::LEFT, Font12, LCD_COLOR_WHITE, LCD_COLOR_BLACK); //NumericLabel<int> lbl_etat1(LEFT_PADDING, 200, "etat1: %d", etat1, Label::LEFT, Font12, LCD_COLOR_WHITE, LCD_COLOR_BLACK); //NumericLabel<int> lbl_etat2(LEFT_PADDING, 210, "etat2: %d", etat2, Label::LEFT, Font12, LCD_COLOR_WHITE, LCD_COLOR_BLACK); //------------------------------------------------------// //---------- INITIALISATION OBJETS INTERFACE ----------// //------------------------------------------------------// btnStart.Erase(); btnStop.Erase(); btnDebug_On.Erase(); btnDebug_Off.Erase(); btnReset.Erase(); btnStart.Activate(); btnStop.Inactivate(); btnDebug_On.Inactivate(); btnDebug_Off.Inactivate(); btnReset.Inactivate(); lbl_ax.Redraw(LCD_COLOR_BLACK); lbl_ay.Redraw(LCD_COLOR_BLACK); lbl_az.Redraw(LCD_COLOR_BLACK); lbl_roll.Redraw(LCD_COLOR_BLACK); lbl_pitch.Redraw(LCD_COLOR_BLACK); //lbl_etat1.Redraw(LCD_COLOR_BLACK); //lbl_etat2.Redraw(LCD_COLOR_BLACK); HAL_Delay(500); btnReset.Activate(); //BOUCLE INFINI while (true) { ts.GetState(&TS_State);//lecture ecran touch = TS_State.touchDetected; //gestion interface switch (etat1) { case 0: //idle if(btnStart.Touched()) { etat1 = 1; btnStart.Inactivate(); btnStop.Activate(); btnDebug_On.Activate(); } break; case 1://started masquer valeurs readSensor(); accRoll = atan2(accelY, accelZ) * 180/M_PI; accPitch = atan2(-accelX, sqrt(accelY*accelY + accelZ*accelZ)) * 180/M_PI; if (btnDebug_On.Touched()) { btnDebug_On.Inactivate(); btnDebug_Off.Activate(); etat1 = 2; } if(btnStop.Touched()) { etat1 = 0; btnStart.Activate(); btnStop.Inactivate(); btnDebug_On.Inactivate(); btnDebug_Off.Inactivate(); } break; case 2 ://Afficher valeurs readSensor(); accRoll = atan2(accelY, accelZ) * 180/M_PI; accPitch = atan2(-accelX, sqrt(accelY*accelY + accelZ*accelZ)) * 180/M_PI; lbl_ax.Draw(accelX); lbl_ay.Draw(accelY); lbl_az.Draw(accelZ); lbl_roll.Draw(accRoll); lbl_pitch.Draw(accPitch); //lbl_etat1.Draw(etat1); //lbl_etat2.Draw(etat2); if(btnDebug_Off.Touched()) { etat1 = 1; btnDebug_Off.Inactivate(); btnDebug_On.Activate(); lbl_ax.Redraw(LCD_COLOR_BLACK); lbl_ay.Redraw(LCD_COLOR_BLACK); lbl_az.Redraw(LCD_COLOR_BLACK); lbl_roll.Redraw(LCD_COLOR_BLACK); lbl_pitch.Redraw(LCD_COLOR_BLACK); //lbl_etat1.Redraw(LCD_COLOR_BLACK); //lbl_etat2.Redraw(LCD_COLOR_BLACK); } if(btnStop.Touched()) { etat1 = 0; btnStart.Activate(); btnStop.Inactivate(); btnDebug_On.Inactivate(); btnDebug_Off.Inactivate(); lbl_ax.Redraw(LCD_COLOR_BLACK); lbl_ay.Redraw(LCD_COLOR_BLACK); lbl_az.Redraw(LCD_COLOR_BLACK); lbl_roll.Redraw(LCD_COLOR_BLACK); lbl_pitch.Redraw(LCD_COLOR_BLACK); //lbl_etat1.Redraw(LCD_COLOR_BLACK); //lbl_etat2.Redraw(LCD_COLOR_BLACK); } break; } switch(etat2) { case 0: //ecran bleu if((abs(accRoll)>SEUIL_1) || (abs(accPitch)>SEUIL_1)) { etat2 = 1; BSP_LCD_Clear(LCD_COLOR_YELLOW); touchFlag = true; lbSystem.Draw("legerement incline"); } break; case 1: //attention ecran jaune if((abs(accRoll)<SEUIL_1) && (abs(accPitch)<SEUIL_1)) { etat2 = 0; BSP_LCD_Clear(LCD_COLOR_GREEN); touchFlag = true; lbSystem.Draw("plat"); } if((abs(accRoll)>SEUIL_2) || (abs(accPitch)>SEUIL_2)) { etat2 = 2; BSP_LCD_Clear(LCD_COLOR_ORANGE); touchFlag = true; lbSystem.Draw("incline"); } break; case 2: //orange if((abs(accRoll)<SEUIL_2) && (abs(accPitch)<SEUIL_2)) { etat2 = 1; BSP_LCD_Clear(LCD_COLOR_YELLOW); touchFlag = true; lbSystem.Draw("legerement incline"); } if((abs(accRoll)>SEUIL_3) || (abs(accPitch)>SEUIL_3)) { etat2 = 3; BSP_LCD_Clear(LCD_COLOR_RED); touchFlag = true; lbSystem.Draw("tres incline"); } //danger ecran rouge + clignotement break; case 3: //rouge if((abs(accRoll)<SEUIL_3) && (abs(accPitch)<SEUIL_3)) { etat2 = 2; BSP_LCD_Clear(LCD_COLOR_ORANGE); touchFlag = true; lbSystem.Draw("incline"); } //danger ecran rouge + clignotement break; } //lcd_print(250, 10, "etat1 : %0.0f", (float) etat1); //lcd_print(250, 11, "etat2 : %0.0f", (float) etat2); //lcd_print(250, 12, "touchstate : %0.0f", (float) touch); //---------------------------- DEBUG ------------------------------// if(touch == 2 && oldTouch != touch) { if(touchFlag) { pc.printf("touchflag true / etat1 : %d\r\n", etat1); btnStart.Draw(); btnStop.Draw(); btnDebug_On.Draw(); btnDebug_Off.Draw(); btnReset.Draw(); if(etat1 == 0) { btnStart.Activate(); btnStop.Inactivate(); btnDebug_On.Inactivate(); btnDebug_Off.Inactivate(); } else if(etat1 == 1) { btnStart.Inactivate(); btnDebug_Off.Inactivate(); } else { btnStart.Inactivate(); btnDebug_On.Inactivate(); } } else { pc.printf("touchflag false / etat1 : %d\r\n", etat1); btnStart.Erase(); btnStop.Erase(); btnDebug_On.Erase(); btnDebug_Off.Erase(); } touchFlag ^= 1; } if(btnReset.Touched()) { NVIC_SystemReset(); } oldTouch = TS_State.touchDetected; lbTitre.Draw(); lbInfo.Draw(); wait_ms(REFRESH_TIME_MS); } } //------------------- FONCTIONS ------------------------------------// void setup() { pc.printf("\r\n------ LSM6DS3 Demo -----------\r\n"); pc.printf("LDSM6DS3 Init\r\n"); uint16_t status = sensor.begin(sensor.G_SCALE_245DPS, //plage de mesure minimale du gyroscope : jusqu'à +/- 245 deg/s (déjà assez rapide) sensor.A_SCALE_2G, //plage de mesure minimale de l'accéléromètre : +/- 2G (accélération relativement faible) sensor.G_ODR_1660, //fréquence d'échantillonage des valeurs du gyroscope max à 1,66 kEch/s (précision max) sensor.A_ODR_6660); //fréquence d'échantillonage des valeurs de l'accéléromètre à 6.66 kEch/s (précision max) pc.printf("LSM6DS3 WHO_AM_I's returned: 0x%X\r\n", status); //renvoie l'adresse i2c du capteur pc.printf("Should be 0x69\r\n"); pc.printf("LCD Init size(%d, %d)\r\n", BSP_LCD_GetXSize(), BSP_LCD_GetYSize()); } void readSensor() { sensor.readTemp(); //température en °C sensor.readAccel();//accélération en x, y, z en G(environ 9 m/s²) accelX = sensor.ax; accelY = sensor.ay; accelZ = sensor.az; sensor.readGyro();//vitesse de rotation en x, y, z en deg/s gyroX = sensor.gx; gyroY = sensor.gy; gyroZ = sensor.gz; } void printSensor() { pc.printf("accelX : %3.2f\n\r", accelX); pc.printf("accelY : %3.2f\n\r", accelY); pc.printf("accelZ : %3.2f\n\r", accelZ); pc.printf("accRoll : %3.2f\n\r", accRoll); pc.printf("accPitch : %3.2f\n\r", accPitch); } void drawImageLogo(int offsetX, int offsetY) { int x = 0; int y = 0; uint32_t* dataPtr = (uint32_t*)logo.data; while(y < logo.height) { while(x < logo.width) { BSP_LCD_DrawPixel(x + offsetX, y + offsetY, *dataPtr); dataPtr++; x++; } x = 0; y++; } }