Yaroslav Krainyk / Mbed 2 deprecated DISCO-F429ZI_Gyr

gyro

Dependencies:   BSP_DISCO_F429ZI GYRO_DISCO_F429ZI LCD_DISCO_F429ZI mbed

Fork of DISCO-F429ZI_Gyro_demo by ST

main.cpp@3:68f55e42c85f, 2017-11-15 (annotated)

Committer:
codebreaker7
Date:
Wed Nov 15 10:51:01 2017 +0000
Revision:
3:68f55e42c85f
Parent:
2:b878d61bf57b 
Cleaned up some code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bcostm 0:44f624c5501e 1 #include "mbed.h"
bcostm 0:44f624c5501e 2 #include "GYRO_DISCO_F429ZI.h"
codebreaker7 2:b878d61bf57b 3 #include "LCD_DISCO_F429ZI.h"
codebreaker7 2:b878d61bf57b 4 #include "stm32f429i_discovery_lcd.h"
bcostm 0:44f624c5501e 5
bcostm 0:44f624c5501e 6 GYRO_DISCO_F429ZI gyro;
codebreaker7 2:b878d61bf57b 7 LCD_DISCO_F429ZI lcd;
bcostm 0:44f624c5501e 8
bcostm 0:44f624c5501e 9 DigitalOut led1(LED1);
bcostm 0:44f624c5501e 10
bcostm 0:44f624c5501e 11 int main()
bcostm 0:44f624c5501e 12 {
bcostm 0:44f624c5501e 13 float GyroBuffer[3];
codebreaker7 2:b878d61bf57b 14 float zero_level[3] = {0, 0, 0};
codebreaker7 2:b878d61bf57b 15 uint8_t buffer[100];
codebreaker7 2:b878d61bf57b 16 lcd.Init();
codebreaker7 2:b878d61bf57b 17 lcd.DisplayStringAt(10, 10, (uint8_t*)"Gyroscope_started", CENTER_MODE);
codebreaker7 2:b878d61bf57b 18 uint8_t tmpreg = 0;
codebreaker7 2:b878d61bf57b 19 GYRO_IO_Read(&tmpreg,L3GD20_CTRL_REG4_ADDR,1);
codebreaker7 2:b878d61bf57b 20 switch(tmpreg & L3GD20_FULLSCALE_SELECTION)
codebreaker7 2:b878d61bf57b 21 {
codebreaker7 2:b878d61bf57b 22 case L3GD20_FULLSCALE_250:
codebreaker7 2:b878d61bf57b 23 lcd.DisplayStringAt(20, 20, (uint8_t*)"250", CENTER_MODE);
codebreaker7 2:b878d61bf57b 24 break;
codebreaker7 2:b878d61bf57b 25
codebreaker7 2:b878d61bf57b 26 case L3GD20_FULLSCALE_500:
codebreaker7 2:b878d61bf57b 27 lcd.DisplayStringAt(20, 20, (uint8_t*)"500", CENTER_MODE);
codebreaker7 2:b878d61bf57b 28 break;
codebreaker7 2:b878d61bf57b 29
codebreaker7 2:b878d61bf57b 30 case L3GD20_FULLSCALE_2000:
codebreaker7 2:b878d61bf57b 31 lcd.DisplayStringAt(20, 20, (uint8_t*)"2000", CENTER_MODE);
codebreaker7 2:b878d61bf57b 32 break;
codebreaker7 2:b878d61bf57b 33 }
codebreaker7 2:b878d61bf57b 34 GYRO_IO_Read(&tmpreg, L3GD20_CTRL_REG1_ADDR, 1);
codebreaker7 2:b878d61bf57b 35 switch(tmpreg >> 6) {
codebreaker7 2:b878d61bf57b 36 case 0:
codebreaker7 2:b878d61bf57b 37 lcd.DisplayStringAt(20, 40, (uint8_t*)"95", CENTER_MODE);
codebreaker7 2:b878d61bf57b 38 break;
codebreaker7 2:b878d61bf57b 39 case 1:
codebreaker7 2:b878d61bf57b 40 lcd.DisplayStringAt(20, 40, (uint8_t*)"190", CENTER_MODE);
codebreaker7 2:b878d61bf57b 41 break;
codebreaker7 2:b878d61bf57b 42 case 2:
codebreaker7 2:b878d61bf57b 43 lcd.DisplayStringAt(20, 40, (uint8_t*)"380", CENTER_MODE);
codebreaker7 2:b878d61bf57b 44 break;
codebreaker7 2:b878d61bf57b 45 case 3:
codebreaker7 2:b878d61bf57b 46 lcd.DisplayStringAt(20, 40, (uint8_t*)"760", CENTER_MODE);
codebreaker7 2:b878d61bf57b 47 break;
codebreaker7 2:b878d61bf57b 48 }
codebreaker7 3:68f55e42c85f 49
codebreaker7 2:b878d61bf57b 50 for (int i = 0; i < 50; i++) {
codebreaker7 2:b878d61bf57b 51 gyro.GetXYZ(GyroBuffer);
codebreaker7 2:b878d61bf57b 52 zero_level[0] += GyroBuffer[0] / 17.5f;
codebreaker7 2:b878d61bf57b 53 zero_level[1] += GyroBuffer[1] / 17.5f;
codebreaker7 2:b878d61bf57b 54 zero_level[2] += GyroBuffer[2] / 17.5f;
codebreaker7 3:68f55e42c85f 55
codebreaker7 2:b878d61bf57b 56 wait_ms(10);
codebreaker7 2:b878d61bf57b 57 }
codebreaker7 2:b878d61bf57b 58 for (int i = 0; i < 3; i++)
codebreaker7 2:b878d61bf57b 59 zero_level[i] /= 50;
codebreaker7 2:b878d61bf57b 60
codebreaker7 2:b878d61bf57b 61 float noise_level = 0.0;
codebreaker7 2:b878d61bf57b 62 for (int i = 0; i < 50; i++) {
codebreaker7 2:b878d61bf57b 63 gyro.GetXYZ(GyroBuffer);
codebreaker7 2:b878d61bf57b 64 if ((GyroBuffer[0] / 17.5f - zero_level[0]) > noise_level) {
codebreaker7 2:b878d61bf57b 65 noise_level = (GyroBuffer[0] / 17.5f - zero_level[0]);
codebreaker7 2:b878d61bf57b 66 }
codebreaker7 2:b878d61bf57b 67 else if ((GyroBuffer[0] / 17.5f - zero_level[0]) < -noise_level) {
codebreaker7 2:b878d61bf57b 68 noise_level = -(GyroBuffer[0] / 17.5f - zero_level[0]);
codebreaker7 2:b878d61bf57b 69 }
codebreaker7 2:b878d61bf57b 70 wait_ms(10);
codebreaker7 2:b878d61bf57b 71 }
codebreaker7 2:b878d61bf57b 72 sprintf((char*)buffer, "Noise = %.2f\n", noise_level);
codebreaker7 2:b878d61bf57b 73 lcd.DisplayStringAt(20, 70, buffer, CENTER_MODE);
codebreaker7 2:b878d61bf57b 74 float angle = 0.0, prev_value = 0.0;
bcostm 0:44f624c5501e 75 while(1) {
bcostm 0:44f624c5501e 76 // Read Gyroscope values
codebreaker7 2:b878d61bf57b 77 wait(0.02);
codebreaker7 3:68f55e42c85f 78
bcostm 0:44f624c5501e 79 gyro.GetXYZ(GyroBuffer);
codebreaker7 2:b878d61bf57b 80 float value = GyroBuffer[0] / 17.5f - zero_level[0];
codebreaker7 2:b878d61bf57b 81 if (value >= noise_level || value <= -noise_level) {
codebreaker7 2:b878d61bf57b 82 angle += (value + prev_value) / 2 * 0.0175 * 0.02f / 0.825;
codebreaker7 2:b878d61bf57b 83 }
codebreaker7 2:b878d61bf57b 84 prev_value = value;
codebreaker7 2:b878d61bf57b 85 sprintf((char*)buffer, "Angle = %.3f\n", angle);
codebreaker7 2:b878d61bf57b 86 lcd.DisplayStringAt(20, 100, buffer, CENTER_MODE);
codebreaker7 3:68f55e42c85f 87
bcostm 0:44f624c5501e 88 }
bcostm 0:44f624c5501e 89 }