Revision 0:fb9761d3aafb, committed 2021-06-24
- Comitter:
- KheirddineZ
- Date:
- Thu Jun 24 13:53:28 2021 +0000
- Commit message:
- GPS
Changed in this revision
diff -r 000000000000 -r fb9761d3aafb GPS.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/GPS.cpp Thu Jun 24 13:53:28 2021 +0000
@@ -0,0 +1,527 @@
+#include "mbed.h"
+#include "DHT.h"
+#include "stdio.h"
+#include <cstdio>
+#include <cstring>
+#include <FileHandle.h>
+#include <string>
+#include "BufferedSerial.h"
+#include "LSM6DS33.h"
+
+#define N 100
+#define SAMPLE 1000
+#define GAIN 0.004375 //valeur datasheet LSM6DS33
+
+
+typedef struct GGA_DATA_T{
+ char UTC[11]; //UTC Time
+ char LAT[14]; //Latitude
+ char NS; //North or south indicator
+ char LONG[15]; //Longitude
+ char EW; //East or west indicator
+ char PFI; //Position fixed indicator
+ char NUMSAT[3]; //Number of satellites used
+ char HDOP[5]; //HDOP
+ char ALT[10]; //MSL Altitude
+ char AUNIT; //Units
+ char GSEP[5]; //Geoidal Separation
+ char GUNIT; //Units
+ char AODC[11]; //Age of Diff. Corr.
+ char CHECKSUM[3]; //Checksum
+} GGA_DATA;
+
+class donneGPS
+{
+ public:
+
+ GGA_DATA getGGA();
+
+ void formatGGA(char* data_array);
+
+ private:
+
+ void formatCOORDS(char* coords);
+
+
+
+ GGA_DATA GGAdata;
+
+};
+
+GGA_DATA donneGPS::getGGA()
+{
+ return GGAdata;
+}
+
+void donneGPS::formatGGA(char* data_array)
+{
+ enum cases {UTC, LAT, NS, LONG, EW, PFI, NUMSAT, HDOP, ALT, AUNIT, GSEP, GUNIT, AODC};
+ cases datamember= UTC;
+ char* start_ptr;
+ char* end_ptr = data_array+7;//Set start pointer after the message ID ("$GPGGA,")
+ bool flag=1;
+ char COORDbuf[14]={0};
+ char checksum[3]={0};
+
+ while (flag)
+ {
+ start_ptr = end_ptr;
+
+ while (*end_ptr!=',' && (*(end_ptr+1)!=10)&& *end_ptr!='*')end_ptr++;//Increment ptr until a comma is found
+
+ if (*end_ptr==10||*(end_ptr+1)==10||(*end_ptr=='*'&&*(end_ptr-1)==',')){//End reached
+ flag=0;
+ break;
+ }
+
+ switch(datamember){
+ case UTC:
+ memcpy(GGAdata.UTC, start_ptr, (end_ptr - start_ptr));
+ GGAdata.UTC[end_ptr - start_ptr] = '\0';//End null char
+ datamember = LAT;
+ break;
+ case LAT:
+ memcpy(GGAdata.LAT, start_ptr, (end_ptr - start_ptr));
+ GGAdata.LAT[end_ptr - start_ptr] = '\0';
+ datamember=NS;
+ break;
+ case NS:
+ if (*start_ptr!=','){
+ GGAdata.NS = *start_ptr;
+ strncat(GGAdata.LAT, start_ptr, 1);
+ }
+ datamember=LONG;
+ break;
+ case LONG:
+ memcpy(GGAdata.LONG, start_ptr, (end_ptr - start_ptr ));
+ GGAdata.LONG[end_ptr - start_ptr] = '\0';
+ datamember=EW;
+ break;
+ case EW:
+ if (*start_ptr!=','){
+ GGAdata.EW = *start_ptr;
+ strncat(GGAdata.LONG, start_ptr, 1);
+ }
+ datamember=PFI;
+ break;
+ case PFI:
+ if (*start_ptr!=',')GGAdata.PFI = *start_ptr;
+ datamember=NUMSAT;
+ break;
+ case NUMSAT:
+ memcpy(GGAdata.NUMSAT, start_ptr, (end_ptr - start_ptr));
+ GGAdata.NUMSAT[end_ptr - start_ptr] = '\0';
+ datamember=HDOP;
+ break;
+ case HDOP:
+ memcpy(GGAdata.HDOP, start_ptr, (end_ptr - start_ptr));
+ GGAdata.HDOP[end_ptr - start_ptr] = '\0';
+ datamember=ALT;
+ break;
+ case ALT:
+ memcpy(GGAdata.ALT, start_ptr, (end_ptr - start_ptr));
+ GGAdata.ALT[end_ptr - start_ptr] = '\0';
+ datamember=AUNIT;
+ break;
+ case AUNIT:
+ if (*start_ptr!=',')GGAdata.AUNIT= *start_ptr;
+ datamember=GSEP;
+ break;
+ case GSEP:
+ memcpy(GGAdata.GSEP, start_ptr, (end_ptr - start_ptr));
+ GGAdata.GSEP[end_ptr - start_ptr] = '\0';
+ datamember=GUNIT;
+ break;
+ case GUNIT:
+ if (*start_ptr!=',')GGAdata.GUNIT=*start_ptr;
+ datamember=AODC;
+ break;
+ case AODC:
+ memcpy(GGAdata.AODC, start_ptr, (end_ptr - start_ptr));
+ GGAdata.AODC[end_ptr - start_ptr] = '\0';
+ flag=0;
+ break;
+
+ }
+ end_ptr++;//Increment past the last comma
+ }
+ //Get checksum
+ while(*(end_ptr)!=10)end_ptr++;
+ checksum[0] = *(end_ptr - 3);
+ checksum[1] = *(end_ptr - 2);
+ checksum[2] = NULL;
+ memcpy(GGAdata.CHECKSUM, checksum, 3);
+ return;
+}
+
+void donneGPS::formatCOORDS(char* coords)
+{
+ char formatted[14]={0};
+ int i=0;
+ char* coordsstart= coords;
+
+ while (*(coords)){
+
+ formatted[i]=*coords;
+ formatted[++i]; coords++;
+ if (*(coords+2)=='.')
+ {
+ formatted[i]='d';//degrees symbol
+ i++;
+ }
+ else if (*coords=='.')
+ {
+ formatted[i] = 39;// ' symbol for minutes
+ i++;
+ coords++;
+ }
+ else if (*(coords-3)=='.')
+ {
+ formatted[i]='.';//Decimal for seconds
+ i++;
+ }
+ else if (*(coords-5)=='.')
+ {
+ formatted[i]='"';// " for seconds
+ i++;
+ formatted[i]=0;//Null char
+ }
+ }
+
+ strcpy(coordsstart, formatted);
+
+ //coords=formatted;
+ return;
+
+}
+void gyro(void);
+
+static UnbufferedSerial myBT(PA_9,PA_10,9600); //UART1 utilisé pour le module Bluetooth
+static BufferedSerial GPS(PA_2, PB_4,9600); //UART2 utilisé pour l'USB du µc
+
+DigitalOut Rouge(PA_12);
+DigitalOut Verte(PB_0);
+
+AnalogIn lum (A2);//pin capteur de luminosité
+AnalogIn son (A0); // pin capteur de son
+
+AnalogIn pin_SS(A1);
+DHT sensor(A1,SEN11301P); // pin capteur température et humidité
+
+LSM6DS33 acc (PB_7, PB_6); // pin Gyroscope
+
+//Variable gyroscope
+ double rate_gyr_x = 0, rate_gyr_y = 0, rate_gyr_z = 0;
+ double gyroXangle=0, gyroYangle=0, gyroZangle=0;
+ double offsetx=0, offsety=0, offsetz=0;
+ double gxx, gyy, gzz;
+ double time_exec = 0;
+
+ //accelerometre
+ double axx, ayy, azz;
+ double vx, vy, V;
+
+Timer timer;
+
+char messageID [20],
+ UTCtime [20],
+ latitude [20],
+ N_S [2],
+ longitude [20],
+ E_W [2],
+ pos_fix_ind [2],
+ sat [3],
+ HDOP [10],
+ altitude [10],
+ units1 [2],
+ geoidal_sep [5],
+ units [2],
+ AgeofDiff[45],
+ checksum [5];
+
+float valeur,v_luminance;
+
+char buffer[256];
+char envoi[256];
+
+char buff[100]={0};
+char text[] = "Angle X : ";
+
+char buff1[100]={0};
+char text1[] = "Angle Y : ";
+
+char buff2[100]={0};
+char text2[] = "Angle Z : ";
+
+char buff3[100]={0};
+char text3[] = "V : ";
+
+int flag_gyro = 0;
+
+int main()
+{
+ donneGPS myGPS;
+
+ //Variable capteurs analogiques
+ double V_lum, V_son, V_temp, V_hum;
+ double tab_son[N];
+ int erreur;
+
+
+
+ acc.begin();
+
+ //calcule de l'offset moyen des 3 axes du gyroscope
+ for (int i=0; i<SAMPLE; i++){
+ acc.readGyro();
+ offsetx += acc.gx;
+ offsety += acc.gy;
+ offsetz += acc.gz;
+ }
+ offsetx /= SAMPLE;
+ offsety /= SAMPLE;
+ offsetz /= SAMPLE;
+ //printf("calib :\tx:%f y:%f z:%f\n\n\r", offsetx, offsety, offsetz);
+
+
+ GPS.set_baud(9600);
+ //myBT.set_baud(9000);
+
+ GPS.set_blocking(false);
+ myBT.set_blocking(false);
+
+ //Verte = 1;
+ //Rouge = 1;
+ int i = 0;
+ int num = 0;
+
+ while(1)
+ {
+
+ //Récupération des données du capteur de luminosité
+ V_lum = lum.read()*1000;
+
+ //Récupération du capteur de son + calcule du max
+ for (int i=0;i<N;i++){
+ tab_son[i]= son.read();
+ }
+
+ V_son = tab_son[0];
+ for (int j=1;j<N;j++)
+ {
+ if(tab_son[j]>=V_son) V_son = tab_son[j];
+ }
+
+ //calcule du capteur de température + humidité
+ erreur = sensor.readData();
+ if (erreur == 0) {
+ V_temp = sensor.ReadTemperature(CELCIUS);
+ V_hum = sensor.ReadHumidity();
+ }
+
+
+
+ printf("Ecriture GPS : %d\r\n", GPS.readable());
+
+ if(GPS.readable())
+ {
+
+ for(i=0; i<256;i++)
+ {
+ buffer[i] = {0};
+ }
+ //GPS.sync();
+
+ int nb_byte = GPS.read((void*)&buffer[0], 1);
+ //int nb_byte = GPS.read(&buffer[0], 0);
+ printf("Buffer[0] : %s \r\n", (char*)&buffer[0]);
+ printf("Nombre de byte %d\r\n", nb_byte);
+ //printf("Buffer 0 : %s", &buffer[0]);
+ char dollar[] = "$";
+ do{
+ GPS.read((void*)&buffer[0], 1);
+ }
+ while(strcmp(buffer, dollar)!= 0);
+
+ for(i=1; i<100 ; i++)
+ {
+ while(!GPS.readable());
+ GPS.read((void*)&buffer[i], 1);
+ printf("Buffer[%d] = %s\r\n",i,(char*)&buffer[i]);
+ wait_us(1000);
+
+ if((strcmp(&buffer[i], "\r") == 0 || strcmp(&buffer[i], "$") == 0) && strlen(buffer) > 60) //buffer[i] == 10 || buffer[i] == 36)
+ {
+ wait_us(500000);
+ strcpy(envoi, buffer);
+
+ if(((envoi[3]) == 'G') && ((envoi[4]) == 'G') && ((envoi[5]) == 'A') && (strchr(envoi, '*') != 0) )
+ {
+ myGPS.formatGGA(envoi);
+ printf("Valeur de la longitude : %s\r\n", myGPS.getGGA().LONG);
+ printf("Valeur de l'altitude : %s\r\n", myGPS.getGGA().ALT);
+ printf("Valeur de Latitude : %s\r\n",myGPS.getGGA().LAT);
+ }
+
+ printf("Buffer : %s\r\n", buffer);
+ for(i=0; i<256;i++)
+ {
+ buffer[i] = {0};
+ }
+ break;
+ }
+ }
+ }
+
+ printf("Ecriture bluetooth : %d\r\n", myBT.writable());
+
+ printf("Contenu du buffer d'envoi : %s\r\n", (void*)&envoi);
+
+ if(myBT.writable())
+ {
+
+ char ind_long[] = "longitude :";
+ strcat(ind_long,myGPS.getGGA().LONG);
+ myBT.write((void*)ind_long,strlen(ind_long));
+
+ wait_us(100000);
+
+ char ind_lat[] = "latitude :";
+ strcat(ind_lat,myGPS.getGGA().LAT);
+ myBT.write((void*)ind_lat,strlen(ind_lat));
+
+ wait_us(100000);
+
+ char ind_alt[] = "altitude :";
+ strcat(ind_alt,myGPS.getGGA().ALT);
+ myBT.write((void*)ind_alt,strlen(ind_alt));
+
+ wait_us(100000);
+
+ char L_value[100] = {0};
+ char L[] = "L :";
+ sprintf(L_value, "%f", V_lum);
+ strcat(L,L_value);
+ printf("%s\r\n",L);
+ myBT.write((void*)L,strlen(L));
+
+ wait_us(100000);
+
+ char S_value[100] = {0};
+ char S[] = "S :";
+ sprintf(S_value, "%f", V_son);
+ strcat(S,S_value);
+ printf("%s\r\n",S);
+ myBT.write((void*)S,strlen(S));
+
+ wait_us(100000);
+
+ char T_value[100] = {0};
+ char T[] = "T :";
+ sprintf(T_value, "%f", V_temp);
+ strcat(T,T_value);
+ printf("%s\r\n",T);
+ myBT.write((void*)T,strlen(T));
+
+ wait_us(100000);
+
+ char H_value[100] = {0};
+ char H[] = "H :";
+ sprintf(H_value, "%f", V_hum);
+ strcat(H,H_value);
+ printf("%s\r\n",H);
+ myBT.write((void*)H,strlen(H));
+
+
+ /* gyro();
+
+ wait_us(100000);
+ sprintf(buff,"%f",gyroXangle);
+ strcat(text,buff);
+ myBT.write((void*)text,strlen(text));
+
+ wait_us(100000);
+
+ sprintf(buff1,"%f",gyroYangle);
+ strcat(text1,buff1);
+ myBT.write((void*)text1,strlen(text1));
+
+ wait_us(100000);
+
+ sprintf(buff2,"%f",gyroZangle);
+ strcat(text2,buff2);
+ myBT.write((void*)text2,strlen(text2));
+
+ wait_us(100000);
+
+ sprintf(buff3,"%f",V);
+ strcat(text3,buff3);
+ myBT.write((void*)text3,strlen(text3));*/
+
+ }
+
+ printf("Je sors de la boucle d'écriture\r\n");
+ }
+
+}
+
+void gyro(void)
+{
+ printf("Je suis dans la boucle gyro\r\n");
+ timer.start();
+
+ //récupération des valeurs du gyro - offset moyen
+ acc.readGyro();
+ gxx = acc.gx - offsetx;
+ gyy = acc.gy - offsety;
+ gzz = acc.gz - offsetz;
+
+ time_exec = timer.elapsed_time().count();
+
+ //calcule de l'angle de l'axe X
+ if(gxx >= 1 | gxx <= -1){
+
+ //convertion des valeurs brute en angle
+ rate_gyr_x = gxx * GAIN;
+ gyroXangle+=rate_gyr_x*(time_exec/1000);
+
+ //conditionnement de l'angle entre 0° et 360°
+ if(gyroXangle > 360) gyroXangle = 0;
+ else if (gyroXangle < 0) gyroXangle = 360;
+ }
+
+ //calcule de l'angle de l'axe Y
+ if(gyy >= 1 | gyy <= -1){
+ rate_gyr_y = gyy * GAIN;
+ gyroYangle+=rate_gyr_y*(time_exec/1000);
+ if(gyroYangle > 360) gyroYangle = 0;
+ else if (gyroYangle < 0) gyroYangle = 360;
+ }
+
+ //calcule de l'angle de l'axe Z
+ if(gzz >= 1 | gzz <= -1){
+ rate_gyr_z = gzz * GAIN;
+ gyroZangle += rate_gyr_z*(time_exec/1000);
+ if(gyroZangle > 360) gyroZangle = 0;
+ else if (gyroZangle < 0) gyroZangle = 360;
+ }
+
+ timer.reset();
+
+ //calcule de l'accéleration
+ acc.readAccel();
+ axx = acc.ax;
+ ayy = acc.ay;
+ azz = acc.az;
+ vx = vx + axx*9.8*1/104;
+ vy = vy + ayy*9.8*1/104;
+ V = sqrt(vx*vx+vy*vy);
+
+ printf("X: %f\r\n",gyroXangle);
+ printf("Y: %f\r\n", gyroYangle);
+ printf("Z: %f\r\n", gyroZangle);
+ printf("V: %f\r\n", V);
+
+
+
+}
diff -r 000000000000 -r fb9761d3aafb GPS.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/GPS.h Thu Jun 24 13:53:28 2021 +0000
@@ -0,0 +1,34 @@
+#include "mbed.h"
+#include "DHT.h"
+#include "stdio.h"
+#include <cstdio>
+#include <cstring>
+#include <FileHandle.h>
+#include <string>
+#include "BufferedSerial.h"
+#include "LSM6DS33.h"
+
+
+
+class donneGPS
+{
+
+ public:
+
+ void formatGGA();
+
+ private :
+
+ void formatCOORDS();
+
+
+ void donneGPS::formatGGA();
+
+
+ void donneGPS::formatCOORDS();
+
+
+ void gyro();
+
+
+ }
\ No newline at end of file
diff -r 000000000000 -r fb9761d3aafb mbed.bld
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld Thu Jun 24 13:53:28 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file