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?
User | Revision | Line number | New 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 | } |