annotate Small_CPU/Src/compass.c @ 522:7512804bc6cf

Added compile switch for motion control: The motion control function has only beta maturity at the moment. In previous version it was more or less hidden in a sub menu. Because it is now on a top level page the compile switch has been added to enable deactivation till function is more mature.
author Ideenmodellierer
date Thu, 10 Sep 2020 20:51:36 +0200
parents d9dbfa496f7e
children 573a2bc796c8
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 {
514
d9dbfa496f7e Properly send compass 3 into sleepmode (end-2019 hardware)
heinrichsweikamp
parents: 488
diff changeset
263 I2C_DeInit();
d9dbfa496f7e Properly send compass 3 into sleepmode (end-2019 hardware)
heinrichsweikamp
parents: 488
diff changeset
264 HAL_Delay(100);
d9dbfa496f7e Properly send compass 3 into sleepmode (end-2019 hardware)
heinrichsweikamp
parents: 488
diff changeset
265 MX_I2C1_Init();
d9dbfa496f7e Properly send compass 3 into sleepmode (end-2019 hardware)
heinrichsweikamp
parents: 488
diff changeset
266 HAL_Delay(100);
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
267 uint8_t data = WHO_AM_I;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
268 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
269 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1);
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
270 if(data == WHOIAM_VALUE_LSM303D)
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
271 hardwareCompass = compass_generation2; //LSM303D;
358
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
272 data = WHO_AM_I;
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
273 I2C_Master_Transmit( DEVICE_ACCELARATOR_303AGR, &data, 1);
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
274 I2C_Master_Receive( DEVICE_ACCELARATOR_303AGR, &data, 1);
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
275 if(data == WHOIAM_VALUE_LSM303AGR)
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
276 hardwareCompass = compass_generation3; //LSM303AGR;
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
277 }
70
6a6116d7b5bb old (and new) compass support
heinrichsweikamp
parents: 38
diff changeset
278
180
9ecc2e60418d Removed not used compass code
ideenmodellierer
parents: 147
diff changeset
279 /* Assume that a HMC5883L is equipped by default if detection still failed */
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
280 if(hardwareCompass == 0)
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
281 hardwareCompass = compass_generation1; //HMC5883L;
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
282
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
283 HAL_StatusTypeDef resultOfOperationHMC_MMA = HAL_TIMEOUT;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
284
358
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
285 // 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
286 if(hardwareCompass == compass_generation1) // HMC5883L)
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
287 {
488
9eeab3fead8f Added "I2C_DeInit();" in hardware detection routines. It's the recommended way
heinrichsweikamp
parents: 410
diff changeset
288 HAL_Delay(100);
9eeab3fead8f Added "I2C_DeInit();" in hardware detection routines. It's the recommended way
heinrichsweikamp
parents: 410
diff changeset
289 I2C_DeInit();
9eeab3fead8f Added "I2C_DeInit();" in hardware detection routines. It's the recommended way
heinrichsweikamp
parents: 410
diff changeset
290 HAL_Delay(100);
358
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
291 MX_I2C1_Init();
488
9eeab3fead8f Added "I2C_DeInit();" in hardware detection routines. It's the recommended way
heinrichsweikamp
parents: 410
diff changeset
292 HAL_Delay(100);
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
293 uint8_t data = 0x2A; // CTRL_REG1 of DEVICE_ACCELARATOR_MMA8452Q
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
294 resultOfOperationHMC_MMA = I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
295 if(resultOfOperationHMC_MMA == HAL_OK)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
296 {
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
297 hardwareCompass = compass_generation1; //HMC5883L; // all fine, keep it
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
298 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
299 else
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
300 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
301 hardwareCompass = COMPASS_NOT_RECOGNIZED;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
302 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
303 }
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 if(hardwareCompass == compass_generation2) //LSM303D)
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
306 compass_init_LSM303D(fast, gain);
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
307 if(hardwareCompass == compass_generation3) //LSM303AGR)
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
308 compass_init_LSM303AGR(fast, gain);
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
309 if(hardwareCompass == compass_generation1) //HMC5883L)
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
310 compass_init_HMC5883L(fast, gain);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
311
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
312 tfull32 dataBlock[4];
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
313 if(BFA_readLastDataBlock((uint32_t *)dataBlock) == BFA_OK)
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
314 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
315 compass_CX_f = dataBlock[0].Word16.low16;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
316 compass_CY_f = dataBlock[0].Word16.hi16;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
317 compass_CZ_f = dataBlock[1].Word16.low16;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
318 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
319
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
320 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
321
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
322
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
323 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
324 // compass_calib
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
325 /// @brief with onchip_lowpass_filter configuration for accelerometer of LSM303D
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
326 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
327 int compass_calib(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
328 {
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
329 if(hardwareCompass == compass_generation2) //LSM303D)
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
330 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
331 LSM303D_accel_set_onchip_lowpass_filter_bandwidth(773);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
332 int out = compass_calib_common();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
333 LSM303D_accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
334 return out;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
335 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
336 else
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
337 if(hardwareCompass == compass_generation1) //HMC5883L)
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
338 {
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
339 return compass_calib_common();
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
340 }
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
341 else
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
342 if(hardwareCompass == compass_generation3) //LSM303AGR)
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
343 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
344 return compass_calib_common();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
345 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
346 else
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
347 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
348 return 0; // standard answer of compass_calib_common();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
349 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
350
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
351
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
352 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
353
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
354
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
355 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
356 // compass_sleep
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
357 /// @brief low power mode
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
358 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
359 void compass_sleep(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
360 {
514
d9dbfa496f7e Properly send compass 3 into sleepmode (end-2019 hardware)
heinrichsweikamp
parents: 488
diff changeset
361 if(hardwareCompass == compass_generation2) //LSM303D
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
362 compass_sleep_LSM303D();
514
d9dbfa496f7e Properly send compass 3 into sleepmode (end-2019 hardware)
heinrichsweikamp
parents: 488
diff changeset
363 if(hardwareCompass == compass_generation1) //HMC5883L
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
364 compass_sleep_HMC5883L();
514
d9dbfa496f7e Properly send compass 3 into sleepmode (end-2019 hardware)
heinrichsweikamp
parents: 488
diff changeset
365 if(hardwareCompass == compass_generation3) //LSM303AGR
d9dbfa496f7e Properly send compass 3 into sleepmode (end-2019 hardware)
heinrichsweikamp
parents: 488
diff changeset
366 compass_sleep_LSM303AGR();
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
367 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
368
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
369
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
370 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
371 // compass_read
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
372 /// @brief reads magnetometer and accelerometer for LSM303D,
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
373 /// otherwise magnetometer only
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
374 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
375 void compass_read(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
376 {
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
377 if(hardwareCompass == compass_generation2) //LSM303D)
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
378 compass_read_LSM303D();
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
379 if(hardwareCompass == compass_generation1) //HMC5883L)
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
380 compass_read_HMC5883L();
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
381 if(hardwareCompass == compass_generation3) //LSM303AGR)
514
d9dbfa496f7e Properly send compass 3 into sleepmode (end-2019 hardware)
heinrichsweikamp
parents: 488
diff changeset
382 compass_read_LSM303AGR();
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
383
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
384 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
385
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
386
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
387 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
388 // accelerator_init
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
389 /// @brief empty for for LSM303D
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
390 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
391 void accelerator_init(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
392 {
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
393 if(hardwareCompass == compass_generation1) //HMC5883L)
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
394 accelerator_init_MMA8452Q();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
395 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
396
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
397
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
398 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
399 // accelerator_sleep
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
400 /// @brief empty for for LSM303D
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
401 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
402 void accelerator_sleep(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
403 {
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
404 if(hardwareCompass == compass_generation1) //HMC5883L)
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
405 accelerator_sleep_MMA8452Q();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
406 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
407
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
408
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
409 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
410 // acceleration_read
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
411 /// @brief empty for for LSM303D
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
412 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
413 void acceleration_read(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
414 {
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
415 if(hardwareCompass == compass_generation2) //LSM303D)
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
416 acceleration_read_LSM303D();
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
417 if(hardwareCompass == compass_generation1) //HMC5883L)
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
418 acceleration_read_MMA8452Q();
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
419 if(hardwareCompass == compass_generation3) //LSM303AGR)
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
420 acceleration_read_LSM303AGR();
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
421 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
422
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
423
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
424 /* Private functions ---------------------------------------------------------*/
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
425
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
426 // ===============================================================================
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
427 // LSM303AGR_read_reg
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 uint8_t LSM303AGR_read_reg(uint8_t addr)
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
430 {
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
431 uint8_t 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 I2C_Master_Transmit( DEVICE_COMPASS_303AGR, &addr, 1);
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
434 I2C_Master_Receive( DEVICE_COMPASS_303AGR, &data, 1);
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
435 return data;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
436 }
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
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 // LSM303AGR_write_reg
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 void LSM303AGR_write_reg(uint8_t addr, uint8_t value)
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
443 {
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
444 uint8_t 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 data[0] = addr;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
447 data[1] = value;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
448 I2C_Master_Transmit( DEVICE_COMPASS_303AGR, data, 2);
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
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 // LSM303AGR_acc_write_reg
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 void LSM303AGR_acc_write_reg(uint8_t addr, uint8_t value)
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
455 {
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
456 uint8_t 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 data[0] = addr;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
459 data[1] = value;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
460 I2C_Master_Transmit( DEVICE_ACCELARATOR_303AGR, data, 2);
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
461 }
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
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_checked_reg
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 void LSM303AGR_write_checked_reg(uint8_t addr, uint8_t value)
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_write_reg(addr, value);
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
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_checked_reg
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 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
476 {
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
477 LSM303AGR_acc_write_reg(addr, value);
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
478 }
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
479
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
480 // ===============================================================================
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
481 // LSM303D_read_reg
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
482 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
483 uint8_t LSM303D_read_reg(uint8_t addr)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
484 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
485 uint8_t data;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
486
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
487 I2C_Master_Transmit( DEVICE_COMPASS_303D, &addr, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
488 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
489 return data;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
490 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
491
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
492
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
493 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
494 // LSM303D_write_reg
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
495 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
496 void LSM303D_write_reg(uint8_t addr, uint8_t value)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
497 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
498 uint8_t data[2];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
499
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
500 /* enable accel*/
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
501 data[0] = addr;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
502 data[1] = value;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
503 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
504 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
505
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
506
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
507 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
508 // LSM303D_write_checked_reg
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
509 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
510 void LSM303D_write_checked_reg(uint8_t addr, uint8_t value)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
511 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
512 LSM303D_write_reg(addr, value);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
513 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
514
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
515
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
516 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
517 // LSM303D_modify_reg
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
518 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
519 void LSM303D_modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
520 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
521 uint8_t val;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
522
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
523 val = LSM303D_read_reg(reg);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
524 val &= ~clearbits;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
525 val |= setbits;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
526 LSM303D_write_checked_reg(reg, val);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
527 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
528
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
529 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
530 // LSM303D_accel_set_onchip_lowpass_filter_bandwidth
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
531 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
532 int LSM303D_accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
533 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
534 uint8_t setbits = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
535 uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
536
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
537 if (bandwidth == 0) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
538 bandwidth = 773;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
539 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
540
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
541 if (bandwidth <= 50) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
542 setbits |= REG2_AA_FILTER_BW_50HZ_A;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
543 _accel_onchip_filter_bandwith = 50;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
544
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
545 } else if (bandwidth <= 194) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
546 setbits |= REG2_AA_FILTER_BW_194HZ_A;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
547 _accel_onchip_filter_bandwith = 194;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
548
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
549 } else if (bandwidth <= 362) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
550 setbits |= REG2_AA_FILTER_BW_362HZ_A;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
551 _accel_onchip_filter_bandwith = 362;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
552
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
553 } else if (bandwidth <= 773) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
554 setbits |= REG2_AA_FILTER_BW_773HZ_A;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
555 _accel_onchip_filter_bandwith = 773;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
556
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
557 } else {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
558 return -1;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
559 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
560
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
561 LSM303D_modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
562
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
563 return 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
564 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
565
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
566
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
567 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
568 // LSM303D_accel_set_driver_lowpass_filter
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
569 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
570 int LSM303D_accel_set_driver_lowpass_filter(float samplerate, float bandwidth)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
571 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
572 /*
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
573 _accel_filter_x_set_cutoff_frequency(samplerate, bandwidth);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
574 _accel_filter_y_set_cutoff_frequency(samplerate, bandwidth);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
575 _accel_filter_z_set_cutoff_frequency(samplerate, bandwidth);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
576 */
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
577 return 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
578 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
579
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
580
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
581 // rotate_mag_3f: nicht genutzt aber praktisch; rotate_accel_3f wird benutzt
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
582 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
583 // rotate_mag_3f
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
584 /// @brief swap axis in convient way, by hw
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
585 /// @param *x raw input is set to *y input
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
586 /// @param *y raw input is set to -*x input
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
587 /// @param *z raw is not touched
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
588 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
589 void rotate_mag_3f(float *x, float *y, float *z)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
590 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
591 return;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
592 /*
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
593 *x = *x; // HMC: *x = -*y
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
594 *y = *y; // HMC: *y = *x // change 20.04.2016: zuvor *y = -*y
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
595 *z = *z; // HMC: *z = *z
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
596 */
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
597 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
598
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
599
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
600 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
601 // rotate_accel_3f
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
602 /// @brief swap axis in convient way, by hw, same as MMA8452Q
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
603 /// @param *x raw input, output is with sign change
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
604 /// @param *y raw input, output is with sign change
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
605 /// @param *z raw input, output is with sign change
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
606 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
607 void rotate_accel_3f(float *x, float *y, float *z)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
608 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
609 *x = -*x;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
610 *y = -*y;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
611 *z = -*z;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
612 /* tested:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
613 x = x, y =-y, z=-z: does not work with roll
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
614 x = x, y =y, z=-z: does not work with pitch
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
615 x = x, y =y, z=z: does not work at all
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
616 */
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
617 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
618
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
619
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
620 // ===============================================================================
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
621 // compass_init_LSM303D
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
622 /// This might be called several times with different gain values during calibration
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
623 /// but gain change is not supported at the moment.
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
624 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
625 /// @param gain: 7 is max gain and set with here, compass_calib() might reduce it
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
626 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
627
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
628 //uint8_t testCompassLS303D[11];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
629
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
630 void compass_init_LSM303D(uint8_t fast, uint8_t gain)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
631 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
632 if(fast == 0)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
633 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
634 LSM303D_write_checked_reg(ADDR_CTRL_REG0, 0x00);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
635 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
636 LSM303D_write_checked_reg(ADDR_CTRL_REG2, 0xC0);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
637 LSM303D_write_checked_reg(ADDR_CTRL_REG3, 0x00);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
638 LSM303D_write_checked_reg(ADDR_CTRL_REG4, 0x00);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
639 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
640 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
641 else
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
642 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
643 LSM303D_write_checked_reg(ADDR_CTRL_REG0, 0x00);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
644 LSM303D_write_checked_reg(ADDR_CTRL_REG1, 0x6F); // 100 Hz
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
645 LSM303D_write_checked_reg(ADDR_CTRL_REG2, 0xC0);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
646 LSM303D_write_checked_reg(ADDR_CTRL_REG3, 0x00);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
647 LSM303D_write_checked_reg(ADDR_CTRL_REG4, 0x00);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
648 LSM303D_write_checked_reg(ADDR_CTRL_REG5, 0x74); // 100 Hz
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
649 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
650 LSM303D_write_checked_reg(ADDR_CTRL_REG6, 0x00);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
651 LSM303D_write_checked_reg(ADDR_CTRL_REG7, 0x00);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
652
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
653 return;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
654 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
655
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
656
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
657 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
658 // compass_sleep_LSM303D
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
659 // @brief Gen 2 chip
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
660 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
661 void compass_sleep_LSM303D(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
662 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
663 LSM303D_write_checked_reg(ADDR_CTRL_REG1, 0x00); // CNTRL1: acceleration sensor Power-down mode
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
664 LSM303D_write_checked_reg(ADDR_CTRL_REG7, 0x02); // CNTRL7: magnetic sensor Power-down mode
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
665 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
666
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
667
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
668 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
669 // acceleration_read_LSM303D
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
670 // output is accel_DX_f, accel_DY_f, accel_DZ_f
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
671 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
672 void acceleration_read_LSM303D(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
673 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
674 uint8_t data;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
675 float xraw_f, yraw_f, zraw_f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
676 float accel_report_x, accel_report_y, accel_report_z;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
677
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
678 memset(accDataBuffer,0,6);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
679
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
680 accel_DX_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
681 accel_DY_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
682 accel_DZ_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
683
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
684 for(int i=0;i<6;i++)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
685 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
686 data = ADDR_OUT_X_L_A + i;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
687 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
688 I2C_Master_Receive( DEVICE_COMPASS_303D, &accDataBuffer[i], 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
689 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
690
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
691 xraw_f = ((float)( (int16_t)((accDataBuffer[1] << 8) | (accDataBuffer[0]))));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
692 yraw_f = ((float)( (int16_t)((accDataBuffer[3] << 8) | (accDataBuffer[2]))));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
693 zraw_f = ((float)( (int16_t)((accDataBuffer[5] << 8) | (accDataBuffer[4]))));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
694
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
695 rotate_accel_3f(&xraw_f, &yraw_f, &zraw_f);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
696
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
697 // mh
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
698 accel_report_x = xraw_f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
699 accel_report_y = yraw_f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
700 accel_report_z = zraw_f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
701
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
702 accel_DX_f = ((int16_t)(accel_report_x));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
703 accel_DY_f = ((int16_t)(accel_report_y));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
704 accel_DZ_f = ((int16_t)(accel_report_z));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
705 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
706
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
707
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
708 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
709 // compass_read_LSM303D
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
710 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
711 /// output is compass_DX_f, compass_DY_f, compass_DZ_f
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
712 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
713 void compass_read_LSM303D(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
714 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
715 uint8_t data;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
716 // float xraw_f, yraw_f, zraw_f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
717 // float mag_report_x, mag_report_y, mag_report_z;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
718
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
719 memset(magDataBuffer,0,6);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
720
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
721 compass_DX_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
722 compass_DY_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
723 compass_DZ_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
724
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
725 for(int i=0;i<6;i++)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
726 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
727 data = ADDR_OUT_X_L_M + i;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
728 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
729 I2C_Master_Receive( DEVICE_COMPASS_303D, &magDataBuffer[i], 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
730 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
731
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
732 // mh 160620 flip x and y if flip display
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
733 compass_DX_f = (((int16_t)((magDataBuffer[1] << 8) | (magDataBuffer[0]))));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
734 compass_DY_f = (((int16_t)((magDataBuffer[3] << 8) | (magDataBuffer[2]))));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
735 compass_DZ_f = (((int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4]))));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
736 // no rotation
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
737 return;
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
738 }
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
739
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
740
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 // compass_init_LSM303AGR
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
743 /// 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
744 /// but gain change is not supported at the moment.
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
745 ///
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
746 /// @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
747 // ===============================================================================
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
748
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
749 void compass_init_LSM303AGR(uint8_t fast, uint8_t gain)
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
750 {
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
751 if(fast == 0)
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
752 {
358
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
753 // init compass
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
754 LSM303AGR_write_checked_reg(0x60, 0x80); // 10Hz
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
755 LSM303AGR_write_checked_reg(0x61, 0x03); // CFG_REG_B_M
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
756 LSM303AGR_write_checked_reg(0x62, 0x10); // CFG_REG_C_M
358
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
757 LSM303AGR_write_checked_reg(0x63, 0x00); // INT_CTRL_REG_M
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
758
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
759 // init accel (Same chip, but different address...)
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
760 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
761 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
762 LSM303AGR_acc_write_checked_reg(0x21, 0x00); // CTRL_REG2_A
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
763 LSM303AGR_acc_write_checked_reg(0x22, 0x00); // CTRL_REG3_A
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
764 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
765 }
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
766 else
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
767 {
358
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
768 // init compass
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
769 LSM303AGR_write_checked_reg(0x60, 0x84); // 20Hz
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
770 LSM303AGR_write_checked_reg(0x61, 0x03); // CFG_REG_B_M
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
771 LSM303AGR_write_checked_reg(0x62, 0x10); // CFG_REG_C_M
358
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
772 LSM303AGR_write_checked_reg(0x63, 0x00); // INT_CTRL_REG_M
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
773
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
774 // init accel (Same chip, but different address...)
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
775 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
776 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
777 LSM303AGR_acc_write_checked_reg(0x21, 0x00); // CTRL_REG2_A
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
778 LSM303AGR_acc_write_checked_reg(0x22, 0x00); // CTRL_REG3_A
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
779 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
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 return;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
783 }
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
784
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 // ===============================================================================
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
787 // compass_sleep_LSM303D
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
788 // @brief Gen 2 chip
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
789 // ===============================================================================
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
790 void compass_sleep_LSM303AGR(void)
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
791 {
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
792 LSM303AGR_write_checked_reg(0x60, 0x03); //
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
793 LSM303AGR_write_checked_reg(0x61, 0x04); //
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
794 LSM303AGR_write_checked_reg(0x62, 0x51); //
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
795 LSM303AGR_write_checked_reg(0x63, 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
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
798 LSM303AGR_acc_write_checked_reg(0x1F, 0x00); //
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
799 LSM303AGR_acc_write_checked_reg(0x20, 0x00); //
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
800 }
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
801
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
802
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
803 // ===============================================================================
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
804 // acceleration_read_LSM303AGR
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
805 // output is accel_DX_f, accel_DY_f, accel_DZ_f
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
806 // ===============================================================================
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
807 void acceleration_read_LSM303AGR(void)
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 uint8_t data;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
810 float xraw_f, yraw_f, zraw_f;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
811 float accel_report_x, accel_report_y, accel_report_z;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
812
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
813 memset(accDataBuffer,0,6);
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 accel_DX_f = 0;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
816 accel_DY_f = 0;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
817 accel_DZ_f = 0;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
818
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
819 for(int i=0;i<6;i++)
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
820 {
358
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
821 data = 0x28 + i; // OUT_X_L_A
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
822 I2C_Master_Transmit( DEVICE_ACCELARATOR_303AGR, &data, 1);
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
823 I2C_Master_Receive( DEVICE_ACCELARATOR_303AGR, &accDataBuffer[i], 1);
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
824 }
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 xraw_f = ((float)( (int16_t)((accDataBuffer[1] << 8) | (accDataBuffer[0]))));
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
827 yraw_f = ((float)( (int16_t)((accDataBuffer[3] << 8) | (accDataBuffer[2]))));
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
828 zraw_f = ((float)( (int16_t)((accDataBuffer[5] << 8) | (accDataBuffer[4]))));
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
829
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
830 rotate_accel_3f(&xraw_f, &yraw_f, &zraw_f);
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
831
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
832 // mh
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
833 accel_report_x = xraw_f;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
834 accel_report_y = yraw_f;
358
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
835 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
836
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
837 accel_DX_f = ((int16_t)(accel_report_x));
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
838 accel_DY_f = ((int16_t)(accel_report_y));
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
839 accel_DZ_f = ((int16_t)(accel_report_z));
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
840 }
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
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 // compass_read_LSM303AGR
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 /// output is compass_DX_f, compass_DY_f, compass_DZ_f
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
847 // ===============================================================================
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
848 void compass_read_LSM303AGR(void)
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 uint8_t data;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
851 // float xraw_f, yraw_f, zraw_f;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
852 // float mag_report_x, mag_report_y, mag_report_z;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
853
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
854 memset(magDataBuffer,0,6);
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 compass_DX_f = 0;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
857 compass_DY_f = 0;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
858 compass_DZ_f = 0;
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
859
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
860 for(int i=0;i<6;i++)
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 data = 0x68 + i; // OUTX_L_REG_M
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
863 I2C_Master_Transmit( DEVICE_COMPASS_303AGR, &data, 1);
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
864 I2C_Master_Receive( DEVICE_COMPASS_303AGR, &magDataBuffer[i], 1);
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
865 }
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
866
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
867 // mh 160620 flip x and y if flip display
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
868 compass_DX_f = (((int16_t)((magDataBuffer[1] << 8) | (magDataBuffer[0]))));
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
869 compass_DY_f = (((int16_t)((magDataBuffer[3] << 8) | (magDataBuffer[2]))));
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
870 compass_DZ_f = (((int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4]))));
358
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
871
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
872 // align axis in gen 2 hardware
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
873 compass_DZ_f *= -1;
c6a084d1433f Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 357
diff changeset
874
357
c3d511365552 Add Support for new end-2019 hardware:
heinrichsweikamp
parents: 313
diff changeset
875 return;
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
876 }
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 // ----------EARLIER COMPONENTS ---------------------------------------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
881 // --------------------------------------------------------------------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
882
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
883 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
884 // compass_init_HMC5883L
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
885 /// @brief The horrible Honeywell compass chip
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
886 /// This might be called several times during calibration
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
887 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
888 /// @param fast: 1 is fast mode, 0 is normal mode
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
889 /// @param gain: 7 is max gain and set with here, compass_calib() might reduce it
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
890 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
891 void compass_init_HMC5883L(uint8_t fast, uint8_t gain)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
892 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
893 uint8_t write_buffer[4];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
894
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
895 compass_gain = gain;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
896 uint16_t length = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
897 write_buffer[0] = 0x00; // 00 = config Register A
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
898
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
899 if( fast )
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
900 write_buffer[1] = 0x38; // 0b 0011 1000; // ConfigA: 75Hz, 2 Samples averaged
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
901 else
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
902 write_buffer[1] = 0x68; // 0b 0110 1000; // ConfigA: 3Hz, 8 Samples averaged
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
903
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
904 switch(gain)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
905 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
906 case 7:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
907 write_buffer[2] = 0xE0; //0b 1110 0000; // ConfigB: gain
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
908 break;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
909 case 6:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
910 write_buffer[2] = 0xC0; //0b 1100 0000; // ConfigB: gain
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
911 break;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
912 case 5:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
913 write_buffer[2] = 0xA0; //0b 1010 0000; // ConfigB: gain
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
914 break;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
915 case 4:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
916 write_buffer[2] = 0x80; //0b 1000 0000; // ConfigB: gain
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
917 break;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
918 case 3:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
919 write_buffer[2] = 0x60; //0b 0110 0000; // ConfigB: gain
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
920 break;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
921 case 2:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
922 write_buffer[2] = 0x40; //0b 01000 0000; // ConfigB: gain
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
923 break;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
924 case 1:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
925 write_buffer[2] = 0x20; //0b 00100 0000; // ConfigB: gain
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
926 break;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
927 case 0:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
928 write_buffer[2] = 0x00; //0b 00000 0000; // ConfigB: gain
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
929 break;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
930 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
931 write_buffer[3] = 0x00; // Mode: continuous mode
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
932 length = 4;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
933 //hmc_twi_write(0);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
934 I2C_Master_Transmit( DEVICE_COMPASS_HMC5883L, write_buffer, length);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
935 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
936
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
937
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
938
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
939 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
940 // compass_sleep_HMC5883L
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
941 /// @brief Power-down mode for Honeywell compass chip
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
942 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
943 void compass_sleep_HMC5883L(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
944 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
945 uint8_t write_buffer[4];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
946 uint16_t length = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
947
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
948 write_buffer[0] = 0x00; // 00 = config Register A
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
949 write_buffer[1] = 0x68; // 0b 0110 1000; // ConfigA
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
950 write_buffer[2] = 0x20; // 0b 0010 0000; // ConfigB
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
951 write_buffer[3] = 0x02; // 0b 0000 0010; // Idle Mode
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
952 length = 4;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
953 I2C_Master_Transmit( DEVICE_COMPASS_HMC5883L, write_buffer, length);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
954 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
955
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
956
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
957 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
958 // accelerator_init_MMA8452Q
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
959 /// @brief Power-down mode for acceleration chip used in combination with Honeywell compass
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
960 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
961 void accelerator_init_MMA8452Q(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
962 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
963 uint8_t write_buffer[4];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
964 uint16_t length = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
965 //HAL_Delay(1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
966 //return;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
967 write_buffer[0] = 0x0E; // XYZ_DATA_CFG
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
968 write_buffer[1] = 0x00;//0b00000000; // High pass Filter=0 , +/- 2g range
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
969 length = 2;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
970 I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, write_buffer, length);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
971 //HAL_Delay(1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
972 write_buffer[0] = 0x2A; // CTRL_REG1
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
973 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
974 write_buffer[2] = 0x02; //0b00000010; // CTRL_REG2: High Res in Active mode
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
975 length = 3;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
976 I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, write_buffer, length);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
977
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
978 //HAL_Delay(1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
979 //hw_delay_us(100);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
980 write_buffer[0] = 0x2A; // CTRL_REG1
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
981 write_buffer[1] = 0x35; //0b00110101; // CTRL_REG1: ... Active Mode
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
982 length = 2;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
983 I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, write_buffer, length);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
984 /*
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
985 HAL_Delay(6);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
986 compass_read();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
987 HAL_Delay(1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
988 acceleration_read();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
989
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
990 compass_calc();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
991 */
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
992 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
993
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
994
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
995 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
996 // accelerator_sleep_MMA8452Q
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
997 /// @brief compass_sleep_HMC5883L
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
998 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
999 void accelerator_sleep_MMA8452Q(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1000 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1001 uint16_t length = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1002 uint8_t write_buffer[4];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1003
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1004 write_buffer[0] = 0x2A; // CTRL_REG1
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1005 write_buffer[1] = 0x00; //0b00000000; // CTRL_REG1: Standby Mode
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1006 length = 2;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1007 I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, write_buffer, length);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1008 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1009
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1010
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1011 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1012 // compass_read_HMC5883L
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1013 /// @brief The new ST 303D - get ALL data and store in static variables
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1014 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1015 /// output is compass_DX_f, compass_DY_f, compass_DZ_f
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1016 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1017 void compass_read_HMC5883L(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1018 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1019 uint8_t buffer[20];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1020 compass_DX_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1021 compass_DY_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1022 compass_DZ_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1023 uint8_t length = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1024 uint8_t read_buffer[6];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1025 signed_tword data;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1026 for(int i = 0; i<6;i++)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1027 read_buffer[i] = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1028 buffer[0] = 0x03; // 03 = Data Output X MSB Register
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1029 length = 1;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1030 I2C_Master_Transmit( DEVICE_COMPASS_HMC5883L, buffer, length);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1031 length = 6;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1032 I2C_Master_Receive( DEVICE_COMPASS_HMC5883L, read_buffer, length);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1033
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1034
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1035 data.Byte.hi = read_buffer[0];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1036 data.Byte.low = read_buffer[1];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1037 //Y = Z
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1038 compass_DY_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[2];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1041 data.Byte.low = read_buffer[3];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1042 compass_DZ_f = data.Word;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1043
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1044 data.Byte.hi = read_buffer[4];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1045 data.Byte.low = read_buffer[5];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1046 //X = -Y
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1047 compass_DX_f = data.Word;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1048 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1049
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1050
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1051 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1052 // acceleration_read_MMA8452Q
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1053 /// @brief The old MMA8452Q used with the Honeywell compass
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1054 /// get the acceleration data and store in static variables
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1055 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1056 /// output is accel_DX_f, accel_DY_f, accel_DZ_f
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1057 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1058 void acceleration_read_MMA8452Q(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1059 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1060 uint8_t buffer[20];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1061 accel_DX_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1062 accel_DY_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1063 accel_DZ_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1064 uint8_t length = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1065 // bit8_Type status ;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1066 uint8_t read_buffer[7];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1067 signed_tword data;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1068 for(int i = 0; i<6;i++)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1069 read_buffer[i] = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1070 buffer[0] = 0x00; // 03 = Data Output X MSB Register
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1071 length = 1;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1072 I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, buffer, length);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1073 length = 7;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1074 I2C_Master_Receive( DEVICE_ACCELARATOR_MMA8452Q, read_buffer, length);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1075
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1076 // status.uw = read_buffer[0];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1077 data.Byte.hi = read_buffer[1];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1078 data.Byte.low = read_buffer[2];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1079 accel_DX_f =data.Word/16;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1080 data.Byte.hi = read_buffer[3];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1081 data.Byte.low = read_buffer[4];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1082 accel_DY_f =data.Word/16;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1083 data.Byte.hi = read_buffer[5];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1084 data.Byte.low = read_buffer[6];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1085 accel_DZ_f =data.Word/16;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1086
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1087 accel_DX_f *= -1;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1088 accel_DY_f *= -1;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1089 accel_DZ_f *= -1;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1090 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1091
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1092
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1093 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1094 // compass_calc_roll_pitch_only
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1095 /// @brief only the roll and pitch parts of compass_calc()
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1096 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1097 /// input is accel_DX_f, accel_DY_f, accel_DZ_f
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1098 /// output is compass_pitch and compass_roll
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1099 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1100 void compass_calc_roll_pitch_only(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1101 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1102 float sinPhi, cosPhi;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1103 float Phi, Teta;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1104
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1105 //---- Calculate sine and cosine of roll angle Phi -----------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1106 Phi= atan2f(accel_DY_f, accel_DZ_f) ;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1107 compass_roll = Phi * 180.0f /PI;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1108 sinPhi = sinf(Phi);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1109 cosPhi = cosf(Phi);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1110
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1111 //---- calculate sin and cosine of pitch angle Theta ---------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1112 Teta = atanf(-(float)accel_DX_f/(accel_DY_f * sinPhi + accel_DZ_f * cosPhi));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1113 compass_pitch = Teta * 180.0f /PI;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1114 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1115
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1116
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1117 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1118 // compass_calc
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1119 /// @brief all the fancy stuff first implemented in OSTC3
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1120 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1121 /// 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
1122 /// and compass_CX_f, compass_CY_f, compass_CZ_f
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1123 /// output is compass_heading, compass_pitch and compass_roll
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1124 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1125 void compass_calc(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1126 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1127 float sinPhi, cosPhi, sinTeta, cosTeta;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1128 float Phi, Teta, Psi;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1129 int16_t iBfx, iBfy;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1130 int16_t iBpx, iBpy, iBpz;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1131
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1132 //---- Make hard iron correction -----------------------------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1133 // Measured magnetometer orientation, measured ok.
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1134 // From matthias drawing: (X,Y,Z) --> (X,Y,Z) : no rotation.
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1135 iBpx = compass_DX_f - compass_CX_f; // X
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1136 iBpy = compass_DY_f - compass_CY_f; // Y
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1137 iBpz = compass_DZ_f - compass_CZ_f; // Z
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1138
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1139 //---- Calculate sine and cosine of roll angle Phi -----------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1140 //sincos(accel_DZ_f, accel_DY_f, &sin, &cos);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1141 Phi= atan2f(accel_DY_f, accel_DZ_f) ;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1142 compass_roll = Phi * 180.0f /PI;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1143 sinPhi = sinf(Phi);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1144 cosPhi = cosf(Phi);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1145
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1146 //---- rotate by roll angle (-Phi) ---------------------------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1147 iBfy = iBpy * cosPhi - iBpz * sinPhi;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1148 iBpz = iBpy * sinPhi + iBpz * cosPhi;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1149 //Gz = imul(accel_DY_f, sin) + imul(accel_DZ_f, cos);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1150
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1151 //---- calculate sin and cosine of pitch angle Theta ---------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1152 //sincos(Gz, -accel_DX_f, &sin, &cos); // NOTE: changed sin sign.
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1153 // 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
1154 Teta = atanf(-(float)accel_DX_f/(accel_DY_f * sinPhi + accel_DZ_f * cosPhi));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1155 compass_pitch = Teta * 180.0f /PI;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1156 sinTeta = sinf(Teta);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1157 cosTeta = cosf(Teta);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1158 /* correct cosine if pitch not in range -90 to 90 degrees */
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1159 if( cosTeta < 0 ) cosTeta = -cosTeta;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1160
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1161 ///---- de-rotate by pitch angle Theta -----------------------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1162 iBfx = iBpx * cosTeta + iBpz * sinTeta;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1163
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1164 //---- Detect uncalibrated compass ---------------------------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1165 if( !compass_CX_f && !compass_CY_f && !compass_CZ_f )
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1166 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1167 compass_heading = -1;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1168 return;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1169 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1170
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1171 //---- calculate current yaw = e-compass angle Psi -----------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1172 // Result in degree (no need of 0.01 deg precision...
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1173 Psi = atan2f(-iBfy,iBfx);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1174 compass_heading = Psi * 180.0f /PI;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1175 // Result in 0..360 range:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1176 if( compass_heading < 0 )
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1177 compass_heading += 360;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1178 }
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 // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1182 // // - Calibration - ///////////////////////////////////////////////////////////////////////////////////////////////////////
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1183 // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1184
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1185 /* can be lost during sleep as those are reset with compass_reset_calibration() */
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1186
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1187 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1188 // compass_reset_calibration
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1189 /// @brief all the fancy stuff first implemented in OSTC3
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1190 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1191 /// output is struct g and compass_CX_f, compass_CY_f, compass_CZ_f
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1192 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1193 /// @param g: is a struct with crazy stuff like Suuu, Svvv, Svvu, etc.
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1194 /// all is set to zero here
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1195 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1196 void compass_reset_calibration(SCompassCalib *g)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1197 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1198 g->compass_N = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1199 g->Su = g->Sv = g->Sw = 0.0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1200 g->Suu = g->Svv = g->Sww = g->Suv = g->Suw = g->Svw = 0.0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1201 g->Suuu = g->Svvv = g->Swww = 0.0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1202 g->Suuv = g->Suuw = g->Svvu = g->Svvw = g->Swwu = g->Swwv = 0.0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1203 compass_CX_f = compass_CY_f = compass_CZ_f = 0.0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1204 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1205
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1206
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1207 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1208 // compass_add_calibration
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1209 /// @brief all the fancy stuff first implemented in OSTC3
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1210 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1211 /// input is compass_DX_f, compass_DY_f, compass_DZ_f
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1212 /// and compass_CX_f, compass_CY_f, compass_CZ_f
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1213 /// output is struct g
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1214 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1215 /// @param g: is a struct with crazy stuff like Suuu, Svvv, Svvu, etc.
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1216 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1217 void compass_add_calibration(SCompassCalib *g)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1218 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1219 float u, v, w;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1220
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1221 u = (compass_DX_f - compass_CX_f) / 32768.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1222 v = (compass_DY_f - compass_CY_f) / 32768.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1223 w = (compass_DZ_f - compass_CZ_f) / 32768.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1224
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1225 g->compass_N++;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1226 g->Su += u;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1227 g->Sv += v;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1228 g->Sw += w;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1229 g->Suv += u*v;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1230 g->Suw += u*w;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1231 g->Svw += v*w;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1232 g->Suu += u*u;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1233 g->Suuu += u*u*u;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1234 g->Suuv += v*u*u;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1235 g->Suuw += w*u*u;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1236 g->Svv += v*v;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1237 g->Svvv += v*v*v;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1238 g->Svvu += u*v*v;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1239 g->Svvw += w*v*v;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1240 g->Sww += w*w;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1241 g->Swww += w*w*w;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1242 g->Swwu += u*w*w;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1243 g->Swwv += v*w*w;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1244 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1245
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1246 //////////////////////////////////////////////////////////////////////////////
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1247
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1248 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1249 // compass_solve_calibration
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1250 /// @brief all the fancy stuff first implemented in OSTC3
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1251 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1252 /// input is compass_CX_f, compass_CY_f, compass_CZ_f and g
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1253 /// output is struct g
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1254 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1255 /// @param g: is a struct with crazy stuff like Suuu, Svvv, Svvu, etc.
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1256 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1257 void compass_solve_calibration(SCompassCalib *g)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1258 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1259 float yu, yv, yw;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1260 float delta;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1261 float uc, vc, wc;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1262
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1263
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1264 //---- Normalize partial sums --------------------------------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1265 //
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1266 // u, v, w should be centered on the mean value um, vm, wm:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1267 // x = u + um, with um = Sx/N
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1268 //
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1269 // So:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1270 // (u + um)**2 = u**2 + 2u*um + um**2
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1271 // Su = 0, um = Sx/N
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1272 // Sxx = Suu + 2 um Su + N*(Sx/N)**2 = Suu + Sx**2/N
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1273 // Suu = Sxx - Sx**2/N
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1274 yu = g->Su/g->compass_N;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1275 yv = g->Sv/g->compass_N;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1276 yw = g->Sw/g->compass_N;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1277
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1278 g->Suu -= g->Su*yu;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1279 g->Svv -= g->Sv*yv;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1280 g->Sww -= g->Sw*yw;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1281
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1282 // (u + um)(v + vm) = uv + u vm + v um + um vm
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1283 // Sxy = Suv + N * um vm
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1284 // Suv = Sxy - N * (Sx/N)(Sy/N);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1285 g->Suv -= g->Su*yv;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1286 g->Suw -= g->Su*yw;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1287 g->Svw -= g->Sv*yw;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1288
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1289 // (u + um)**3 = u**3 + 3 u**2 um + 3 u um**2 + um**3
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1290 // Sxxx = Suuu + 3 um Suu + 3 um**2 Su + N.um**3
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1291 // Su = 0, um = Sx/N:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1292 // Suuu = Sxxx - 3 Sx*Suu/N - N.(Sx/N)**3
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1293 // = Sxxx - 3 Sx*Suu/N - Sx**3/N**2
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1294
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1295 // (u + um)**2 (v + vm) = (u**2 + 2 u um + um**2)(v + vm)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1296 // Sxxy = Suuv + vm Suu + 2 um (Suv + vm Su) + um**2 (Sv + N.vm)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1297 //
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1298 // Su = 0, Sv = 0, vm = Sy/N:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1299 // Sxxy = Suuv + vm Suu + 2 um Suv + N um**2 vm
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1300 //
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1301 // Suuv = Sxxy - (Sy/N) Suu - 2 (Sx/N) Suv - (Sx/N)**2 Sy
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1302 // = Sxxy - Suu*Sy/N - 2 Suv*Sx/N - Sx*Sx*Sy/N/N
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1303 // = Sxxy - (Suu + Sx*Sx/N)*Sy/N - 2 Suv*Sx/N
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1304 g->Suuu -= (3*g->Suu + g->Su*yu)*yu;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1305 g->Suuv -= (g->Suu + g->Su*yu)*yv + 2*g->Suv*yu;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1306 g->Suuw -= (g->Suu + g->Su*yu)*yw + 2*g->Suw*yu;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1307
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1308 g->Svvu -= (g->Svv + g->Sv*yv)*yu + 2*g->Suv*yv;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1309 g->Svvv -= (3*g->Svv + g->Sv*yv)*yv;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1310 g->Svvw -= (g->Svv + g->Sv*yv)*yw + 2*g->Svw*yv;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1311
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1312 g->Swwu -= (g->Sww + g->Sw*yw)*yu + 2*g->Suw*yw;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1313 g->Swwv -= (g->Sww + g->Sw*yw)*yv + 2*g->Svw*yw;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1314 g->Swww -= (3*g->Sww + g->Sw*yw)*yw;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1315
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1316 //---- Solve the system --------------------------------------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1317 // uc Suu + vc Suv + wc Suw = (Suuu + Svvu + Swwu) / 2
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1318 // uc Suv + vc Svv + wc Svw = (Suuv + Svvv + Swwv) / 2
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1319 // uc Suw + vc Svw + wc Sww = (Suuw + Svvw + Swww) / 2
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1320 // Note this is symetric, with a positiv diagonal, hence
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1321 // it always have a uniq solution.
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1322 yu = 0.5f * (g->Suuu + g->Svvu + g->Swwu);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1323 yv = 0.5f * (g->Suuv + g->Svvv + g->Swwv);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1324 yw = 0.5f * (g->Suuw + g->Svvw + g->Swww);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1325 delta = g->Suu * (g->Svv * g->Sww - g->Svw * g->Svw)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1326 - g->Suv * (g->Suv * g->Sww - g->Svw * g->Suw)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1327 + g->Suw * (g->Suv * g->Svw - g->Svv * g->Suw);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1328
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1329 uc = (yu * (g->Svv * g->Sww - g->Svw * g->Svw)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1330 - yv * (g->Suv * g->Sww - g->Svw * g->Suw)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1331 + yw * (g->Suv * g->Svw - g->Svv * g->Suw) )/delta;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1332 vc = (g->Suu * ( yv * g->Sww - yw * g->Svw)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1333 - g->Suv * ( yu * g->Sww - yw * g->Suw)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1334 + g->Suw * ( yu * g->Svw - yv * g->Suw) )/delta;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1335 wc = (g->Suu * (g->Svv * yw - g->Svw * yv )
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1336 - g->Suv * (g->Suv * yw - g->Svw * yu )
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1337 + g->Suw * (g->Suv * yv - g->Svv * yu ) )/delta;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1338
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1339 // Back to uncentered coordinates:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1340 // xc = um + uc
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1341 uc = g->Su/g->compass_N + compass_CX_f/32768.0f + uc;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1342 vc = g->Sv/g->compass_N + compass_CY_f/32768.0f + vc;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1343 wc = g->Sw/g->compass_N + compass_CZ_f/32768.0f + wc;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1344
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1345 // Then save the new calibrated center:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1346 compass_CX_f = (short)(32768 * uc);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1347 compass_CY_f = (short)(32768 * vc);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1348 compass_CZ_f = (short)(32768 * wc);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1349 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1350
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1351
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1352 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1353 // compass_calib
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1354 /// @brief the main loop for calibration
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1355 /// output is compass_CX_f, compass_CY_f, compass_CZ_f and g
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1356 /// 160704 removed -4096 limit for LSM303D
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1357 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1358 /// @return always 0
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1359 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1360 int compass_calib_common(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1361 {
410
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1362 SCompassCalib g;
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1363
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1364 // Starts with no calibration at all:
410
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1365 compass_reset_calibration(&g);
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1366 uint32_t tickstart = 0;
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1367 tickstart = HAL_GetTick();
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1368 /* run calibration for one minute */
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1369 while(time_elapsed_ms(tickstart,HAL_GetTick()) < 60000)
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1370 {
410
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1371 while((SPI_Evaluate_RX_Data() == 0) && (time_elapsed_ms(tickstart,HAL_GetTick()) < 60000))
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1372 {
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1373 HAL_Delay(1);
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1374 }
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1375 compass_read();
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1376 acceleration_read();
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1377 compass_calc_roll_pitch_only();
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1378
410
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1379 if((hardwareCompass == compass_generation1 ) //HMC5883L)
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1380 &&((compass_DX_f == -4096) ||
410
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1381 (compass_DY_f == -4096) ||
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1382 (compass_DZ_f == -4096) ))
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1383 {
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1384 if(compass_gain == 0)
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1385 return -1;
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1386 compass_gain--;
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1387 compass_init(1, compass_gain);
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1388 compass_reset_calibration(&g);
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1389 continue;
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1390 }
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1391
410
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1392 copyCompassDataDuringCalibration(compass_DX_f,compass_DY_f,compass_DZ_f);
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1393 compass_add_calibration(&g);
104
22a1094545f3 Tested and alive.
Dmitry Romanov <kitt@bk.ru>
parents: 70
diff changeset
1394 }
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1395
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1396 compass_solve_calibration(&g);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1397
410
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1398 tfull32 dataBlock[4];
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1399 dataBlock[0].Word16.low16 = compass_CX_f;
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1400 dataBlock[0].Word16.hi16 = compass_CY_f;
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1401 dataBlock[1].Word16.low16 = compass_CZ_f;
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1402 dataBlock[1].Word16.hi16 = 0xFFFF;
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1403 dataBlock[2].Full32 = 0x7FFFFFFF;
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1404 dataBlock[3].Full32 = 0x7FFFFFFF;
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1405 BFA_writeDataBlock((uint32_t *)dataBlock);
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1406
410
f9458e979154 Bugfix display compass calibration frozen:
ideenmodellierer
parents: 358
diff changeset
1407 return 0;
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1408 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1409