MPU-6050使用Python上位机监控实时动作

单片机：STM32F4_MD6
上位机：Python
MPU-6050-Python上位机.zip (19.63 MB, 售价: 1 工控币)

2020-1-20 19:52 上传

点击文件名下载附件

1. #ifdef COMPASS_ENABLED
   
2. void send_status_compass() {
   
3.         long data[3] = { 0 };
   
4.         int8_t accuracy = { 0 };
   
5.         unsigned long timestamp;
   
6.         inv_get_compass_set(data, &accuracy, (inv_time_t*) &timestamp);
   
7.         MPL_LOGI("Compass: %7.4f %7.4f %7.4f ",
   
8.                         data[0]/65536.f, data[1]/65536.f, data[2]/65536.f);
   
9.         MPL_LOGI("Accuracy= %d\r\n", accuracy);
   
10. 
    
11. }
    
12. #endif
    
13. 
    
14. /* Handle sensor on/off combinations. */
    
15. static void setup_gyro(void)
    
16. {
    
17.     unsigned char mask = 0, lp_accel_was_on = 0;
    
18.     if (hal.sensors & ACCEL_ON)
    
19.         mask |= INV_XYZ_ACCEL;
    
20.     if (hal.sensors & GYRO_ON) {
    
21.         mask |= INV_XYZ_GYRO;
    
22.         lp_accel_was_on |= hal.lp_accel_mode;
    
23.     }
    
24. #ifdef COMPASS_ENABLED
    
25.     if (hal.sensors & COMPASS_ON) {
    
26.         mask |= INV_XYZ_COMPASS;
    
27.         lp_accel_was_on |= hal.lp_accel_mode;
    
28.     }
    
29. #endif
    
30.     /* If you need a power transition, this function should be called with a
    
31.      * mask of the sensors still enabled. The driver turns off any sensors
    
32.      * excluded from this mask.
    
33.      */
    
34.     mpu_set_sensors(mask);
    
35.     mpu_configure_fifo(mask);
    
36.     if (lp_accel_was_on) {
    
37.         unsigned short rate;
    
38.         hal.lp_accel_mode = 0;
    
39.         /* Switching out of LP accel, notify MPL of new accel sampling rate. */
    
40.         mpu_get_sample_rate(&rate);
    
41.         inv_set_accel_sample_rate(1000000L / rate);
    
42.     }
    
43. }
    
44. 
    
45. static void tap_cb(unsigned char direction, unsigned char count)
    
46. {
    
47.     switch (direction) {
    
48.     case TAP_X_UP:
    
49.         MPL_LOGI("Tap X+ ");
    
50.         break;
    
51.     case TAP_X_DOWN:
    
52.         MPL_LOGI("Tap X- ");
    
53.         break;
    
54.     case TAP_Y_UP:
    
55.         MPL_LOGI("Tap Y+ ");
    
56.         break;
    
57.     case TAP_Y_DOWN:
    
58.         MPL_LOGI("Tap Y- ");
    
59.         break;
    
60.     case TAP_Z_UP:
    
61.         MPL_LOGI("Tap Z+ ");
    
62.         break;
    
63.     case TAP_Z_DOWN:
    
64.         MPL_LOGI("Tap Z- ");
    
65.         break;
    
66.     default:
    
67.         return;
    
68.     }
    
69.     MPL_LOGI("x%d\n", count);
    
70.     return;
    
71. }
    
72. 
    
73. static void android_orient_cb(unsigned char orientation)
    
74. {
    
75.         switch (orientation) {
    
76.         case ANDROID_ORIENT_PORTRAIT:
    
77.         MPL_LOGI("Portrait\n");
    
78.         break;
    
79.         case ANDROID_ORIENT_LANDSCAPE:
    
80.         MPL_LOGI("Landscape\n");
    
81.         break;
    
82.         case ANDROID_ORIENT_REVERSE_PORTRAIT:
    
83.         MPL_LOGI("Reverse Portrait\n");
    
84.         break;
    
85.         case ANDROID_ORIENT_REVERSE_LANDSCAPE:
    
86.         MPL_LOGI("Reverse Landscape\n");
    
87.         break;
    
88.         default:
    
89.                 return;
    
90.         }
    
91. }
    
