main.c 8.74 KB
Newer Older
schneider's avatar
schneider committed
1
/*******************************************************************************
2
 * License: TBD
schneider's avatar
schneider committed
3
4
5
 ******************************************************************************/

/***** Includes *****/
6
#include "bhy_uc_driver.h"
schneider's avatar
schneider committed
7
#include "board.h"
8
9
10
#include "display.h"
#include "gfx.h"
#include "gpio.h"
schneider's avatar
schneider committed
11
#include "i2c.h"
12
13
14
#include "led.h"
#include "mxc_config.h"
#include "pmic.h"
schneider's avatar
schneider committed
15
16
#include "rtc.h"
#include "spi.h"
17
#include "tmr_utils.h"
schneider's avatar
schneider committed
18

19
#include "card10.h"
schneider's avatar
schneider committed
20

21
#include <math.h>
22
#include <stdbool.h>
23
#include <stdint.h>
24
#include <stdio.h>
25
#include <string.h>
schneider's avatar
schneider committed
26

Rahix's avatar
Rahix committed
27
#define M_PI 3.1415
28
/***** Definitions *****/
schneider's avatar
schneider committed
29

30
31
/* should be greater or equal to 69 bytes, page size (50) + maximum packet
 * size(18) + 1 */
Rahix's avatar
Rahix committed
32
33
34
35
#define FIFO_SIZE 300
#define ROTATION_VECTOR_SAMPLE_RATE 10
#define MAX_PACKET_LENGTH 18
#define OUT_BUFFER_SIZE 60
schneider's avatar
schneider committed
36
37

/***** Globals *****/
Rahix's avatar
Rahix committed
38
39
char out_buffer[OUT_BUFFER_SIZE] =
	" W: 0.999  X: 0.999  Y: 0.999  Z: 0.999   \r";
schneider's avatar
schneider committed
40
41
uint8_t fifo[FIFO_SIZE];

42
43
void draw_arrow(int angle, int color)
{
Rahix's avatar
Rahix committed
44
45
	float sin = sinf(angle * 2 * M_PI / 360.);
	float cos = cosf(angle * 2 * M_PI / 360.);
46

Rahix's avatar
Rahix committed
47
48
	int x1 = 160 / 2 + 30;
	int y1 = 80 / 2;
49

Rahix's avatar
Rahix committed
50
51
	int x2 = x1 - sin * 30;
	int y2 = y1 - cos * 30;
52

53
	gfx_line(&display_screen, x1, y1, x2, y2, 2, color);
54

Rahix's avatar
Rahix committed
55
56
	sin = sinf((angle - 140) * 2 * M_PI / 360.);
	cos = cosf((angle - 140) * 2 * M_PI / 360.);
57

Rahix's avatar
Rahix committed
58
59
	int x3 = x2 - sin * 10;
	int y3 = y2 - cos * 10;
60

61
	gfx_line(&display_screen, x2, y2, x3, y3, 2, color);
62

Rahix's avatar
Rahix committed
63
64
	sin = sinf((angle + 140) * 2 * M_PI / 360.);
	cos = cosf((angle + 140) * 2 * M_PI / 360.);
65

Rahix's avatar
Rahix committed
66
67
	int x4 = x2 - sin * 10;
	int y4 = y2 - cos * 10;
68

69
	gfx_line(&display_screen, x2, y2, x4, y4, 2, color);
70
}
schneider's avatar
schneider committed
71
72

/***** Functions *****/
Rahix's avatar
Rahix committed
73
74
75
76
77
78
79
80
81
82
83
84
85
static void sensors_callback_orientation(
	bhy_data_generic_t *sensor_data, bhy_virtual_sensor_t sensor_id
) {
	static int prev = -1;
	printf("azimuth=%05d, pitch=%05d, roll=%05d status=%d\n",
	       sensor_data->data_vector.x * 360 / 32768,
	       sensor_data->data_vector.y * 360 / 32768,
	       sensor_data->data_vector.z * 360 / 32768,
	       sensor_data->data_vector.status);

	int angle = sensor_data->data_vector.x * 360 / 32768;

	if (angle != prev) {
86
87
88
89
90
91
		gfx_clear(&display_screen);

		int colors[] = { gfx_color(&display_screen, RED),
				 gfx_color(&display_screen, YELLOW),
				 gfx_color(&display_screen, YELLOW),
				 gfx_color(&display_screen, GREEN) };
Rahix's avatar
Rahix committed
92
93
94
95
		int color    = colors[sensor_data->data_vector.status];
		draw_arrow(sensor_data->data_vector.x * 360 / 32768, color);

		char buf[128];
96
97
		// sprintf(buf, "Azimuth: %3d", angle);
		// Paint_DrawString_EN(0, 0, buf, &Font12, BLACK, color);
Rahix's avatar
Rahix committed
98
99

		sprintf(buf, "%3d", angle);
100
101
102
103
104
105
106
107
		gfx_puts(
			&Font24,
			&display_screen,
			0,
			30,
			buf,
			color,
			gfx_color(&display_screen, BLACK)
Rahix's avatar
Rahix committed
108
		);
109
		gfx_circle(&display_screen, 57, 35, 4, 2, color);
Rahix's avatar
Rahix committed
110

111
		gfx_update(&display_screen);
Rahix's avatar
Rahix committed
112
113
		prev = angle;
	}
114
115
}

