Inclinomètre a fusion de données entre gyromètre et accéléromètre
Dependencies: GYRO_DISCO_L476VG mbed BSP_DISCO_L476VG COMPASS_DISCO_L476VG
Diff: main.cpp
- Revision:
- 6:81d8b03a9673
- Parent:
- 5:f4a35a2a9085
- Child:
- 7:65ef53c3b7bc
diff -r f4a35a2a9085 -r 81d8b03a9673 main.cpp --- a/main.cpp Mon Jun 08 17:42:48 2020 +0000 +++ b/main.cpp Mon Jun 15 14:48:08 2020 +0000 @@ -1,55 +1,64 @@ #include "mbed.h" #include "GYRO_DISCO_L476VG.h" - +#include "COMPASS_DISCO_L476VG.h" +// +#define TE 0.01 GYRO_DISCO_L476VG gyro; +COMPASS_DISCO_L476VG compass; Serial pc(SERIAL_TX, SERIAL_RX,115200); Ticker ticker; DigitalOut led1(LED1); volatile bool flag=0; -float psig=0; -float gyro_zero(void) +double psig=0,psia; +double gyro_zero(void) { - const int NN=100000; + const int NN=10000; float GyroBuffer[3]; - float gy_off=0; - for(int i=0; i<NN; i++) - { + double gy_off=0; + for(int i=0; i<NN; i++) { gyro.GetXYZ(GyroBuffer); gy_off=gy_off+GyroBuffer[1]/NN; - } return(gy_off); } +double angle_zero(void) +{ + const int NN=1000; + int16_t AccBuffer[3]; + double PI=4*atan(1.0); + double ang_off=0; + for(int i=0; i<NN; i++) { + compass.AccGetXYZ(AccBuffer); + double ang=(180/PI)*atan2((double)AccBuffer[0],(double)AccBuffer[2]); + ang_off=ang_off+ang/NN; + } + return ang_off; +} void mesure(void) { flag=1; } - int main() { float GyroBuffer[3]; - - printf("Gyroscope started\n"); - ticker.attach(&mesure,0.01); + int16_t AccBuffer[3]; + printf("Super inclinometre\n\r"); + double PI=4*atan(1.0); + double gyr_off=gyro_zero(); + double ang_off=angle_zero(); + ticker.attach(&mesure,TE); unsigned char cpt=0; - float gyoff=gyro_zero(); + while(1) { if(flag) { - - // Read Gyroscope values + compass.AccGetXYZ(AccBuffer); + psia=((180.0/PI)*atan2((double)AccBuffer[0],(double)AccBuffer[2]))-ang_off; gyro.GetXYZ(GyroBuffer); - psig=psig+(GyroBuffer[1]-gyoff)*0.01/1000; - - // Display values - //printf("X = %8.1f \n", GyroBuffer[0]); - // printf("Y = %8.1f \n", GyroBuffer[1]); - //printf("Z = %8.1f \n", GyroBuffer[2]); - // printf("\033[3A"); // Moves cursor up x lines (x value is between [ and A) + psig=psig+(GyroBuffer[1]-gyr_off)*TE/1000; if(cpt==9) { cpt=0; led1 = !led1; - //pc.printf("$%f;\n",GyroBuffer[1]); - pc.printf("$%f;\n",psig); + pc.printf("$%f %f;\n",psia,psig); } cpt++; flag=0;