92. 
    
93. 
    
94. static inline void run_self_test(void)
    
95. {
    
96.     int result;
    
97.     long gyro[3], accel[3];
    
98. 
    
99. #if defined (MPU6500) || defined (MPU9250)
    
100.     result = mpu_run_6500_self_test(gyro, accel, 0);
     
101. #elif defined (MPU6050) || defined (MPU9150)
     
102.     result = mpu_run_self_test(gyro, accel);
     
103. #endif
     
104.     if (result == 0x7) {
     
105.         MPL_LOGI("Passed!\n");
     
106.         MPL_LOGI("accel: %7.4f %7.4f %7.4f\n",
     
107.                     accel[0]/65536.f,
     
108.                     accel[1]/65536.f,
     
109.                     accel[2]/65536.f);
     
110.         MPL_LOGI("gyro: %7.4f %7.4f %7.4f\n",
     
111.                     gyro[0]/65536.f,
     
112.                     gyro[1]/65536.f,
     
113.                     gyro[2]/65536.f);
     
114.         /* Test passed. We can trust the gyro data here, so now we need to update calibrated data*/
     
115. 
     
116. #ifdef USE_CAL_HW_REGISTERS
     
117.         /*
     
118.          * This portion of the code uses the HW offset registers that are in the MPUxxxx devices
     
119.          * instead of pushing the cal data to the MPL software library
     
120.          */
     
121.         unsigned char i = 0;
     
122. 
     
123.         for(i = 0; i<3; i++) {
     
124.                 gyro[i] = (long)(gyro[i] * 32.8f); //convert to +-1000dps
     
125.                 accel[i] *= 2048.f; //convert to +-16G
     
126.                 accel[i] = accel[i] >> 16;
     
127.                 gyro[i] = (long)(gyro[i] >> 16);
     
128.         }
     
129. 
     
130.         mpu_set_gyro_bias_reg(gyro);
     
131. 
     
132. #if defined (MPU6500) || defined (MPU9250)
     
133.         mpu_set_accel_bias_6500_reg(accel);
     
134. #elif defined (MPU6050) || defined (MPU9150)
     
135.         mpu_set_accel_bias_6050_reg(accel);
     
136. #endif
     
137. #else
     
138.         /* Push the calibrated data to the MPL library.
     
139.          *
     
140.          * MPL expects biases in hardware units << 16, but self test returns
     
141.                  * biases in g's << 16.
     
142.                  */
     
143.             unsigned short accel_sens;
     
144.             float gyro_sens;
     
145. 
     
146.                 mpu_get_accel_sens(&accel_sens);
     
147.                 accel[0] *= accel_sens;
     
148.                 accel[1] *= accel_sens;
     
149.                 accel[2] *= accel_sens;
     
150.                 inv_set_accel_bias(accel, 3);
     
151.                 mpu_get_gyro_sens(&gyro_sens);
     
152.                 gyro[0] = (long) (gyro[0] * gyro_sens);
     
153.                 gyro[1] = (long) (gyro[1] * gyro_sens);
     
154.                 gyro[2] = (long) (gyro[2] * gyro_sens);
     
155.                 inv_set_gyro_bias(gyro, 3);
     
156. #endif
     
157.     }
     
158.     else {
     
159.             if (!(result & 0x1))
     
160.                 MPL_LOGE("Gyro failed.\n");
     
161.             if (!(result & 0x2))
     
162.                 MPL_LOGE("Accel failed.\n");
     
163.             if (!(result & 0x4))
     
164.                 MPL_LOGE("Compass failed.\n");
     
165.      }
     
166. 
     
167. }
     
168. 
     

复制代码