embox

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

8
#include <util/log.h>
9

10
#include <stdint.h>
11

12
#include <stm32f3_discovery_accelerometer.h>
13

14
#include <embox/unit.h>
15

16
#include "drivers/sensors/acc.h"
17

18
EMBOX_UNIT_INIT(acc_init);
19

20
static float acc_offset[3];
21

22
void acc_get(int16_t *value) {
23
	BSP_ACCELERO_GetXYZ(value);
24
}
25

26
/* mm/s^2 */
27
void acc_data_normalize(int16_t *in, float *out) {
28
	for (int i = 0; i < 3; i++) {
29
		out[i] = (in[i] / 16) * g;
30
	}
31
}
32

33
/* mm/s^2, without offset */
34
void acc_data_obtain(float *out) {
35
	int16_t acc[3] = { 0 };
36

37
	acc_get(acc);
38
	acc_data_normalize(acc, out);
39

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

45
static void acc_calculate_offset(void) {
46
	int16_t temp[3] = { 0 };
47
	float eps = 0.2;
48

49
	for (int i = 0; i < 1000; i++) {
50
		int16_t acc[3] = { 0 };
51

52
		acc_get(acc);
53

54
		for (int j = 0; j < 3; j++) {
55
			temp[j] = (1 - eps) * temp[j] + eps * acc[j];
56
		}
57
	}
58

59
	acc_data_normalize(temp, acc_offset);
60
	acc_offset[2] -= g * 1000;
61
}
62

63
static int acc_init(void) {
64
	int res;
65

66
	res = BSP_ACCELERO_Init();
67
	if (res != HAL_OK) {
68
		log_error("BSP_ACCLEERO_Init failed, returned %d\n", res);
69
		return -1;
70
	}
71

72
	acc_calculate_offset();
73

74
	return res;
75
}
76