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@2:b878d61bf57b, 2017-02-16 (annotated)

Committer:
codebreaker7
Date:
Thu Feb 16 11:43:51 2017 +0000
Revision:
2:b878d61bf57b
Parent:
0:44f624c5501e
Child:
3:68f55e42c85f
Gyro example

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 2:b878d61bf57b 49 //BSP_LCD_Init();
codebreaker7 2:b878d61bf57b 50 //BSP_LCD_De
codebreaker7 2:b878d61bf57b 51 //BSP_LCD_DisplayStringAt(10,10, (uint8_t*)"Gyroscope_started", CENTER_MODE);
codebreaker7 2:b878d61bf57b 52
codebreaker7 2:b878d61bf57b 53 for (int i = 0; i < 50; i++) {
codebreaker7 2:b878d61bf57b 54 gyro.GetXYZ(GyroBuffer);
codebreaker7 2:b878d61bf57b 55 zero_level[0] += GyroBuffer[0] / 17.5f;
codebreaker7 2:b878d61bf57b 56 zero_level[1] += GyroBuffer[1] / 17.5f;
codebreaker7 2:b878d61bf57b 57 zero_level[2] += GyroBuffer[2] / 17.5f;
codebreaker7 2:b878d61bf57b 58 //zero_level[0] += GyroBuffer[0];
codebreaker7 2:b878d61bf57b 59 //zero_level[1] += GyroBuffer[1];
codebreaker7 2:b878d61bf57b 60 //zero_level[2] += GyroBuffer[2];
codebreaker7 2:b878d61bf57b 61 wait_ms(10);
codebreaker7 2:b878d61bf57b 62 }
codebreaker7 2:b878d61bf57b 63 for (int i = 0; i < 3; i++)
codebreaker7 2:b878d61bf57b 64 zero_level[i] /= 50;
codebreaker7 2:b878d61bf57b 65
codebreaker7 2:b878d61bf57b 66 float noise_level = 0.0;
codebreaker7 2:b878d61bf57b 67 for (int i = 0; i < 50; i++) {
codebreaker7 2:b878d61bf57b 68 gyro.GetXYZ(GyroBuffer);
codebreaker7 2:b878d61bf57b 69 if ((GyroBuffer[0] / 17.5f - zero_level[0]) > noise_level) {
codebreaker7 2:b878d61bf57b 70 noise_level = (GyroBuffer[0] / 17.5f - zero_level[0]);
codebreaker7 2:b878d61bf57b 71 }
codebreaker7 2:b878d61bf57b 72 else if ((GyroBuffer[0] / 17.5f - zero_level[0]) < -noise_level) {
codebreaker7 2:b878d61bf57b 73 noise_level = -(GyroBuffer[0] / 17.5f - zero_level[0]);
codebreaker7 2:b878d61bf57b 74 }
codebreaker7 2:b878d61bf57b 75 wait_ms(10);
codebreaker7 2:b878d61bf57b 76 }
codebreaker7 2:b878d61bf57b 77 sprintf((char*)buffer, "Noise = %.2f\n", noise_level);
codebreaker7 2:b878d61bf57b 78 lcd.DisplayStringAt(20, 70, buffer, CENTER_MODE);
codebreaker7 2:b878d61bf57b 79 float angle = 0.0, prev_value = 0.0;
bcostm 0:44f624c5501e 80 while(1) {
bcostm 0:44f624c5501e 81 // Read Gyroscope values
codebreaker7 2:b878d61bf57b 82 wait(0.02);
codebreaker7 2:b878d61bf57b 83 //GYRO_IO_Read(&tmpreg, L3GD20_STATUS_REG_ADDR, 1);
codebreaker7 2:b878d61bf57b 84 //while((tmpreg >> 3) & 0x01 != 1) GYRO_IO_Read(&tmpreg, L3GD20_STATUS_REG_ADDR, 1);
bcostm 0:44f624c5501e 85 gyro.GetXYZ(GyroBuffer);
codebreaker7 2:b878d61bf57b 86 float value = GyroBuffer[0] / 17.5f - zero_level[0];
codebreaker7 2:b878d61bf57b 87 if (value >= noise_level || value <= -noise_level) {
codebreaker7 2:b878d61bf57b 88 angle += (value + prev_value) / 2 * 0.0175 * 0.02f / 0.825;
codebreaker7 2:b878d61bf57b 89 }
codebreaker7 2:b878d61bf57b 90 prev_value = value;
codebreaker7 2:b878d61bf57b 91 sprintf((char*)buffer, "Angle = %.3f\n", angle);
codebreaker7 2:b878d61bf57b 92 lcd.DisplayStringAt(20, 100, buffer, CENTER_MODE);
bcostm 0:44f624c5501e 93 // Display values
codebreaker7 2:b878d61bf57b 94 //sprintf((char*)buffer, "X = %.2f\n", (GyroBuffer[0] - zero_level[0]) / 1000);
codebreaker7 2:b878d61bf57b 95 //sprintf((char*)buffer, "X = %.2f\n", GyroBuffer[0] / 17.5f - zero_level[0]);
codebreaker7 2:b878d61bf57b 96 //lcd.DisplayStringAt(20, 10, buffer, CENTER_MODE);
codebreaker7 2:b878d61bf57b 97 //sprintf((char*)buffer, "Y = %.2f\n", (GyroBuffer[1] - zero_level[1]) / 1000);
codebreaker7 2:b878d61bf57b 98 //sprintf((char*)buffer, "Y = %.2f\n", GyroBuffer[1] / 17.5f - zero_level[1]);
codebreaker7 2:b878d61bf57b 99 //lcd.DisplayStringAt(20, 40, buffer, CENTER_MODE);
codebreaker7 2:b878d61bf57b 100 //sprintf((char*)buffer, "Z = %.2f\n", (GyroBuffer[2] - zero_level[2]) / 1000);
codebreaker7 2:b878d61bf57b 101 //sprintf((char*)buffer, "Z = %.2f\n", GyroBuffer[2] / 17.5f - zero_level[2]);
codebreaker7 2:b878d61bf57b 102 //lcd.DisplayStringAt(20, 70, buffer, CENTER_MODE);
codebreaker7 2:b878d61bf57b 103 //led1 = !led1;
codebreaker7 2:b878d61bf57b 104 //wait(1);
codebreaker7 2:b878d61bf57b 105 //lcd.Clear(0xffffffff);
bcostm 0:44f624c5501e 106 }
bcostm 0:44f624c5501e 107 }