main.cpp

00001 /**
00002  ******************************************************************************
00003  * @file    main.cpp
00004  * @author  CLab
00005  * @version V1.0.0
00006  * @date    2-December-2016
00007  * @brief   Simple Example application for using the X_NUCLEO_IKS01A1
00008  *          MEMS Inertial & Environmental Sensor Nucleo expansion board.
00009  ******************************************************************************
00010  * @attention
00011  *
00012  * <h2><center>&copy; COPYRIGHT(c) 2016 STMicroelectronics</center></h2>
00013  *
00014  * Redistribution and use in source and binary forms, with or without modification,
00015  * are permitted provided that the following conditions are met:
00016  *   1. Redistributions of source code must retain the above copyright notice,
00017  *      this list of conditions and the following disclaimer.
00018  *   2. Redistributions in binary form must reproduce the above copyright notice,
00019  *      this list of conditions and the following disclaimer in the documentation
00020  *      and/or other materials provided with the distribution.
00021  *   3. Neither the name of STMicroelectronics nor the names of its contributors
00022  *      may be used to endorse or promote products derived from this software
00023  *      without specific prior written permission.
00024  *
00025  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00026  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00027  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00028  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
00029  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00030  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00031  *  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
00033  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00034  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  ******************************************************************************
00037 */
00038 
00039 /* Includes */
00040 #include "mbed.h"
00041 #include "XNucleoIKS01A2.h"
00042 /* Include arm_math.h mathematic functions */
00043 #include "arm_math.h"
00044 /* Include mbed-dsp libraries */
00045 #include "arm_common_tables.h"
00046 #include "arm_const_structs.h"
00047 #include "math_helper.h"
00048 
00049 
00050 /* FFT settings */
00051 #define SAMPLES                 4096           /* 2048 real party and 2048 imaginary parts */
00052 #define FFT_SIZE                SAMPLES / 2     /* FFT size is always the same size as we have samples, so 256 in our case */
00053 
00054 /* Variables FFT*/
00055 float InputX[SAMPLES],InputY[SAMPLES],InputZ[SAMPLES];
00056 //float Input_X[SAMPLES-2],Input_Y[SAMPLES-2],Input_Z[SAMPLES-2];
00057 float OutputX[FFT_SIZE],OutputY[FFT_SIZE],OutputZ[FFT_SIZE];
00058 float Output_X[FFT_SIZE-1],Output_Y[FFT_SIZE-1],Output_Z[FFT_SIZE-1];
00059 uint8_t cx_f, cy_f, cz_f;
00060 uint8_t M_cx_f,M_cy_f,M_cz_f;
00061 //int M_cx_f,M_cy_f,M_cz_f;
00062 bool trig=0;
00063 
00064 /*Variabili accellerometro*/
00065 //int32_t axes[3];
00066 int16_t int_axes[3];
00067 
00068 /* Instantiate the expansion board */
00069 static XNucleoIKS01A2 *mems_expansion_board = XNucleoIKS01A2::instance(D14, D15, D4, D5);
00070 
00071 static LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro;
00072 
00073 //Seriali
00074 Serial     pc(USBTX, USBRX,115200);
00075 I2C i2c(D14, D15);
00076 
00077 /* LoRaWAN */
00078 Serial lora(D8,D2,115200);
00079 void setup_lora(void);
00080 void modem_at_cmd(char*,int);
00081 void wait4join(void);
00082 void inviaMessaggio(float,uint8_t, float,uint8_t, float,uint8_t);
00083 char c;
00084 uint16_t i_M=351;// indice per invio messaggio
00085 
00086 char* msg1 = {"AT"};
00087 char* msg2 = {"AT+APPEUI=1111111111111111"};
00088 char* msg3 = {"AT+AK=11111111111111111111111111111111"};
00089 char* msg4 = {"AT+JOIN=1"};
00090 //char* msg5 = {"AT+SEND=15,3031323334,0"};
00091 char msg5[64]; // = {"AT+SEND=15,3031323334,0"};
00092 char msg6[80];
00093 
00094 /* Timer */
00095 //Timer   tempo, temp;
00096 //Ticker tic;
00097 
00098 
00099 void sample()
00100 {
00101     trig=1;
00102 
00103 }
00104 int calcolare_max(float* Vettore)
00105 {
00106 
00107     uint16_t I=0;
00108     float max=Vettore[0];
00109     for(int w=0; w<=sizeof(Vettore); w++)
00110         if(max<Vettore[w]) {
00111             max=Vettore[w];
00112             I=w;
00113         }
00114     return I;
00115 }
00116 
00117 
00118 int main()
00119 {   
00120     i2c.frequency(1000000);
00121     float dato_final[3];
00122     float sens; 
00123     //sens =0.061; //FS= 2g
00124     uint16_t imX, imY, imZ;  
00125     
00126     float MaxValueX,MaxValueY,MaxValueZ; //Massimo valore dei vettoridei massimi di FFT in 1 ora
00127     //arm_cfft_instance_f32 S;   // ARM CFFT module
00128     float maxValueX,maxValueY,maxValueZ;            // Max FFT value is stored here
00129     uint32_t maxIndexX,maxIndexY,maxIndexZ;         // Index in Output array where max value is
00130     float V_maxX[i_M],V_maxY[i_M],V_maxZ[i_M]; // Vettori contenente i max del FFT in un ora
00131     float V_cx_f[i_M],V_cy_f[i_M], V_cz_f[i_M];
00132     i_M=0;
00133     /* Enable accellerometro */
00134     acc_gyro->enable_x();
00135     acc_gyro->set_x_odr(1666.0);
00136     acc_gyro->get_x_sensitivity(&sens);
00137     //Inizializzazione comunicazione con modem LoRaWAN
00138     setup_lora();
00139 
00140     pc.printf("\r\n--- Starting new run ---\r\n");
00141 
00142     while(1) {
00143         pc.printf("Nuovo ciclo while \r\n");
00144       /*  memset(V_maxX,0,360);
00145         memset(V_maxY,0,360);
00146         memset(V_maxZ,0,360);
00147         
00148         pc.printf("Vettore dei max di x: ");
00149                 for(int j=0; j<360; j++) {
00150                     pc.printf(" %f",V_maxX[j]);
00151                 }
00152                 pc.printf("\r\n");
00153                 */
00154         /*
00155             // azzera vettori
00156             memset(InputX,0,SAMPLES*sizeof(float));
00157             //for(int j=0;j<SAMPLES;j++)
00158             //printf(" %f%",InputX[j]);
00159             memset(InputY,0,SAMPLES*sizeof(float));
00160             memset(InputZ,0,SAMPLES*sizeof(float));
00161             memset(OutputX,0,FFT_SIZE*sizeof(float));
00162             memset(OutputY,0,FFT_SIZE*sizeof(float));
00163             memset(OutputZ,0,FFT_SIZE*sizeof(float));
00164          */
00165         /*
00166          float t=0;
00167          tempo.start();
00168          tempo.reset();
00169          acc_gyro->get_x_axes_raw(int_axes);
00170          t=tempo.read();
00171          printf("Tempo1: %f\r\n", t);
00172          */
00173         //pc.printf("Fa partire tic\r\n");
00174         /*
00175         float t0=0;
00176         temp.start();
00177         temp.reset();
00178         */
00179         //printf("Acquisisco\r\n");
00180         Ticker tic;
00181         tic.attach_us(&sample,5000); //5 ms 200Hz sampling rate
00182         //pc.printf("Trig prima del for di acquisizione = %d\r\n",trig);
00183         for (int i = 0; i < SAMPLES; i += 2) {
00184             //printf("Dentro il for \r\n");
00185             while (trig==0);
00186             //pc.printf("Trig = %d e i= %d\r\n",trig,i);
00187             trig=0;
00188             //printf("trig= %d\r\n",trig);
00189             acc_gyro->get_x_axes_raw(int_axes);
00190             //printf("Preso dato\r\n");
00191             dato_final[0]=float(int_axes[0]*sens);//mg //solo x
00192             dato_final[1]=float(int_axes[1]*sens);//mg //solo y
00193             dato_final[2]=float(int_axes[2]*sens);//mg //solo z
00194             InputX[i] = dato_final[0]; //Real part NB removing DC offset
00195             InputX[i + 1] = 0;               //Imaginary Part set to zero
00196             InputY[i] = dato_final[1];
00197             InputY[i + 1] = 0;
00198             InputZ[i] = dato_final[2];
00199             InputZ[i + 1] = 0;
00200         }
00201         tic.detach();
00202 
00203         /*
00204         t0=temp.read();
00205         float t=0;
00206         tempo.start();
00207         tempo.reset();
00208         */
00209         // azzera vettori
00210       /*  memset(Input_X,0,SAMPLES*sizeof(float));
00211         memset(Input_Y,0,SAMPLES*sizeof(float));
00212         memset(Input_Z,0,SAMPLES*sizeof(float));
00213         */
00214         //printf("Tolgo l'indice zero\r\n");
00215         //Tolgo l'indice zero che è quell della componete continua
00216         /*for (int h=0; h<SAMPLES-2; h++) {
00217             Input_X[h]=InputX[h+2];
00218             Input_Y[h]=InputY[h+2];
00219             Input_Z[h]=InputZ[h+2];
00220         }*/
00221 
00222         // Init the Complex FFT module, intFlag = 0, doBitReverse = 1
00223         //NB using predefined arm_cfft_sR_f32_lenXXX, in this case XXX is 4096
00224         arm_cfft_f32(&arm_cfft_sR_f32_len2048, InputX, 0, 1);
00225         arm_cfft_f32(&arm_cfft_sR_f32_len2048, InputY, 0, 1);
00226         arm_cfft_f32(&arm_cfft_sR_f32_len2048, InputZ, 0, 1);
00227         // Complex Magniture Module put results into Output(Half size of the Input)
00228         arm_cmplx_mag_f32(InputX, OutputX, FFT_SIZE);
00229         arm_cmplx_mag_f32(InputY, OutputY, FFT_SIZE);
00230         arm_cmplx_mag_f32(InputZ, OutputZ, FFT_SIZE);
00231 
00232         for (int h=0; h<FFT_SIZE-1; h++) {
00233             Output_X[h]=OutputX[h+1];
00234             Output_Y[h]=OutputY[h+1];
00235             Output_Z[h]=OutputZ[h+1];
00236         }
00237         //Calculates maxValue and returns corresponding value
00238         arm_max_f32(Output_X, FFT_SIZE-1, &maxValueX, &maxIndexX);
00239         arm_max_f32(Output_Y, FFT_SIZE-1, &maxValueY, &maxIndexY);
00240         arm_max_f32(Output_Z, FFT_SIZE-1, &maxValueZ, &maxIndexZ);
00241          printf("Massimo valore di X e' : %f\r\n",maxValueX/2048*2/1000);
00242          printf("Massimo valore di Y e' : %f\r\n",maxValueY/2048*2/1000);
00243          printf("Massimo valore di Z e' : %f\r\n",maxValueZ/2048*2/1000);
00244          
00245       
00246 
00247         cx_f= maxIndexX*200/2048;
00248         cy_f= maxIndexY*200/2048;
00249         cz_f= maxIndexZ*200/2048;
00250        /* if (maxValueX>142000 || maxValueY>213000 || maxValueZ>211000){
00251            printf("ALLARME, troppe vibrazioni!!!\r\n");
00252            inviaMessaggio(maxValueX,cx_f,maxValueY,cy_f,maxValueZ,cz_f);
00253             i_M=0;
00254            }
00255            */
00256         //pc.printf("Valore del Max di x: %f e valore della sua componete frequenziale: %d\r\n",maxValueX,cx_f);
00257         //printf("Valore del Max di y: %f e valore della sua componete frequenziale: %d\r\n",maxValueY,cy_f);
00258              /* printf("Indice del max di x %d\r\n",maxIndexX);
00259               printf("Valore del Max di x: %f e valore della sua componete frequenziale: %d\r\n",maxValueX,cx_f);
00260               printf("Indice del max di y: %d\r\n",maxIndexY);
00261               printf("Valore del Max di y: %f e valore della sua componete frequenziale: %d\r\n",maxValueY,cy_f);
00262               printf("Indice del max di z: %d\r\n",maxIndexZ);
00263               printf("Valore del Max di z: %f e valore della sua componete frequenziale: %d\r\n",maxValueZ,cz_f);
00264               printf("\r\n");
00265           */
00266         /* t=tempo.read();
00267          printf("Tempo acquisizione: %f\r\n", t0);
00268          printf("Tempo per FFT e max: %f\r\n", t);
00269          */
00270         
00271         V_maxX[i_M]=maxValueX/2048*2/1000; //Ai=bin/N*2 diviso 1000 per passare da mg a g
00272         V_cx_f[i_M]=cx_f;
00273         V_maxY[i_M]=maxValueY/2048*2/1000;
00274         V_cy_f[i_M]=cy_f;
00275         V_maxZ[i_M]=maxValueZ/2048*2/1000;
00276         V_cz_f[i_M]=cz_f;
00277 
00278 
00279         if(i_M==60 || V_maxX[i_M]>0.139 || V_maxY[i_M]>0.208 || V_maxZ[i_M]>0.206) { //360 circa un ora con precisione 351
00280             //X
00281             /*
00282                 pc.printf("Vettore dei max di x: ");
00283                 for(int j=0; j<=i_M; j++) {
00284                     pc.printf(" %f",V_maxX[j]);
00285                 }
00286                 pc.printf("\r\n");*/
00287             imX=calcolare_max(V_maxX);
00288             MaxValueX=V_maxX[imX];
00289             M_cx_f=V_cx_f[imX];
00290             pc.printf("Massimo di x rilevato in questo lasso temporale e' di : %f, componente frequenziale %d\r\n",MaxValueX,M_cx_f);
00291             //Y
00292                /*      printf("Vettore dei max di y: ");
00293                      for(int j=0; j<=i_M; j++) {
00294                          printf(" %f",V_maxY[j]);
00295                      }
00296                      printf("\r\n"); */
00297             imY=calcolare_max(V_maxY);
00298             MaxValueY=V_maxY[imY];
00299             M_cy_f=V_cy_f[calcolare_max(V_maxY)];
00300             printf("Massimo di y rilevato in questo lasso temporale e' di : %f, componente frequenziale %d\r\n",MaxValueY,M_cy_f);
00301 
00302             //Z
00303             
00304              /*    printf("Vettore dei max di z: ");
00305                  for(int j=0; j<=i_M; j++) {
00306                      printf(" %f",V_maxZ[j]);
00307                  }
00308                  printf("\r\n"); */
00309             imZ=calcolare_max(V_maxZ);
00310             MaxValueZ=V_maxZ[imZ];
00311             M_cz_f=V_cz_f[calcolare_max(V_maxZ)];
00312             printf("Massimo di z rilevato in questo lasso temporale e' di : %f, componente frequenziale %d\r\n",MaxValueZ,M_cz_f);
00313             inviaMessaggio(MaxValueX,M_cx_f,MaxValueY,M_cy_f,MaxValueZ,M_cz_f);
00314 
00315             i_M=0;
00316             printf("\r\n");
00317         } else{
00318             
00319             i_M++;
00320             }
00321            pc.printf("Valore di i_M : %d\r\n",i_M); 
00322     }
00323 
00324 }//Fine while(1)
00325 
00326 
00327 
00328 void setup_lora()
00329 {
00330 
00331     pc.printf("Setup comunicazione LoRa\r\n");
00332     modem_at_cmd(msg1,(int)strlen(msg1));
00333     pc.printf("Inviato AT\r\n");
00334     wait(1);
00335     modem_at_cmd(msg2,(int)strlen(msg2));
00336     pc.printf("Inviato EUI\r\n");
00337     wait(1);
00338     modem_at_cmd(msg3,(int)strlen(msg3));
00339     pc.printf("Inviato AK\r\n");
00340     wait(1);
00341     modem_at_cmd(msg4,(int)strlen(msg4));
00342     pc.printf("Inviato JOIN\r\n");
00343     wait4join();
00344     //pc.printf("+JoinAccepted\r\n");
00345     pc.printf("\r\n");
00346     //modem_at_cmd(msg5,(int)strlen(msg5));
00347     //pc.printf("Inviato send\r\n");
00348 }
00349 
00350 void modem_at_cmd(char* buffer, int n)
00351 {
00352     for(uint8_t i=0; i<n; i++) {
00353         lora.putc(buffer[i]);
00354         pc.putc(buffer[i]);
00355     }
00356     lora.putc(13);//CR
00357     pc.putc(13);
00358     pc.printf("\n");
00359     c=0;
00360     do {
00361         if (lora.readable()) {
00362             c = lora.getc();
00363             pc.putc(c);
00364         }
00365     } while(c!=' ');
00366 }
00367 
00368 void wait4join()
00369 {
00370     c=0;
00371     do {
00372         if (lora.readable()) {
00373             c = lora.getc();
00374             pc.putc(c);
00375         }
00376     } while(c!='d');// Perchè il messaggio di successo è +JoinAccepted
00377 }
00378 
00379 // invia messaggio
00380 void inviaMessaggio(float maxValueX,uint8_t M_cx_f,float maxValueY,uint8_t M_cy_f, float maxValueZ,uint8_t M_cz_f)
00381 {
00382     int i;
00383 
00384 
00385     // azzera vettori
00386     memset(msg5,0,64*sizeof(char));
00387     memset(msg6,0,80*sizeof(char));
00388 
00389     // comporre il JSON
00390 
00391     sprintf(msg5,"%f, %d; %f, %d; %f, %d;", maxValueX, M_cx_f, maxValueY, M_cy_f, maxValueZ, M_cz_f );
00392     //printf("Compongo msg5 \r\n");
00393     pc.printf(" Il messaggio da inviare e': %s\r\n",msg5);
00394 
00395     //pc.printf(msg5);
00396     //printf("\r\n");
00397     //printf("Lunghezza messaggio %d",(int)strlen(msg5));
00398     //printf("\r\n");
00399 
00400     int len = sprintf(msg6,"AT+SEND=");
00401 
00402     len += sprintf(msg6+len,"15");
00403     len += sprintf(msg6+len,",");
00404 
00405     for(i=0; i<strlen(msg5); i++) {
00406         sprintf(msg6+len+2*i,"%X",*(msg5+i));
00407     }
00408     sprintf(msg6+len+2*i,",0");
00409     modem_at_cmd(msg6,(int)strlen(msg6));
00410     pc.printf("Inviato send\r\n");
00411 }