Rahix's avatar
Rahix committed
116
117
118
119
120
121
122
123
static __attribute__((unused)) void sensors_callback_vector(
	bhy_data_generic_t *sensor_data, bhy_virtual_sensor_t sensor_id
) {
	printf("x=%05d, y=%05d, z=%05d status=%d\n",
	       sensor_data->data_vector.x,
	       sensor_data->data_vector.y,
	       sensor_data->data_vector.z,
	       sensor_data->data_vector.status);
schneider's avatar
schneider committed
124
125
}

Rahix's avatar
Rahix committed
126
127
128
129
130
131
132
133
static __attribute__((unused)) void sensors_callback_vector_uncalib(
	bhy_data_generic_t *sensor_data, bhy_virtual_sensor_t sensor_id
) {
	printf("x=%05d, y=%05d, z=%05d status=%d\n",
	       sensor_data->data_uncalib_vector.x,
	       sensor_data->data_uncalib_vector.y,
	       sensor_data->data_uncalib_vector.z,
	       sensor_data->data_uncalib_vector.status);
134
135
}

schneider's avatar
schneider committed
136
137
138
139
140
141
/*!
 * @brief This function is  callback function for acquring sensor datas
 *
 * @param[in]   sensor_data
 * @param[in]   sensor_id
 */
Rahix's avatar
Rahix committed
142
143
144
static __attribute__((unused)) void sensors_callback_rotation_vector(
	bhy_data_generic_t *sensor_data, bhy_virtual_sensor_t sensor_id
) {
schneider's avatar
schneider committed
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
#if 0
    float temp;
    uint8_t index;

    temp = sensor_data->data_quaternion.w / 16384.0f; /* change the data unit by dividing 16384 */
    out_buffer[3] = temp < 0 ? '-' : ' ';
    temp = temp < 0 ? -temp : temp;
    out_buffer[4] = floorf(temp) + '0';

    for (index = 6; index <= 8; index++)
    {
        temp = (temp - floorf(temp)) * 10;
        out_buffer[index] = floorf(temp) + '0';
    }

    temp = sensor_data->data_quaternion.x / 16384.0f;
    out_buffer[13] = temp < 0 ? '-' : ' ';
    temp = temp < 0 ? -temp : temp;
    out_buffer[14] = floorf(temp) + '0';

    for (index = 16; index <= 18; index++)
    {
        temp = (temp - floorf(temp)) * 10;
        out_buffer[index] = floorf(temp) + '0';
    }

    temp = sensor_data->data_quaternion.y / 16384.0f;
    out_buffer[23] = temp < 0 ? '-' : ' ';
    temp = temp < 0 ? -temp : temp;
    out_buffer[24] = floorf(temp) + '0';

    for (index = 26; index <= 28; index++)
    {
        temp = (temp - floorf(temp)) * 10;
        out_buffer[index] = floorf(temp) + '0';
    }

    temp = sensor_data->data_quaternion.z / 16384.0f;
    out_buffer[33] = temp < 0 ? '-' : ' ';
    temp = temp < 0 ? -temp : temp;
    out_buffer[34] = floorf(temp) + '0';

    for (index = 36; index <= 38; index++)
    {
        temp = (temp - floorf(temp)) * 10;
        out_buffer[index] = floorf(temp) + '0';
    }
#endif

Rahix's avatar
Rahix committed
194
195
196
197
198
	printf("x=%d, y=%d, z=%d, w=%d\n",
	       sensor_data->data_quaternion.x,
	       sensor_data->data_quaternion.y,
	       sensor_data->data_quaternion.z,
	       sensor_data->data_quaternion.w);
schneider's avatar
schneider committed
199
200
201
202
203
}

