1 #include "compass.h"
2 #include "drv_mag_qst_qmc5883l.h"
3 #include "drv_mag_qst_qmc6310.h"
4 #include <stdio.h>
5 #include <stdlib.h>
6
7 #define COMPASS_CENTER_X 38
8 #define COMPASS_CENTER_Y 32
9
10 // 严格限制长度
11 static char code_str[3] = " "; // ES
12 static char number_str[4] = " "; // 329
13 static int heading = 0;
14 static uint8_t arror_len = 16;
15
16 static int running = 1;
17
18 MENU_COVER_TYP compass_cover = {MENU_COVER_NONE};
19 MENU_TASK_TYP compass_tasks = {compass_init, compass_uninit};
20 MENU_TYP compass = {"compass", &compass_cover, &compass_tasks, NULL, NULL};
21
22 static aos_task_t compass_task_handle;
23
compass_init(void)24 int compass_init(void)
25 {
26 if (g_haasboard_is_k1c) {
27 qmc6310_init();
28 LOGI(EDU_TAG, "qmc6310_init done\n");
29 } else {
30 qmc5883l_init();
31 LOGI(EDU_TAG, "qmc5883l_init done\n");
32 }
33
34 OLED_Clear();
35 OLED_Refresh_GRAM();
36 aos_task_new_ext(&compass_task_handle, "compass_task", compass_task, NULL, 1024, AOS_DEFAULT_APP_PRI);
37 LOGI(EDU_TAG, "aos_task_new compass_task\n");
38 return 0;
39 }
40
compass_task()41 void compass_task()
42 {
43 while (running) {
44 if (g_haasboard_is_k1c) {
45 heading = qmc6310_readHeading();
46 LOGD(EDU_TAG, "heading %d\n", heading);
47 } else {
48 heading = qmc5883l_readHeading();
49 LOGD(EDU_TAG, "heading %d\n", heading);
50 }
51 OLED_Clear();
52 OLED_Icon_Draw(COMPASS_CENTER_X - 27, COMPASS_CENTER_Y - 27,
53 &icon_compass_55_55, 0);
54 OLED_Icon_Draw(72, 4, &icon_compass_arror_24_24, 0);
55 format_compass_str(number_str, code_str, (-heading), &arror_len);
56 OLED_DrawLine_ByAngle(COMPASS_CENTER_X, COMPASS_CENTER_Y,
57 (-heading - 90), arror_len, 1);
58 OLED_Show_String(96, 4, code_str, 24, 1);
59 OLED_Show_String(78, 36, number_str, 24, 1);
60 OLED_Icon_Draw(2, 24, &icon_skip_left, 0);
61 OLED_Icon_Draw(122, 24, &icon_skip_right, 0);
62 OLED_Refresh_GRAM();
63
64 aos_msleep(30);
65 }
66
67 running = 1;
68 }
69
compass_uninit(void)70 int compass_uninit(void)
71 {
72 running = 0;
73
74 while (!running) {
75 aos_msleep(50);
76 }
77 if (g_haasboard_is_k1c) {
78 qmc6310_deinit();
79 } else {
80 qmc5883l_deinit();
81 }
82
83 aos_task_delete(&compass_task_handle);
84 LOGI(EDU_TAG, "aos_task_delete compass_task\n");
85 return 0;
86 }
87
format_compass_str(char * number_str,char * code_str,int heading,uint8_t * arror_len)88 void format_compass_str(char *number_str,
89 char *code_str,
90 int heading,
91 uint8_t *arror_len)
92 {
93 heading = (heading % 360 < 0) ? (heading % 360 + 360) : (heading % 360);
94 sprintf(number_str, "%3d", heading);
95 if ((heading > 0 && heading <= 23) || (heading > 337 && heading <= 360)) {
96 sprintf(code_str, " N", heading);
97 *arror_len = 16;
98 } else if (heading > 292 && heading <= 337) {
99 sprintf(code_str, "WN", heading);
100 *arror_len = 20;
101 } else if (heading > 246 && heading <= 292) {
102 sprintf(code_str, " W", heading);
103 *arror_len = 16;
104 } else if (heading > 202 && heading <= 246) {
105 sprintf(code_str, "WS", heading);
106 *arror_len = 20;
107 } else if (heading > 158 && heading <= 202) {
108 sprintf(code_str, " S", heading);
109 *arror_len = 16;
110 } else if (heading > 112 && heading <= 158) {
111 sprintf(code_str, "ES", heading);
112 *arror_len = 20;
113 } else if (heading > 67 && heading <= 112) {
114 sprintf(code_str, " E", heading);
115 *arror_len = 16;
116 } else if (heading > 23 && heading <= 67) {
117 sprintf(code_str, "EN", heading);
118 *arror_len = 20;
119 }
120 }
121