annotate Small_CPU/Src/compass.c @ 481:89f6857276f8 Improve_Button_Sleep

Bugfix calculation of string center position: Strings shown on the right side at surface mode use special characters ('\016' and '\017') to switch font size. Headline strings like "Desaturation" could also be set to the target font directly, because they do not contain mixed font sizes, but nethertheless they are using the special characters. The function calculating the offset needed to display the strinter with center alignment did not consider the small font size => misalignment during string display. To avoid this an evaluation of the special characters has been added to the helper function.
author ideenmodellierer
date Mon, 18 May 2020 21:51:08 +0200
parents f9458e979154
children 9eeab3fead8f
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
rev   line source
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1 /**
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2 ******************************************************************************
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
3 * @file compass.c
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
4 * @author heinrichs weikamp gmbh
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
5 * @date 27-March-2014
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
6 * @version V0.2.0
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
7 * @since 21-April-2016
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
8 * @brief for Honeywell Compass and ST LSM303D
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
9 *
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
10 @verbatim
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
11 ==============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
12 ##### How to use #####
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
13 ==============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
14 V0.1.0 09-March-2016
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
15 V0.2.0 21-April-2016 Orientation fixed for LSM303D,
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
16 roll and pitch added to calibration output,
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
17 orientation double checked with datasheets and layout
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
18 as well as with value output during calibration
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
19 V0.2.1 19-May-2016 New date rate config and full-scale selection
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
20
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
21 @endverbatim
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
22 ******************************************************************************
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
23 * @attention
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
24 *
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
25 * <h2><center>&copy; COPYRIGHT(c) 2016 heinrichs weikamp</center></h2>
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
26 *
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
27 ******************************************************************************
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
28 */
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
29
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
30 #include <math.h>
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
31 #include <string.h>
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
32
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
33 #include "compass.h"
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
34 #include "compass_LSM303D.h"
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
35
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
36 #include "i2c.h"
219
f9b17e898a7a Resolved some minor warnings
ideenmodellierer
parents: 207
diff changeset
37 #include "spi.h"
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
38 #include "RTE_FlashAccess.h" // to store compass_calib_data
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
39
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
40 #include "stm32f4xx_hal.h"
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
41
410
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
42 extern uint32_t time_elapsed_ms(uint32_t ticksstart,uint32_t ticksnow);
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
43
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
44 /// split byte to bits
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
45 typedef struct{
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
46 uint8_t bit0:1; ///< split byte to bits
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
47 uint8_t bit1:1; ///< split byte to bits
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
48 uint8_t bit2:1; ///< split byte to bits
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
49 uint8_t bit3:1; ///< split byte to bits
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
50 uint8_t bit4:1; ///< split byte to bits
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
51 uint8_t bit5:1; ///< split byte to bits
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
52 uint8_t bit6:1; ///< split byte to bits
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
53 uint8_t bit7:1; ///< split byte to bits
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
54 } ubit8_t;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
55
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
56
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
57 /// split byte to bits
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
58 typedef union{
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
59 ubit8_t ub; ///< split byte to bits
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
60 uint8_t uw; ///< split byte to bits
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
61 } bit8_Type;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
62
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
63
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
64 /// split word to 2 bytes
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
65 typedef struct{
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
66 uint8_t low; ///< split word to 2 bytes
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
67 uint8_t hi; ///< split word to 2 bytes
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
68 } two_byte;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
69
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
70
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
71 /// split word to 2 bytes
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
72 typedef union{
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
73 two_byte Byte; ///< split word to 2 bytes
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
74 uint16_t Word; ///< split word to 2 bytes
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
75 } tword;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
76
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
77
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
78 /// split signed word to 2 bytes
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
79 typedef union{
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
80 two_byte Byte; ///< split signed word to 2 bytes
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
81 int16_t Word; ///< split signed word to 2 bytes
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
82 } signed_tword;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
83
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
84
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
85 /// split full32 to 2 words
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
86 typedef struct{
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
87 uint16_t low16; ///< split word to 2 bytes
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
88 uint16_t hi16; ///< split word to 2 bytes
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
89 } two_word;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
90
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
91 typedef union{
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
92 two_word Word16; ///< split word to 2 bytes
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
93 uint32_t Full32; ///< split word to 2 bytes
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
94 } tfull32;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
95
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
96
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
97 /// crazy compass calibration stuff
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
98 typedef struct
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
99 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
100 unsigned short int compass_N;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
101 float Su, Sv, Sw;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
102 float Suu, Svv, Sww, Suv, Suw, Svw;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
103 float Suuu, Svvv, Swww;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
104 float Suuv, Suuw, Svvu, Svvw, Swwu, Swwv;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
105 } SCompassCalib;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
106
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
107
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
108 #define Q_PI (18000)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
109 #define Q_PIO2 (9000)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
110
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
111
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
112
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
113 //////////////////////////////////////////////////////////////////////////////
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
114 // fifth order of polynomial approximation of atan(), giving 0.05 deg max error
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
115 //
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
116 #define K1 (5701) // Needs K1/2**16
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
117 #define K2 (1645) // Needs K2/2**48 WAS NEGATIV
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
118 #define K3 ( 446) // Needs K3/2**80
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
119
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
120 const float PI = 3.14159265; ///< pi, used in compass_calc()
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
121
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
122 typedef short int Int16;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
123 typedef signed char Int8;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
124 typedef Int16 Angle;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
125
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
126
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
127 /// The (filtered) components of the magnetometer sensor
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
128 int16_t compass_DX_f; ///< output from sensor
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
129 int16_t compass_DY_f; ///< output from sensor
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
130 int16_t compass_DZ_f; ///< output from sensor
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
131
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
132
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
133 /// Found soft-iron calibration values, deduced from already filtered values
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
134 int16_t compass_CX_f; ///< calibration value
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
135 int16_t compass_CY_f; ///< calibration value
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
136 int16_t compass_CZ_f; ///< calibration value
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
137
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
138
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
139 /// The (filtered) components of the accelerometer sensor
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
140 int16_t accel_DX_f; ///< output from sensor
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
141 int16_t accel_DY_f; ///< output from sensor
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
142 int16_t accel_DZ_f; ///< output from sensor
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
143
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
144
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
145 /// The compass result values
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
146 float compass_heading; ///< the final result calculated in compass_calc()
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
147 float compass_roll; ///< the final result calculated in compass_calc()
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
148 float compass_pitch; ///< the final result calculated in compass_calc()
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
149
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
150
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
151 uint8_t compass_gain; ///< 7 on start, can be reduced during calibration
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
152
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
153 uint8_t hardwareCompass = 0; ///< either HMC5883L (=1) or LSM303D (=2) or LSM303AGR (=3) or not defined yet (=0)
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
154
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
155 /// LSM303D variables
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
156 uint8_t magDataBuffer[6]; ///< here raw data from LSM303D is stored, can be local
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
157 uint8_t accDataBuffer[6]; ///< here raw data from LSM303D is stored, can be local
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
158
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
159
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
160 // struct accel_scale _accel_scale;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
161 unsigned _accel_range_m_s2;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
162 float _accel_range_scale;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
163 unsigned _accel_samplerate;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
164 unsigned _accel_onchip_filter_bandwith;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
165
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
166 // struct mag_scale _mag_scale;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
167 unsigned _mag_range_ga;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
168 float _mag_range_scale;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
169 unsigned _mag_samplerate;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
170
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
171 // default scale factors
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
172 float _accel_scale_x_offset = 0.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
173 float _accel_scale_x_scale = 1.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
174 float _accel_scale_y_offset = 0.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
175 float _accel_scale_y_scale = 1.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
176 float _accel_scale_z_offset = 0.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
177 float _accel_scale_z_scale = 1.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
178
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
179 float _mag_scale_x_offset = 0.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
180 float _mag_scale_x_scale = 1.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
181 float _mag_scale_y_offset = 0.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
182 float _mag_scale_y_scale = 1.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
183 float _mag_scale_z_offset = 0.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
184 float _mag_scale_z_scale = 1.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
185
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
186
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
187 /* External function prototypes ----------------------------------------------*/
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
188
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
189 extern void copyCompassDataDuringCalibration(int16_t dx, int16_t dy, int16_t dz);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
190
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
191 /* Private function prototypes -----------------------------------------------*/
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
192
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
193 void compass_reset_calibration(SCompassCalib *g);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
194 void compass_add_calibration(SCompassCalib *g);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
195 void compass_solve_calibration(SCompassCalib *g);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
196
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
197 void compass_init_HMC5883L(uint8_t fast, uint8_t gain);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
198 void compass_sleep_HMC5883L(void);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
199 void compass_read_HMC5883L(void);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
200
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
201 void accelerator_init_MMA8452Q(void);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
202 void accelerator_sleep_MMA8452Q(void);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
203 void acceleration_read_MMA8452Q(void);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
204
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
205 void compass_init_LSM303D(uint8_t fast, uint8_t gain);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
206 void compass_sleep_LSM303D(void);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
207 void compass_read_LSM303D(void);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
208 void acceleration_read_LSM303D(void);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
209
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
210 void compass_init_LSM303AGR(uint8_t fast, uint8_t gain);
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
211 void compass_sleep_LSM303AGR(void);
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
212 void compass_read_LSM303AGR(void);
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
213 void acceleration_read_LSM303AGR(void);
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
214
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
215 int LSM303D_accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
216 int compass_calib_common(void);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
217
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
218 void compass_calc_roll_pitch_only(void);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
219
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
220 void rotate_mag_3f(float *x, float *y, float *z);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
221 void rotate_accel_3f(float *x, float *y, float *z);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
222
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
223
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
224 /* Exported functions --------------------------------------------------------*/
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
225
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
226
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
227 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
228 // compass_init
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
229 /// @brief This might be called several times with different gain values during calibration
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
230 /// On first call it figures out which hardware is integrated
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
231 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
232 /// @param gain: 7 is max gain, compass_calib() might reduce it
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
233 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
234
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
235 void compass_init(uint8_t fast, uint8_t gain)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
236 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
237
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
238 // don't call again with fast, gain in calib mode etc.
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
239 // if unknown
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
240 if(hardwareCompass == COMPASS_NOT_RECOGNIZED)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
241 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
242 return;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
243 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
244
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
245 // old code but without else
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
246 if(hardwareCompass == 0)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
247 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
248 uint8_t data = WHO_AM_I;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
249 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
250 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1);
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
251 if(data == WHOIAM_VALUE_LSM303D)
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
252 hardwareCompass = compass_generation2; //LSM303D;
358
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
253 data = WHO_AM_I;
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
254 I2C_Master_Transmit( DEVICE_ACCELARATOR_303AGR, &data, 1);
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
255 I2C_Master_Receive( DEVICE_ACCELARATOR_303AGR, &data, 1);
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
256 if(data == WHOIAM_VALUE_LSM303AGR)
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
257 hardwareCompass = compass_generation3; //LSM303AGR;
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
258 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
259
180
9ecc2e60418d Removed not used compass code
ideenmodellierer
parents: 147
diff changeset
260 /* No compass identified => Retry */
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
261 if(hardwareCompass == 0)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
262 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
263 uint8_t data = WHO_AM_I;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
264 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
265 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1);
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
266 if(data == WHOIAM_VALUE_LSM303D)
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
267 hardwareCompass = compass_generation2; //LSM303D;
358
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
268 data = WHO_AM_I;
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
269 I2C_Master_Transmit( DEVICE_ACCELARATOR_303AGR, &data, 1);
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
270 I2C_Master_Receive( DEVICE_ACCELARATOR_303AGR, &data, 1);
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
271 if(data == WHOIAM_VALUE_LSM303AGR)
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
272 hardwareCompass = compass_generation3; //LSM303AGR;
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
273 }
70
6a6116d7b5bb old (and new) compass support
heinrichsweikamp
parents: 38
diff changeset
274
180
9ecc2e60418d Removed not used compass code
ideenmodellierer
parents: 147
diff changeset
275 /* Assume that a HMC5883L is equipped by default if detection still failed */
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
276 if(hardwareCompass == 0)
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
277 hardwareCompass = compass_generation1; //HMC5883L;
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
278
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
279 HAL_StatusTypeDef resultOfOperationHMC_MMA = HAL_TIMEOUT;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
280
358
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
281 // test if both chips of the two-chip solution (gen 1) are present
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
282 if(hardwareCompass == compass_generation1) // HMC5883L)
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
283 {
358
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
284 HAL_Delay(10);
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
285 MX_I2C1_Init();
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
286 uint8_t data = 0x2A; // CTRL_REG1 of DEVICE_ACCELARATOR_MMA8452Q
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
287 resultOfOperationHMC_MMA = I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
288 if(resultOfOperationHMC_MMA == HAL_OK)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
289 {
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
290 hardwareCompass = compass_generation1; //HMC5883L; // all fine, keep it
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
291 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
292 else
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
293 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
294 hardwareCompass = COMPASS_NOT_RECOGNIZED;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
295 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
296 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
297
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
298 if(hardwareCompass == compass_generation2) //LSM303D)
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
299 compass_init_LSM303D(fast, gain);
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
300 if(hardwareCompass == compass_generation3) //LSM303AGR)
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
301 compass_init_LSM303AGR(fast, gain);
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
302 if(hardwareCompass == compass_generation1) //HMC5883L)
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
303 compass_init_HMC5883L(fast, gain);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
304
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
305 tfull32 dataBlock[4];
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
306 if(BFA_readLastDataBlock((uint32_t *)dataBlock) == BFA_OK)
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
307 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
308 compass_CX_f = dataBlock[0].Word16.low16;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
309 compass_CY_f = dataBlock[0].Word16.hi16;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
310 compass_CZ_f = dataBlock[1].Word16.low16;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
311 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
312
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
313 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
314
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
315
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
316 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
317 // compass_calib
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
318 /// @brief with onchip_lowpass_filter configuration for accelerometer of LSM303D
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
319 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
320 int compass_calib(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
321 {
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
322 if(hardwareCompass == compass_generation2) //LSM303D)
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
323 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
324 LSM303D_accel_set_onchip_lowpass_filter_bandwidth(773);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
325 int out = compass_calib_common();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
326 LSM303D_accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
327 return out;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
328 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
329 else
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
330 if(hardwareCompass == compass_generation1) //HMC5883L)
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
331 {
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
332 return compass_calib_common();
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
333 }
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
334 else
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
335 if(hardwareCompass == compass_generation3) //LSM303AGR)
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
336 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
337 return compass_calib_common();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
338 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
339 else
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
340 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
341 return 0; // standard answer of compass_calib_common();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
342 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
343
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
344
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
345 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
346
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
347
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
348 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
349 // compass_sleep
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
350 /// @brief low power mode
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
351 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
352 void compass_sleep(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
353 {
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
354 if(hardwareCompass == compass_generation2) //LSM303D)
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
355 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
356 compass_sleep_LSM303D();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
357 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
358 else
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
359 if(hardwareCompass == compass_generation1) //HMC5883L)
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
360 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
361 compass_sleep_HMC5883L();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
362 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
363 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
364
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
365
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
366 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
367 // compass_read
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
368 /// @brief reads magnetometer and accelerometer for LSM303D,
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
369 /// otherwise magnetometer only
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
370 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
371 void compass_read(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
372 {
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
373 if(hardwareCompass == compass_generation2) //LSM303D)
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
374 compass_read_LSM303D();
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
375 if(hardwareCompass == compass_generation1) //HMC5883L)
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
376 compass_read_HMC5883L();
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
377 if(hardwareCompass == compass_generation3) //LSM303AGR)
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
378 compass_read_LSM303AGR();
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
379
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
380 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
381
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
382
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
383 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
384 // accelerator_init
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
385 /// @brief empty for for LSM303D
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
386 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
387 void accelerator_init(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
388 {
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
389 if(hardwareCompass == compass_generation1) //HMC5883L)
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
390 accelerator_init_MMA8452Q();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
391 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
392
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
393
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
394 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
395 // accelerator_sleep
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
396 /// @brief empty for for LSM303D
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
397 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
398 void accelerator_sleep(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
399 {
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
400 if(hardwareCompass == compass_generation1) //HMC5883L)
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
401 accelerator_sleep_MMA8452Q();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
402 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
403
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
404
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
405 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
406 // acceleration_read
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
407 /// @brief empty for for LSM303D
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
408 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
409 void acceleration_read(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
410 {
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
411 if(hardwareCompass == compass_generation2) //LSM303D)
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
412 acceleration_read_LSM303D();
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
413 if(hardwareCompass == compass_generation1) //HMC5883L)
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
414 acceleration_read_MMA8452Q();
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
415 if(hardwareCompass == compass_generation3) //LSM303AGR)
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
416 acceleration_read_LSM303AGR();
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
417 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
418
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
419
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
420 /* Private functions ---------------------------------------------------------*/
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
421
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
422 // ===============================================================================
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
423 // LSM303AGR_read_reg
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
424 // ===============================================================================
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
425 uint8_t LSM303AGR_read_reg(uint8_t addr)
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
426 {
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
427 uint8_t data;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
428
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
429 I2C_Master_Transmit( DEVICE_COMPASS_303AGR, &addr, 1);
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
430 I2C_Master_Receive( DEVICE_COMPASS_303AGR, &data, 1);
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
431 return data;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
432 }
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
433
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
434
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
435 // ===============================================================================
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
436 // LSM303AGR_write_reg
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
437 // ===============================================================================
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
438 void LSM303AGR_write_reg(uint8_t addr, uint8_t value)
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
439 {
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
440 uint8_t data[2];
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
441
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
442 data[0] = addr;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
443 data[1] = value;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
444 I2C_Master_Transmit( DEVICE_COMPASS_303AGR, data, 2);
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
445 }
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
446
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
447 // ===============================================================================
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
448 // LSM303AGR_acc_write_reg
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
449 // ===============================================================================
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
450 void LSM303AGR_acc_write_reg(uint8_t addr, uint8_t value)
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
451 {
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
452 uint8_t data[2];
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
453
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
454 data[0] = addr;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
455 data[1] = value;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
456 I2C_Master_Transmit( DEVICE_ACCELARATOR_303AGR, data, 2);
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
457 }
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
458
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
459
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
460 // ===============================================================================
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
461 // LSM303AGR_write_checked_reg
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
462 // ===============================================================================
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
463 void LSM303AGR_write_checked_reg(uint8_t addr, uint8_t value)
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
464 {
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
465 LSM303AGR_write_reg(addr, value);
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
466 }
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
467
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
468 // ===============================================================================
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
469 // LSM303AGR_acc_write_checked_reg
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
470 // ===============================================================================
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
471 void LSM303AGR_acc_write_checked_reg(uint8_t addr, uint8_t value)
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
472 {
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
473 LSM303AGR_acc_write_reg(addr, value);
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
474 }
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
475
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
476 // ===============================================================================
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
477 // LSM303D_read_reg
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
478 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
479 uint8_t LSM303D_read_reg(uint8_t addr)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
480 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
481 uint8_t data;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
482
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
483 I2C_Master_Transmit( DEVICE_COMPASS_303D, &addr, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
484 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
485 return data;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
486 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
487
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
488
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
489 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
490 // LSM303D_write_reg
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
491 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
492 void LSM303D_write_reg(uint8_t addr, uint8_t value)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
493 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
494 uint8_t data[2];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
495
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
496 /* enable accel*/
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
497 data[0] = addr;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
498 data[1] = value;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
499 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
500 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
501
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
502
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
503 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
504 // LSM303D_write_checked_reg
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
505 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
506 void LSM303D_write_checked_reg(uint8_t addr, uint8_t value)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
507 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
508 LSM303D_write_reg(addr, value);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
509 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
510
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
511
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
512 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
513 // LSM303D_modify_reg
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
514 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
515 void LSM303D_modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
516 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
517 uint8_t val;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
518
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
519 val = LSM303D_read_reg(reg);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
520 val &= ~clearbits;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
521 val |= setbits;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
522 LSM303D_write_checked_reg(reg, val);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
523 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
524
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
525 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
526 // LSM303D_accel_set_onchip_lowpass_filter_bandwidth
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
527 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
528 int LSM303D_accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
529 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
530 uint8_t setbits = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
531 uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
532
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
533 if (bandwidth == 0) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
534 bandwidth = 773;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
535 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
536
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
537 if (bandwidth <= 50) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
538 setbits |= REG2_AA_FILTER_BW_50HZ_A;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
539 _accel_onchip_filter_bandwith = 50;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
540
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
541 } else if (bandwidth <= 194) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
542 setbits |= REG2_AA_FILTER_BW_194HZ_A;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
543 _accel_onchip_filter_bandwith = 194;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
544
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
545 } else if (bandwidth <= 362) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
546 setbits |= REG2_AA_FILTER_BW_362HZ_A;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
547 _accel_onchip_filter_bandwith = 362;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
548
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
549 } else if (bandwidth <= 773) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
550 setbits |= REG2_AA_FILTER_BW_773HZ_A;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
551 _accel_onchip_filter_bandwith = 773;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
552
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
553 } else {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
554 return -1;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
555 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
556
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
557 LSM303D_modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
558
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
559 return 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
560 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
561
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
562
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
563 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
564 // LSM303D_accel_set_driver_lowpass_filter
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
565 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
566 int LSM303D_accel_set_driver_lowpass_filter(float samplerate, float bandwidth)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
567 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
568 /*
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
569 _accel_filter_x_set_cutoff_frequency(samplerate, bandwidth);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
570 _accel_filter_y_set_cutoff_frequency(samplerate, bandwidth);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
571 _accel_filter_z_set_cutoff_frequency(samplerate, bandwidth);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
572 */
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
573 return 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
574 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
575
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
576
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
577 // rotate_mag_3f: nicht genutzt aber praktisch; rotate_accel_3f wird benutzt
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
578 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
579 // rotate_mag_3f
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
580 /// @brief swap axis in convient way, by hw
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
581 /// @param *x raw input is set to *y input
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
582 /// @param *y raw input is set to -*x input
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
583 /// @param *z raw is not touched
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
584 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
585 void rotate_mag_3f(float *x, float *y, float *z)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
586 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
587 return;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
588 /*
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
589 *x = *x; // HMC: *x = -*y
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
590 *y = *y; // HMC: *y = *x // change 20.04.2016: zuvor *y = -*y
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
591 *z = *z; // HMC: *z = *z
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
592 */
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
593 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
594
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
595
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
596 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
597 // rotate_accel_3f
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
598 /// @brief swap axis in convient way, by hw, same as MMA8452Q
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
599 /// @param *x raw input, output is with sign change
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
600 /// @param *y raw input, output is with sign change
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
601 /// @param *z raw input, output is with sign change
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
602 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
603 void rotate_accel_3f(float *x, float *y, float *z)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
604 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
605 *x = -*x;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
606 *y = -*y;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
607 *z = -*z;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
608 /* tested:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
609 x = x, y =-y, z=-z: does not work with roll
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
610 x = x, y =y, z=-z: does not work with pitch
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
611 x = x, y =y, z=z: does not work at all
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
612 */
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
613 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
614
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
615
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
616 // ===============================================================================
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
617 // compass_init_LSM303D
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
618 /// This might be called several times with different gain values during calibration
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
619 /// but gain change is not supported at the moment.
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
620 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
621 /// @param gain: 7 is max gain and set with here, compass_calib() might reduce it
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
622 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
623
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
624 //uint8_t testCompassLS303D[11];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
625
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
626 void compass_init_LSM303D(uint8_t fast, uint8_t gain)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
627 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
628 if(fast == 0)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
629 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
630 LSM303D_write_checked_reg(ADDR_CTRL_REG0, 0x00);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
631 LSM303D_write_checked_reg(ADDR_CTRL_REG1, 0x3F); // mod 12,5 Hz 3 instead of 6,25 Hz 2
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
632 LSM303D_write_checked_reg(ADDR_CTRL_REG2, 0xC0);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
633 LSM303D_write_checked_reg(ADDR_CTRL_REG3, 0x00);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
634 LSM303D_write_checked_reg(ADDR_CTRL_REG4, 0x00);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
635 LSM303D_write_checked_reg(ADDR_CTRL_REG5, 0x68); // mod 12,5 Hz 8 instead of 6,25 Hz 4
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
636 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
637 else
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
638 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
639 LSM303D_write_checked_reg(ADDR_CTRL_REG0, 0x00);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
640 LSM303D_write_checked_reg(ADDR_CTRL_REG1, 0x6F); // 100 Hz
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
641 LSM303D_write_checked_reg(ADDR_CTRL_REG2, 0xC0);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
642 LSM303D_write_checked_reg(ADDR_CTRL_REG3, 0x00);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
643 LSM303D_write_checked_reg(ADDR_CTRL_REG4, 0x00);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
644 LSM303D_write_checked_reg(ADDR_CTRL_REG5, 0x74); // 100 Hz
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
645 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
646 LSM303D_write_checked_reg(ADDR_CTRL_REG6, 0x00);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
647 LSM303D_write_checked_reg(ADDR_CTRL_REG7, 0x00);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
648
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
649 return;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
650 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
651
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
652
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
653 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
654 // compass_sleep_LSM303D
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
655 // @brief Gen 2 chip
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
656 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
657 void compass_sleep_LSM303D(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
658 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
659 LSM303D_write_checked_reg(ADDR_CTRL_REG1, 0x00); // CNTRL1: acceleration sensor Power-down mode
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
660 LSM303D_write_checked_reg(ADDR_CTRL_REG7, 0x02); // CNTRL7: magnetic sensor Power-down mode
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
661 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
662
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
663
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
664 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
665 // acceleration_read_LSM303D
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
666 // output is accel_DX_f, accel_DY_f, accel_DZ_f
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
667 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
668 void acceleration_read_LSM303D(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
669 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
670 uint8_t data;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
671 float xraw_f, yraw_f, zraw_f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
672 float accel_report_x, accel_report_y, accel_report_z;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
673
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
674 memset(accDataBuffer,0,6);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
675
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
676 accel_DX_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
677 accel_DY_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
678 accel_DZ_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
679
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
680 for(int i=0;i<6;i++)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
681 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
682 data = ADDR_OUT_X_L_A + i;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
683 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
684 I2C_Master_Receive( DEVICE_COMPASS_303D, &accDataBuffer[i], 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
685 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
686
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
687 xraw_f = ((float)( (int16_t)((accDataBuffer[1] << 8) | (accDataBuffer[0]))));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
688 yraw_f = ((float)( (int16_t)((accDataBuffer[3] << 8) | (accDataBuffer[2]))));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
689 zraw_f = ((float)( (int16_t)((accDataBuffer[5] << 8) | (accDataBuffer[4]))));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
690
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
691 rotate_accel_3f(&xraw_f, &yraw_f, &zraw_f);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
692
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
693 // mh
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
694 accel_report_x = xraw_f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
695 accel_report_y = yraw_f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
696 accel_report_z = zraw_f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
697
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
698 accel_DX_f = ((int16_t)(accel_report_x));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
699 accel_DY_f = ((int16_t)(accel_report_y));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
700 accel_DZ_f = ((int16_t)(accel_report_z));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
701 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
702
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
703
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
704 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
705 // compass_read_LSM303D
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
706 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
707 /// output is compass_DX_f, compass_DY_f, compass_DZ_f
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
708 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
709 void compass_read_LSM303D(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
710 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
711 uint8_t data;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
712 // float xraw_f, yraw_f, zraw_f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
713 // float mag_report_x, mag_report_y, mag_report_z;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
714
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
715 memset(magDataBuffer,0,6);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
716
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
717 compass_DX_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
718 compass_DY_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
719 compass_DZ_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
720
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
721 for(int i=0;i<6;i++)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
722 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
723 data = ADDR_OUT_X_L_M + i;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
724 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
725 I2C_Master_Receive( DEVICE_COMPASS_303D, &magDataBuffer[i], 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
726 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
727
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
728 // mh 160620 flip x and y if flip display
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
729 compass_DX_f = (((int16_t)((magDataBuffer[1] << 8) | (magDataBuffer[0]))));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
730 compass_DY_f = (((int16_t)((magDataBuffer[3] << 8) | (magDataBuffer[2]))));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
731 compass_DZ_f = (((int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4]))));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
732 // no rotation
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
733 return;
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
734 }
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
735
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
736
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
737 // ===============================================================================
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
738 // compass_init_LSM303AGR
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
739 /// This might be called several times with different gain values during calibration
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
740 /// but gain change is not supported at the moment.
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
741 ///
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
742 /// @param gain: 7 is max gain and set with here, compass_calib() might reduce it
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
743 // ===============================================================================
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
744
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
745 void compass_init_LSM303AGR(uint8_t fast, uint8_t gain)
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
746 {
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
747 if(fast == 0)
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
748 {
358
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
749 // init compass
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
750 LSM303AGR_write_checked_reg(0x60, 0x80); // 10Hz
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
751 LSM303AGR_write_checked_reg(0x61, 0x03); // CFG_REG_B_M
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
752 LSM303AGR_write_checked_reg(0x62, 0x10); // CFG_REG_C_M
358
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
753 LSM303AGR_write_checked_reg(0x63, 0x00); // INT_CTRL_REG_M
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
754
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
755 // init accel (Same chip, but different address...)
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
756 LSM303AGR_acc_write_checked_reg(0x1F, 0x00); // TEMP_CFG_REG_A (Temp sensor off)
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
757 LSM303AGR_acc_write_checked_reg(0x20, 0x4F); // CTRL_REG1_A (50Hz, x,y,z = ON)
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
758 LSM303AGR_acc_write_checked_reg(0x21, 0x00); // CTRL_REG2_A
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
759 LSM303AGR_acc_write_checked_reg(0x22, 0x00); // CTRL_REG3_A
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
760 LSM303AGR_acc_write_checked_reg(0x23, 0x08); // CTRL_REG4_A, High Resolution Mode enabled
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
761 }
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
762 else
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
763 {
358
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
764 // init compass
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
765 LSM303AGR_write_checked_reg(0x60, 0x84); // 20Hz
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
766 LSM303AGR_write_checked_reg(0x61, 0x03); // CFG_REG_B_M
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
767 LSM303AGR_write_checked_reg(0x62, 0x10); // CFG_REG_C_M
358
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
768 LSM303AGR_write_checked_reg(0x63, 0x00); // INT_CTRL_REG_M
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
769
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
770 // init accel (Same chip, but different address...)
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
771 LSM303AGR_acc_write_checked_reg(0x1F, 0x00); // TEMP_CFG_REG_A (Temp sensor off)
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
772 LSM303AGR_acc_write_checked_reg(0x20, 0x4F); // CTRL_REG1_A (50Hz, x,y,z = ON)
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
773 LSM303AGR_acc_write_checked_reg(0x21, 0x00); // CTRL_REG2_A
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
774 LSM303AGR_acc_write_checked_reg(0x22, 0x00); // CTRL_REG3_A
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
775 LSM303AGR_acc_write_checked_reg(0x23, 0x0); // CTRL_REG4_A, High Resolution Mode enabled
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
776 }
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
777
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
778 return;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
779 }
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
780
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
781
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
782 // ===============================================================================
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
783 // compass_sleep_LSM303D
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
784 // @brief Gen 2 chip
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
785 // ===============================================================================
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
786 void compass_sleep_LSM303AGR(void)
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
787 {
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
788 LSM303AGR_write_checked_reg(0x60, 0x03); //
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
789 LSM303AGR_write_checked_reg(0x61, 0x04); //
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
790 LSM303AGR_write_checked_reg(0x62, 0x51); //
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
791 LSM303AGR_write_checked_reg(0x63, 0x00); //
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
792
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
793
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
794 LSM303AGR_acc_write_checked_reg(0x1F, 0x00); //
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
795 LSM303AGR_acc_write_checked_reg(0x20, 0x00); //
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
796 }
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
797
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
798
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
799 // ===============================================================================
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
800 // acceleration_read_LSM303AGR
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
801 // output is accel_DX_f, accel_DY_f, accel_DZ_f
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
802 // ===============================================================================
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
803 void acceleration_read_LSM303AGR(void)
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
804 {
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
805 uint8_t data;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
806 float xraw_f, yraw_f, zraw_f;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
807 float accel_report_x, accel_report_y, accel_report_z;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
808
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
809 memset(accDataBuffer,0,6);
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
810
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
811 accel_DX_f = 0;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
812 accel_DY_f = 0;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
813 accel_DZ_f = 0;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
814
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
815 for(int i=0;i<6;i++)
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
816 {
358
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
817 data = 0x28 + i; // OUT_X_L_A
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
818 I2C_Master_Transmit( DEVICE_ACCELARATOR_303AGR, &data, 1);
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
819 I2C_Master_Receive( DEVICE_ACCELARATOR_303AGR, &accDataBuffer[i], 1);
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
820 }
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
821
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
822 xraw_f = ((float)( (int16_t)((accDataBuffer[1] << 8) | (accDataBuffer[0]))));
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
823 yraw_f = ((float)( (int16_t)((accDataBuffer[3] << 8) | (accDataBuffer[2]))));
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
824 zraw_f = ((float)( (int16_t)((accDataBuffer[5] << 8) | (accDataBuffer[4]))));
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
825
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
826 rotate_accel_3f(&xraw_f, &yraw_f, &zraw_f);
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
827
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
828 // mh
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
829 accel_report_x = xraw_f;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
830 accel_report_y = yraw_f;
358
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
831 accel_report_z = -zraw_f; // flip Z in gen 2 hardware
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
832
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
833 accel_DX_f = ((int16_t)(accel_report_x));
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
834 accel_DY_f = ((int16_t)(accel_report_y));
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
835 accel_DZ_f = ((int16_t)(accel_report_z));
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
836 }
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
837
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
838
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
839 // ===============================================================================
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
840 // compass_read_LSM303AGR
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
841 ///
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
842 /// output is compass_DX_f, compass_DY_f, compass_DZ_f
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
843 // ===============================================================================
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
844 void compass_read_LSM303AGR(void)
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
845 {
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
846 uint8_t data;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
847 // float xraw_f, yraw_f, zraw_f;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
848 // float mag_report_x, mag_report_y, mag_report_z;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
849
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
850 memset(magDataBuffer,0,6);
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
851
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
852 compass_DX_f = 0;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
853 compass_DY_f = 0;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
854 compass_DZ_f = 0;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
855
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
856 for(int i=0;i<6;i++)
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
857 {
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
858 data = 0x68 + i; // OUTX_L_REG_M
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
859 I2C_Master_Transmit( DEVICE_COMPASS_303AGR, &data, 1);
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
860 I2C_Master_Receive( DEVICE_COMPASS_303AGR, &magDataBuffer[i], 1);
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
861 }
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
862
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
863 // mh 160620 flip x and y if flip display
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
864 compass_DX_f = (((int16_t)((magDataBuffer[1] << 8) | (magDataBuffer[0]))));
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
865 compass_DY_f = (((int16_t)((magDataBuffer[3] << 8) | (magDataBuffer[2]))));
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
866 compass_DZ_f = (((int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4]))));
358
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
867
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
868 // align axis in gen 2 hardware
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
869 compass_DZ_f *= -1;
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
870
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
871 return;
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
872 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
873
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
874
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
875 // --------------------------------------------------------------------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
876 // ----------EARLIER COMPONENTS ---------------------------------------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
877 // --------------------------------------------------------------------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
878
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
879 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
880 // compass_init_HMC5883L
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
881 /// @brief The horrible Honeywell compass chip
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
882 /// This might be called several times during calibration
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
883 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
884 /// @param fast: 1 is fast mode, 0 is normal mode
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
885 /// @param gain: 7 is max gain and set with here, compass_calib() might reduce it
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
886 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
887 void compass_init_HMC5883L(uint8_t fast, uint8_t gain)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
888 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
889 uint8_t write_buffer[4];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
890
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
891 compass_gain = gain;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
892 uint16_t length = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
893 write_buffer[0] = 0x00; // 00 = config Register A
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
894
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
895 if( fast )
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
896 write_buffer[1] = 0x38; // 0b 0011 1000; // ConfigA: 75Hz, 2 Samples averaged
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
897 else
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
898 write_buffer[1] = 0x68; // 0b 0110 1000; // ConfigA: 3Hz, 8 Samples averaged
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
899
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
900 switch(gain)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
901 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
902 case 7:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
903 write_buffer[2] = 0xE0; //0b 1110 0000; // ConfigB: gain
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
904 break;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
905 case 6:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
906 write_buffer[2] = 0xC0; //0b 1100 0000; // ConfigB: gain
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
907 break;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
908 case 5:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
909 write_buffer[2] = 0xA0; //0b 1010 0000; // ConfigB: gain
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
910 break;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
911 case 4:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
912 write_buffer[2] = 0x80; //0b 1000 0000; // ConfigB: gain
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
913 break;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
914 case 3:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
915 write_buffer[2] = 0x60; //0b 0110 0000; // ConfigB: gain
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
916 break;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
917 case 2:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
918 write_buffer[2] = 0x40; //0b 01000 0000; // ConfigB: gain
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
919 break;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
920 case 1:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
921 write_buffer[2] = 0x20; //0b 00100 0000; // ConfigB: gain
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
922 break;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
923 case 0:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
924 write_buffer[2] = 0x00; //0b 00000 0000; // ConfigB: gain
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
925 break;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
926 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
927 write_buffer[3] = 0x00; // Mode: continuous mode
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
928 length = 4;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
929 //hmc_twi_write(0);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
930 I2C_Master_Transmit( DEVICE_COMPASS_HMC5883L, write_buffer, length);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
931 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
932
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
933
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
934
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
935 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
936 // compass_sleep_HMC5883L
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
937 /// @brief Power-down mode for Honeywell compass chip
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
938 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
939 void compass_sleep_HMC5883L(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
940 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
941 uint8_t write_buffer[4];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
942 uint16_t length = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
943
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
944 write_buffer[0] = 0x00; // 00 = config Register A
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
945 write_buffer[1] = 0x68; // 0b 0110 1000; // ConfigA
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
946 write_buffer[2] = 0x20; // 0b 0010 0000; // ConfigB
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
947 write_buffer[3] = 0x02; // 0b 0000 0010; // Idle Mode
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
948 length = 4;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
949 I2C_Master_Transmit( DEVICE_COMPASS_HMC5883L, write_buffer, length);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
950 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
951
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
952
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
953 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
954 // accelerator_init_MMA8452Q
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
955 /// @brief Power-down mode for acceleration chip used in combination with Honeywell compass
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
956 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
957 void accelerator_init_MMA8452Q(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
958 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
959 uint8_t write_buffer[4];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
960 uint16_t length = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
961 //HAL_Delay(1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
962 //return;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
963 write_buffer[0] = 0x0E; // XYZ_DATA_CFG
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
964 write_buffer[1] = 0x00;//0b00000000; // High pass Filter=0 , +/- 2g range
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
965 length = 2;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
966 I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, write_buffer, length);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
967 //HAL_Delay(1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
968 write_buffer[0] = 0x2A; // CTRL_REG1
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
969 write_buffer[1] = 0x34; //0b00110100; // CTRL_REG1: 160ms data rate, St.By Mode, reduced noise mode
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
970 write_buffer[2] = 0x02; //0b00000010; // CTRL_REG2: High Res in Active mode
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
971 length = 3;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
972 I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, write_buffer, length);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
973
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
974 //HAL_Delay(1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
975 //hw_delay_us(100);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
976 write_buffer[0] = 0x2A; // CTRL_REG1
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
977 write_buffer[1] = 0x35; //0b00110101; // CTRL_REG1: ... Active Mode
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
978 length = 2;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
979 I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, write_buffer, length);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
980 /*
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
981 HAL_Delay(6);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
982 compass_read();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
983 HAL_Delay(1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
984 acceleration_read();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
985
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
986 compass_calc();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
987 */
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
988 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
989
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
990
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
991 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
992 // accelerator_sleep_MMA8452Q
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
993 /// @brief compass_sleep_HMC5883L
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
994 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
995 void accelerator_sleep_MMA8452Q(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
996 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
997 uint16_t length = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
998 uint8_t write_buffer[4];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
999
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1000 write_buffer[0] = 0x2A; // CTRL_REG1
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1001 write_buffer[1] = 0x00; //0b00000000; // CTRL_REG1: Standby Mode
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1002 length = 2;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1003 I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, write_buffer, length);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1004 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1005
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1006
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1007 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1008 // compass_read_HMC5883L
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1009 /// @brief The new ST 303D - get ALL data and store in static variables
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1010 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1011 /// output is compass_DX_f, compass_DY_f, compass_DZ_f
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1012 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1013 void compass_read_HMC5883L(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1014 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1015 uint8_t buffer[20];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1016 compass_DX_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1017 compass_DY_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1018 compass_DZ_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1019 uint8_t length = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1020 uint8_t read_buffer[6];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1021 signed_tword data;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1022 for(int i = 0; i<6;i++)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1023 read_buffer[i] = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1024 buffer[0] = 0x03; // 03 = Data Output X MSB Register
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1025 length = 1;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1026 I2C_Master_Transmit( DEVICE_COMPASS_HMC5883L, buffer, length);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1027 length = 6;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1028 I2C_Master_Receive( DEVICE_COMPASS_HMC5883L, read_buffer, length);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1029
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1030
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1031 data.Byte.hi = read_buffer[0];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1032 data.Byte.low = read_buffer[1];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1033 //Y = Z
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1034 compass_DY_f = - data.Word;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1035
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1036 data.Byte.hi = read_buffer[2];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1037 data.Byte.low = read_buffer[3];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1038 compass_DZ_f = data.Word;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1039
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1040 data.Byte.hi = read_buffer[4];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1041 data.Byte.low = read_buffer[5];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1042 //X = -Y
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1043 compass_DX_f = data.Word;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1044 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1045
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1046
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1047 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1048 // acceleration_read_MMA8452Q
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1049 /// @brief The old MMA8452Q used with the Honeywell compass
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1050 /// get the acceleration data and store in static variables
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1051 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1052 /// output is accel_DX_f, accel_DY_f, accel_DZ_f
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1053 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1054 void acceleration_read_MMA8452Q(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1055 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1056 uint8_t buffer[20];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1057 accel_DX_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1058 accel_DY_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1059 accel_DZ_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1060 uint8_t length = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1061 // bit8_Type status ;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1062 uint8_t read_buffer[7];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1063 signed_tword data;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1064 for(int i = 0; i<6;i++)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1065 read_buffer[i] = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1066 buffer[0] = 0x00; // 03 = Data Output X MSB Register
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1067 length = 1;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1068 I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, buffer, length);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1069 length = 7;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1070 I2C_Master_Receive( DEVICE_ACCELARATOR_MMA8452Q, read_buffer, length);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1071
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1072 // status.uw = read_buffer[0];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1073 data.Byte.hi = read_buffer[1];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1074 data.Byte.low = read_buffer[2];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1075 accel_DX_f =data.Word/16;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1076 data.Byte.hi = read_buffer[3];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1077 data.Byte.low = read_buffer[4];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1078 accel_DY_f =data.Word/16;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1079 data.Byte.hi = read_buffer[5];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1080 data.Byte.low = read_buffer[6];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1081 accel_DZ_f =data.Word/16;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1082
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1083 accel_DX_f *= -1;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1084 accel_DY_f *= -1;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1085 accel_DZ_f *= -1;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1086 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1087
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1088
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1089 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1090 // compass_calc_roll_pitch_only
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1091 /// @brief only the roll and pitch parts of compass_calc()
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1092 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1093 /// input is accel_DX_f, accel_DY_f, accel_DZ_f
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1094 /// output is compass_pitch and compass_roll
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1095 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1096 void compass_calc_roll_pitch_only(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1097 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1098 float sinPhi, cosPhi;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1099 float Phi, Teta;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1100
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1101 //---- Calculate sine and cosine of roll angle Phi -----------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1102 Phi= atan2f(accel_DY_f, accel_DZ_f) ;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1103 compass_roll = Phi * 180.0f /PI;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1104 sinPhi = sinf(Phi);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1105 cosPhi = cosf(Phi);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1106
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1107 //---- calculate sin and cosine of pitch angle Theta ---------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1108 Teta = atanf(-(float)accel_DX_f/(accel_DY_f * sinPhi + accel_DZ_f * cosPhi));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1109 compass_pitch = Teta * 180.0f /PI;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1110 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1111
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1112
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1113 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1114 // compass_calc
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1115 /// @brief all the fancy stuff first implemented in OSTC3
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1116 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1117 /// input is compass_DX_f, compass_DY_f, compass_DZ_f, accel_DX_f, accel_DY_f, accel_DZ_f
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1118 /// and compass_CX_f, compass_CY_f, compass_CZ_f
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1119 /// output is compass_heading, compass_pitch and compass_roll
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1120 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1121 void compass_calc(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1122 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1123 float sinPhi, cosPhi, sinTeta, cosTeta;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1124 float Phi, Teta, Psi;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1125 int16_t iBfx, iBfy;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1126 int16_t iBpx, iBpy, iBpz;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1127
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1128 //---- Make hard iron correction -----------------------------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1129 // Measured magnetometer orientation, measured ok.
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1130 // From matthias drawing: (X,Y,Z) --> (X,Y,Z) : no rotation.
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1131 iBpx = compass_DX_f - compass_CX_f; // X
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1132 iBpy = compass_DY_f - compass_CY_f; // Y
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1133 iBpz = compass_DZ_f - compass_CZ_f; // Z
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1134
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1135 //---- Calculate sine and cosine of roll angle Phi -----------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1136 //sincos(accel_DZ_f, accel_DY_f, &sin, &cos);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1137 Phi= atan2f(accel_DY_f, accel_DZ_f) ;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1138 compass_roll = Phi * 180.0f /PI;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1139 sinPhi = sinf(Phi);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1140 cosPhi = cosf(Phi);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1141
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1142 //---- rotate by roll angle (-Phi) ---------------------------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1143 iBfy = iBpy * cosPhi - iBpz * sinPhi;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1144 iBpz = iBpy * sinPhi + iBpz * cosPhi;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1145 //Gz = imul(accel_DY_f, sin) + imul(accel_DZ_f, cos);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1146
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1147 //---- calculate sin and cosine of pitch angle Theta ---------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1148 //sincos(Gz, -accel_DX_f, &sin, &cos); // NOTE: changed sin sign.
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1149 // Teta takes into account roll of computer and sends combination of Y and Z :-) understand now hw 160421
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1150 Teta = atanf(-(float)accel_DX_f/(accel_DY_f * sinPhi + accel_DZ_f * cosPhi));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1151 compass_pitch = Teta * 180.0f /PI;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1152 sinTeta = sinf(Teta);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1153 cosTeta = cosf(Teta);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1154 /* correct cosine if pitch not in range -90 to 90 degrees */
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1155 if( cosTeta < 0 ) cosTeta = -cosTeta;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1156
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1157 ///---- de-rotate by pitch angle Theta -----------------------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1158 iBfx = iBpx * cosTeta + iBpz * sinTeta;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1159
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1160 //---- Detect uncalibrated compass ---------------------------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1161 if( !compass_CX_f && !compass_CY_f && !compass_CZ_f )
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1162 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1163 compass_heading = -1;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1164 return;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1165 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1166
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1167 //---- calculate current yaw = e-compass angle Psi -----------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1168 // Result in degree (no need of 0.01 deg precision...
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1169 Psi = atan2f(-iBfy,iBfx);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1170 compass_heading = Psi * 180.0f /PI;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1171 // Result in 0..360 range:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1172 if( compass_heading < 0 )
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1173 compass_heading += 360;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1174 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1175
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1176
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1177 // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1178 // // - Calibration - ///////////////////////////////////////////////////////////////////////////////////////////////////////
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1179 // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1180
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1181 /* can be lost during sleep as those are reset with compass_reset_calibration() */
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1182
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1183 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1184 // compass_reset_calibration
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1185 /// @brief all the fancy stuff first implemented in OSTC3
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1186 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1187 /// output is struct g and compass_CX_f, compass_CY_f, compass_CZ_f
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1188 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1189 /// @param g: is a struct with crazy stuff like Suuu, Svvv, Svvu, etc.
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1190 /// all is set to zero here
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1191 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1192 void compass_reset_calibration(SCompassCalib *g)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1193 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1194 g->compass_N = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1195 g->Su = g->Sv = g->Sw = 0.0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1196 g->Suu = g->Svv = g->Sww = g->Suv = g->Suw = g->Svw = 0.0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1197 g->Suuu = g->Svvv = g->Swww = 0.0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1198 g->Suuv = g->Suuw = g->Svvu = g->Svvw = g->Swwu = g->Swwv = 0.0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1199 compass_CX_f = compass_CY_f = compass_CZ_f = 0.0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1200 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1201
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1202
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1203 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1204 // compass_add_calibration
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1205 /// @brief all the fancy stuff first implemented in OSTC3
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1206 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1207 /// input is compass_DX_f, compass_DY_f, compass_DZ_f
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1208 /// and compass_CX_f, compass_CY_f, compass_CZ_f
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1209 /// output is struct g
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1210 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1211 /// @param g: is a struct with crazy stuff like Suuu, Svvv, Svvu, etc.
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1212 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1213 void compass_add_calibration(SCompassCalib *g)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1214 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1215 float u, v, w;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1216
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1217 u = (compass_DX_f - compass_CX_f) / 32768.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1218 v = (compass_DY_f - compass_CY_f) / 32768.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1219 w = (compass_DZ_f - compass_CZ_f) / 32768.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1220
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1221 g->compass_N++;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1222 g->Su += u;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1223 g->Sv += v;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1224 g->Sw += w;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1225 g->Suv += u*v;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1226 g->Suw += u*w;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1227 g->Svw += v*w;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1228 g->Suu += u*u;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1229 g->Suuu += u*u*u;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1230 g->Suuv += v*u*u;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1231 g->Suuw += w*u*u;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1232 g->Svv += v*v;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1233 g->Svvv += v*v*v;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1234 g->Svvu += u*v*v;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1235 g->Svvw += w*v*v;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1236 g->Sww += w*w;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1237 g->Swww += w*w*w;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1238 g->Swwu += u*w*w;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1239 g->Swwv += v*w*w;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1240 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1241
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1242 //////////////////////////////////////////////////////////////////////////////
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1243
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1244 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1245 // compass_solve_calibration
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1246 /// @brief all the fancy stuff first implemented in OSTC3
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1247 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1248 /// input is compass_CX_f, compass_CY_f, compass_CZ_f and g
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1249 /// output is struct g
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1250 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1251 /// @param g: is a struct with crazy stuff like Suuu, Svvv, Svvu, etc.
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1252 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1253 void compass_solve_calibration(SCompassCalib *g)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1254 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1255 float yu, yv, yw;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1256 float delta;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1257 float uc, vc, wc;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1258
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1259
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1260 //---- Normalize partial sums --------------------------------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1261 //
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1262 // u, v, w should be centered on the mean value um, vm, wm:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1263 // x = u + um, with um = Sx/N
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1264 //
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1265 // So:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1266 // (u + um)**2 = u**2 + 2u*um + um**2
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1267 // Su = 0, um = Sx/N
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1268 // Sxx = Suu + 2 um Su + N*(Sx/N)**2 = Suu + Sx**2/N
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1269 // Suu = Sxx - Sx**2/N
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1270 yu = g->Su/g->compass_N;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1271 yv = g->Sv/g->compass_N;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1272 yw = g->Sw/g->compass_N;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1273
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1274 g->Suu -= g->Su*yu;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1275 g->Svv -= g->Sv*yv;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1276 g->Sww -= g->Sw*yw;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1277
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1278 // (u + um)(v + vm) = uv + u vm + v um + um vm
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1279 // Sxy = Suv + N * um vm
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1280 // Suv = Sxy - N * (Sx/N)(Sy/N);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1281 g->Suv -= g->Su*yv;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1282 g->Suw -= g->Su*yw;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1283 g->Svw -= g->Sv*yw;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1284
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1285 // (u + um)**3 = u**3 + 3 u**2 um + 3 u um**2 + um**3
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1286 // Sxxx = Suuu + 3 um Suu + 3 um**2 Su + N.um**3
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1287 // Su = 0, um = Sx/N:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1288 // Suuu = Sxxx - 3 Sx*Suu/N - N.(Sx/N)**3
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1289 // = Sxxx - 3 Sx*Suu/N - Sx**3/N**2
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1290
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1291 // (u + um)**2 (v + vm) = (u**2 + 2 u um + um**2)(v + vm)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1292 // Sxxy = Suuv + vm Suu + 2 um (Suv + vm Su) + um**2 (Sv + N.vm)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1293 //
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1294 // Su = 0, Sv = 0, vm = Sy/N:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1295 // Sxxy = Suuv + vm Suu + 2 um Suv + N um**2 vm
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1296 //
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1297 // Suuv = Sxxy - (Sy/N) Suu - 2 (Sx/N) Suv - (Sx/N)**2 Sy
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1298 // = Sxxy - Suu*Sy/N - 2 Suv*Sx/N - Sx*Sx*Sy/N/N
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1299 // = Sxxy - (Suu + Sx*Sx/N)*Sy/N - 2 Suv*Sx/N
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1300 g->Suuu -= (3*g->Suu + g->Su*yu)*yu;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1301 g->Suuv -= (g->Suu + g->Su*yu)*yv + 2*g->Suv*yu;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1302 g->Suuw -= (g->Suu + g->Su*yu)*yw + 2*g->Suw*yu;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1303
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1304 g->Svvu -= (g->Svv + g->Sv*yv)*yu + 2*g->Suv*yv;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1305 g->Svvv -= (3*g->Svv + g->Sv*yv)*yv;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1306 g->Svvw -= (g->Svv + g->Sv*yv)*yw + 2*g->Svw*yv;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1307
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1308 g->Swwu -= (g->Sww + g->Sw*yw)*yu + 2*g->Suw*yw;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1309 g->Swwv -= (g->Sww + g->Sw*yw)*yv + 2*g->Svw*yw;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1310 g->Swww -= (3*g->Sww + g->Sw*yw)*yw;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1311
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1312 //---- Solve the system --------------------------------------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1313 // uc Suu + vc Suv + wc Suw = (Suuu + Svvu + Swwu) / 2
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1314 // uc Suv + vc Svv + wc Svw = (Suuv + Svvv + Swwv) / 2
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1315 // uc Suw + vc Svw + wc Sww = (Suuw + Svvw + Swww) / 2
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1316 // Note this is symetric, with a positiv diagonal, hence
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1317 // it always have a uniq solution.
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1318 yu = 0.5f * (g->Suuu + g->Svvu + g->Swwu);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1319 yv = 0.5f * (g->Suuv + g->Svvv + g->Swwv);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1320 yw = 0.5f * (g->Suuw + g->Svvw + g->Swww);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1321 delta = g->Suu * (g->Svv * g->Sww - g->Svw * g->Svw)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1322 - g->Suv * (g->Suv * g->Sww - g->Svw * g->Suw)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1323 + g->Suw * (g->Suv * g->Svw - g->Svv * g->Suw);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1324
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1325 uc = (yu * (g->Svv * g->Sww - g->Svw * g->Svw)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1326 - yv * (g->Suv * g->Sww - g->Svw * g->Suw)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1327 + yw * (g->Suv * g->Svw - g->Svv * g->Suw) )/delta;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1328 vc = (g->Suu * ( yv * g->Sww - yw * g->Svw)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1329 - g->Suv * ( yu * g->Sww - yw * g->Suw)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1330 + g->Suw * ( yu * g->Svw - yv * g->Suw) )/delta;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1331 wc = (g->Suu * (g->Svv * yw - g->Svw * yv )
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1332 - g->Suv * (g->Suv * yw - g->Svw * yu )
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1333 + g->Suw * (g->Suv * yv - g->Svv * yu ) )/delta;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1334
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1335 // Back to uncentered coordinates:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1336 // xc = um + uc
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1337 uc = g->Su/g->compass_N + compass_CX_f/32768.0f + uc;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1338 vc = g->Sv/g->compass_N + compass_CY_f/32768.0f + vc;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1339 wc = g->Sw/g->compass_N + compass_CZ_f/32768.0f + wc;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1340
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1341 // Then save the new calibrated center:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1342 compass_CX_f = (short)(32768 * uc);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1343 compass_CY_f = (short)(32768 * vc);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1344 compass_CZ_f = (short)(32768 * wc);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1345 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1346
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1347
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1348 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1349 // compass_calib
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1350 /// @brief the main loop for calibration
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1351 /// output is compass_CX_f, compass_CY_f, compass_CZ_f and g
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1352 /// 160704 removed -4096 limit for LSM303D
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1353 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1354 /// @return always 0
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1355 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1356 int compass_calib_common(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1357 {
410
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1358 SCompassCalib g;
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1359
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1360 // Starts with no calibration at all:
410
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1361 compass_reset_calibration(&g);
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1362 uint32_t tickstart = 0;
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1363 tickstart = HAL_GetTick();
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1364 /* run calibration for one minute */
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1365 while(time_elapsed_ms(tickstart,HAL_GetTick()) < 60000)
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1366 {
410
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1367 while((SPI_Evaluate_RX_Data() == 0) && (time_elapsed_ms(tickstart,HAL_GetTick()) < 60000))
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1368 {
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1369 HAL_Delay(1);
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1370 }
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1371 compass_read();
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1372 acceleration_read();
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1373 compass_calc_roll_pitch_only();
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1374
410
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1375 if((hardwareCompass == compass_generation1 ) //HMC5883L)
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1376 &&((compass_DX_f == -4096) ||
410
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1377 (compass_DY_f == -4096) ||
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1378 (compass_DZ_f == -4096) ))
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1379 {
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1380 if(compass_gain == 0)
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1381 return -1;
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1382 compass_gain--;
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1383 compass_init(1, compass_gain);
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1384 compass_reset_calibration(&g);
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1385 continue;
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1386 }
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1387
410
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1388 copyCompassDataDuringCalibration(compass_DX_f,compass_DY_f,compass_DZ_f);
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1389 compass_add_calibration(&g);
104
22a1094545f3 Tested and alive.
Dmitry Romanov <kitt@bk.ru>
parents: 70
diff changeset
1390 }
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1391
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1392 compass_solve_calibration(&g);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1393
410
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1394 tfull32 dataBlock[4];
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1395 dataBlock[0].Word16.low16 = compass_CX_f;
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1396 dataBlock[0].Word16.hi16 = compass_CY_f;
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1397 dataBlock[1].Word16.low16 = compass_CZ_f;
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1398 dataBlock[1].Word16.hi16 = 0xFFFF;
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1399 dataBlock[2].Full32 = 0x7FFFFFFF;
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1400 dataBlock[3].Full32 = 0x7FFFFFFF;
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1401 BFA_writeDataBlock((uint32_t *)dataBlock);
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1402
410
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1403 return 0;
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1404 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1405