// *****************************************************************************
int main(void)
{
Rahix's avatar
Rahix committed
204
205
206
207
208
209
210
211
212
213
214
	/* BHY Variable*/
	uint8_t *fifoptr           = NULL;
	uint8_t bytes_left_in_fifo = 0;
	uint16_t bytes_remaining   = 0;
	uint16_t bytes_read        = 0;
	bhy_data_generic_t fifo_packet;
	bhy_data_type_t packet_type;
	BHY_RETURN_FUNCTION_TYPE result;

	card10_init();
	card10_diag();
215

schneider's avatar
schneider committed
216
217
218
219
220
221
222
223
#if 0
    /* install the callback function for parse fifo data */
    if(bhy_install_sensor_callback(VS_TYPE_ROTATION_VECTOR, VS_WAKEUP, sensors_callback_rotation_vector))
    {
        printf("Fail to install sensor callback\n");
    }
#endif

224
225
226
227
228
229
230
231
	// if(bhy_install_sensor_callback(VS_TYPE_GEOMAGNETIC_FIELD, VS_WAKEUP,
	// sensors_callback_vector))
	// if(bhy_install_sensor_callback(VS_TYPE_GRAVITY, VS_WAKEUP,
	// sensors_callback_vector))
	// if(bhy_install_sensor_callback(VS_TYPE_ACCELEROMETER, VS_WAKEUP,
	// sensors_callback_vector))
	// if(bhy_install_sensor_callback(VS_TYPE_MAGNETIC_FIELD_UNCALIBRATED,
	// VS_WAKEUP, sensors_callback_vector_uncalib))
Rahix's avatar
Rahix committed
232
233
234
235
236
237
	if (bhy_install_sensor_callback(
		    VS_TYPE_ORIENTATION,
		    VS_WAKEUP,
		    sensors_callback_orientation)) {
		printf("Fail to install sensor callback\n");
	}
schneider's avatar
schneider committed
238
239
240
241
242
243
244
245
246

#if 0
    /* enables the virtual sensor */
    if(bhy_enable_virtual_sensor(VS_TYPE_ROTATION_VECTOR, VS_WAKEUP, ROTATION_VECTOR_SAMPLE_RATE, 0, VS_FLUSH_NONE, 0, 0))
    {
        printf("Fail to enable sensor id=%d\n", VS_TYPE_ROTATION_VECTOR);
    }
#endif

Rahix's avatar
Rahix committed
247
	/* enables the virtual sensor */
248
249
250
251
252
253
254
255
	// if(bhy_enable_virtual_sensor(VS_TYPE_GEOMAGNETIC_FIELD, VS_WAKEUP,
	// ROTATION_VECTOR_SAMPLE_RATE, 0, VS_FLUSH_NONE, 0, 0))
	// if(bhy_enable_virtual_sensor(VS_TYPE_GRAVITY, VS_WAKEUP,
	// ROTATION_VECTOR_SAMPLE_RATE, 0, VS_FLUSH_NONE, 0, 0))
	// if(bhy_enable_virtual_sensor(VS_TYPE_ACCELEROMETER, VS_WAKEUP,
	// ROTATION_VECTOR_SAMPLE_RATE, 0, VS_FLUSH_NONE, 0, 0))
	// if(bhy_enable_virtual_sensor(VS_TYPE_MAGNETIC_FIELD_UNCALIBRATED,
	// VS_WAKEUP, ROTATION_VECTOR_SAMPLE_RATE, 0, VS_FLUSH_NONE, 0, 0))
Rahix's avatar
Rahix committed
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
	if (bhy_enable_virtual_sensor(
		    VS_TYPE_ORIENTATION,
		    VS_WAKEUP,
		    ROTATION_VECTOR_SAMPLE_RATE,
		    0,
		    VS_FLUSH_NONE,
		    0,
		    0)) {
		printf("Fail to enable sensor id=%d\n",
		       VS_TYPE_GEOMAGNETIC_FIELD);
	}

	while (1) {
		/* wait until the interrupt fires */
		/* unless we already know there are bytes remaining in the fifo */
		while (!GPIO_InGet(&bhi_interrupt_pin) && !bytes_remaining)
			;

		bhy_read_fifo(
			fifo + bytes_left_in_fifo,
			FIFO_SIZE - bytes_left_in_fifo,
			&bytes_read,
			&bytes_remaining
		);
		bytes_read += bytes_left_in_fifo;
		fifoptr     = fifo;
		packet_type = BHY_DATA_TYPE_PADDING;

		do {
			/* this function will call callbacks that are registered */
			result = bhy_parse_next_fifo_packet(
				&fifoptr,
				&bytes_read,
				&fifo_packet,
				&packet_type
			);

			/* prints all the debug packets */
			if (packet_type == BHY_DATA_TYPE_DEBUG) {
				bhy_print_debug_packet(
					&fifo_packet.data_debug, bhy_printf
				);
			}

300
301
302
303
			/* the logic here is that if doing a partial parsing of the fifo, then we
       * should not parse  */
			/* the last 18 bytes (max length of a packet) so that we don't try to
       * parse an incomplete   */
Rahix's avatar
Rahix committed
304
305
306
307
308
309
310
311
312
313
314
315
316
317
			/* packet */
		} while ((result == BHY_SUCCESS) &&
			 (bytes_read >
			  (bytes_remaining ? MAX_PACKET_LENGTH : 0)));

		bytes_left_in_fifo = 0;

		if (bytes_remaining) {
			/* shifts the remaining bytes to the beginning of the buffer */
			while (bytes_left_in_fifo < bytes_read) {
				fifo[bytes_left_in_fifo++] = *(fifoptr++);
			}
		}
	}
schneider's avatar
schneider committed
318
}