1 #include "gyroscope.h"
2 #include "drv_acc_gyro_inv_mpu6050.h"
3 #include "drv_acc_gyro_qst_qmi8610.h"
4 #include <stdio.h>
5 #include <stdlib.h>
6 
7 MENU_COVER_TYP gyroscope_cover = {MENU_COVER_NONE};
8 MENU_TASK_TYP  gyroscope_tasks = {gyroscope_init, gyroscope_uninit};
9 MENU_TYP gyroscope = {"gyroscope", &gyroscope_cover, &gyroscope_tasks, NULL,
10                       NULL};
11 
12 static aos_task_t gyroscope_task_handle;
13 
14 static short r_ax = 0, r_ay = 0, r_az = 0;
15 static int   running = 1;
16 
17 typedef struct {
18     int x;
19     int y;
20     int z;
21 } node_t;
22 
23 typedef struct {
24     node_t *start_node;
25     node_t *end_node;
26 } line_t;
27 
28 node_t node_list[] = {{0, 0, 0},      {20, 20, 20},   {-20, 20, 20},
29                       {20, -20, 20},  {20, 20, -20},  {-20, -20, 20},
30                       {-20, 20, -20}, {20, -20, -20}, {-20, -20, -20}
31 };
32 
33 line_t line_list[] = {
34     {&node_list[1], &node_list[2]}, {&node_list[1], &node_list[3]},
35     {&node_list[3], &node_list[2]}, {&node_list[3], &node_list[4]},
36     {&node_list[1], &node_list[2]}, {&node_list[1], &node_list[2]},
37     {&node_list[1], &node_list[2]}, {&node_list[1], &node_list[2]},
38     {&node_list[1], &node_list[2]}, {&node_list[1], &node_list[2]},
39     {&node_list[1], &node_list[2]}, {&node_list[1], &node_list[2]},
40 };
41 
gyroscope_init(void)42 int gyroscope_init(void)
43 {
44     if (g_haasboard_is_k1c) {
45         FisImu_init();
46         LOGI(EDU_TAG, "FisImu_init done\n");
47     } else {
48         MPU_Init();
49         LOGI(EDU_TAG, "MPU_Init done\n");
50     }
51 
52     LOGI(EDU_TAG, "MPU_Init done\n");
53     aos_task_new_ext(&gyroscope_task_handle, "gyroscope_task", gyroscope_task, NULL, 1024, AOS_DEFAULT_APP_PRI);
54     LOGI(EDU_TAG, "aos_task_new gyroscope_task\n");
55     return 0;
56 }
57 
gyroscope_task(void)58 void gyroscope_task(void)
59 {
60     float acc[3];
61     short ax, ay;
62 
63     while (running) {
64         OLED_Clear();
65 
66         if (g_haasboard_is_k1c) {
67             FisImu_read_acc_xyz(acc);
68             ax = (short)(acc[1] * 6.6);
69             ay = (short)(acc[0] * 3.2);
70         } else {
71             MPU_Get_Accelerometer(&r_ax, &r_ay, &r_az);
72         }
73         OLED_Icon_Draw(2, 24, &icon_skip_left, 0);
74         OLED_Icon_Draw(122, 24, &icon_skip_right, 0);
75         OLED_DrawCircle(66, 32, 10, 1, 1);
76         if (g_haasboard_is_k1c) {
77             OLED_FillCircle(66 + ax, 32 + ay, 8, 1);
78         } else {
79             OLED_FillCircle(66 - r_ax / 250, 32 + r_ay / 500, 8, 1);
80         }
81         OLED_Refresh_GRAM();
82         aos_msleep(20);
83     }
84 
85     running = 1;
86 }
87 
gyroscope_uninit(void)88 int gyroscope_uninit(void)
89 {
90     running = 0;
91 
92     while (!running) {
93         aos_msleep(50);
94     }
95 
96     if (g_haasboard_is_k1c) {
97         FisImu_deinit();
98         LOGI(EDU_TAG, "FisImu_deinit done\n");
99     } else {
100         MPU_Deinit();
101         LOGI(EDU_TAG, "MPU_Deinit done\n");
102     }
103 
104     aos_task_delete(&gyroscope_task_handle);
105     LOGI(EDU_TAG, "aos_task_delete gyroscope_task\n");
106     return 0;
107 }
108