embox

1
/**
2
 * @file
3
 *
4
 * @date 06 july 2015
5
 * @author Anton Bondarev
6
 */
7

8
#include <util/log.h>
9

10
#include <math.h>
11

12
#include <stm32f3_discovery_gyroscope.h>
13

14
#include <embox/unit.h>
15

16
#include <drivers/sensors/gyro.h>
17

18
EMBOX_UNIT_INIT(gyro_init);
19

20
static float gyro_offset[3];
21

22
void gyro_get(float *value) {
23
	BSP_GYRO_GetXYZ(value);
24
}
25

26
/* mrad/s */
27
static void gyro_data_normalize(float *in, float *out) {
28
	for (int i = 0; i < 3; i++) {
29
		out[i] = in[i] * 8.75f / 360 * 2 * M_PI;
30
	}
31
}
32

33
/* mrad/s, without offset */
34
void gyro_data_obtain(float *out) {
35
	float gyro[3] = { 0 };
36

37
	gyro_get(gyro);
38
	gyro_data_normalize(gyro, out);
39

40
	for (int i = 0; i < 3; i++) {
41
		out[i] -= gyro_offset[i];
42
	}
43
}
44

45
static void gyro_calculate_offset(void) {
46
	float eps = 0.2;
47

48
	for (int i = 0; i < 1000; i++) {
49
		float gyro[3] = { 0 };
50

51
		gyro_get(gyro);
52

53
		for (int j = 0; j < 3; j++) {
54
			gyro_offset[j] = (1 - eps) * gyro_offset[j] + eps * gyro[j];
55
		}
56
	}
57

58
	gyro_data_normalize(gyro_offset, gyro_offset);
59
}
60

61
static int gyro_init(void) {
62
	int res;
63

64
	res = BSP_GYRO_Init();
65
	if (res != HAL_OK) {
66
		log_error("BSP_GYRO_Init failed, returned %d\n", res);
67
		return -1;
68
	}
69
	gyro_calculate_offset();
70

71
	return 0;
72
}
73