E&R S3 prime / Mbed 2 deprecated DISCO_AcGy1_OK

Version pour Q15

Dependencies:   GYRO_DISCO_L476VG mbed BSP_DISCO_L476VG COMPASS_DISCO_L476VG

main.cpp@6:81d8b03a9673, 2020-06-15 (annotated)

Committer:
gr91
Date:
Mon Jun 15 14:48:08 2020 +0000
Revision:
6:81d8b03a9673
Parent:
5:f4a35a2a9085 
Version OK

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bcostm 0:5432bdf904f9 1 #include "mbed.h"
bcostm 0:5432bdf904f9 2 #include "GYRO_DISCO_L476VG.h"
gr91 6:81d8b03a9673 3 #include "COMPASS_DISCO_L476VG.h"
gr91 6:81d8b03a9673 4 //
gr91 6:81d8b03a9673 5 #define TE 0.01
bcostm 0:5432bdf904f9 6 GYRO_DISCO_L476VG gyro;
gr91 6:81d8b03a9673 7 COMPASS_DISCO_L476VG compass;
gr91 5:f4a35a2a9085 8 Serial pc(SERIAL_TX, SERIAL_RX,115200);
gr91 5:f4a35a2a9085 9 Ticker ticker;
bcostm 0:5432bdf904f9 10 DigitalOut led1(LED1);
gr91 5:f4a35a2a9085 11 volatile bool flag=0;
gr91 6:81d8b03a9673 12 double psig=0,psia;
gr91 6:81d8b03a9673 13 double gyro_zero(void)
gr91 5:f4a35a2a9085 14 {
gr91 6:81d8b03a9673 15 const int NN=10000;
gr91 5:f4a35a2a9085 16 float GyroBuffer[3];
gr91 6:81d8b03a9673 17 double gy_off=0;
gr91 6:81d8b03a9673 18 for(int i=0; i<NN; i++) {
gr91 5:f4a35a2a9085 19 gyro.GetXYZ(GyroBuffer);
gr91 5:f4a35a2a9085 20 gy_off=gy_off+GyroBuffer[1]/NN;
gr91 5:f4a35a2a9085 21 }
gr91 5:f4a35a2a9085 22 return(gy_off);
gr91 5:f4a35a2a9085 23 }
gr91 6:81d8b03a9673 24 double angle_zero(void)
gr91 6:81d8b03a9673 25 {
gr91 6:81d8b03a9673 26 const int NN=1000;
gr91 6:81d8b03a9673 27 int16_t AccBuffer[3];
gr91 6:81d8b03a9673 28 double PI=4*atan(1.0);
gr91 6:81d8b03a9673 29 double ang_off=0;
gr91 6:81d8b03a9673 30 for(int i=0; i<NN; i++) {
gr91 6:81d8b03a9673 31 compass.AccGetXYZ(AccBuffer);
gr91 6:81d8b03a9673 32 double ang=(180/PI)*atan2((double)AccBuffer[0],(double)AccBuffer[2]);
gr91 6:81d8b03a9673 33 ang_off=ang_off+ang/NN;
gr91 6:81d8b03a9673 34 }
gr91 6:81d8b03a9673 35 return ang_off;
gr91 6:81d8b03a9673 36 }
gr91 5:f4a35a2a9085 37 void mesure(void)
gr91 5:f4a35a2a9085 38 {
gr91 5:f4a35a2a9085 39 flag=1;
gr91 5:f4a35a2a9085 40 }
bcostm 0:5432bdf904f9 41 int main()
bcostm 0:5432bdf904f9 42 {
bcostm 0:5432bdf904f9 43 float GyroBuffer[3];
gr91 6:81d8b03a9673 44 int16_t AccBuffer[3];
gr91 6:81d8b03a9673 45 printf("Super inclinometre\n\r");
gr91 6:81d8b03a9673 46 double PI=4*atan(1.0);
gr91 6:81d8b03a9673 47 double gyr_off=gyro_zero();
gr91 6:81d8b03a9673 48 double ang_off=angle_zero();
gr91 6:81d8b03a9673 49 ticker.attach(&mesure,TE);
gr91 5:f4a35a2a9085 50 unsigned char cpt=0;
gr91 6:81d8b03a9673 51
bcostm 0:5432bdf904f9 52 while(1) {
gr91 5:f4a35a2a9085 53 if(flag) {
gr91 6:81d8b03a9673 54 compass.AccGetXYZ(AccBuffer);
gr91 6:81d8b03a9673 55 psia=((180.0/PI)*atan2((double)AccBuffer[0],(double)AccBuffer[2]))-ang_off;
gr91 5:f4a35a2a9085 56 gyro.GetXYZ(GyroBuffer);
gr91 6:81d8b03a9673 57 psig=psig+(GyroBuffer[1]-gyr_off)*TE/1000;
gr91 5:f4a35a2a9085 58 if(cpt==9) {
gr91 5:f4a35a2a9085 59 cpt=0;
gr91 5:f4a35a2a9085 60 led1 = !led1;
gr91 6:81d8b03a9673 61 pc.printf("$%f %f;\n",psia,psig);
gr91 5:f4a35a2a9085 62 }
gr91 5:f4a35a2a9085 63 cpt++;
gr91 5:f4a35a2a9085 64 flag=0;
gr91 5:f4a35a2a9085 65 }
bcostm 0:5432bdf904f9 66 }
bcostm 0:5432bdf904f9 67 }