annotate Small_CPU/Src/compass.c @ 179:a492a7a457b7 Cleanup_Compass_Wireless

Starting 'Cleanup_Compass_Wireless' branch
author ideenmodellierer
date Wed, 13 Mar 2019 20:53:05 +0100
parents 14e4c83a7559
children 9ecc2e60418d
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 #include "compass_LSM303DLHC.h"
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
36
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
37 #include "i2c.h"
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
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
42 #define MODE_LSM303DLHC
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
43 #define TEST_IF_HMC5883L
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
44 //#define COMPASS_DEACTIVATE
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
45
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
46 /// split byte to bits
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
47 typedef struct{
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
48 uint8_t bit0:1; ///< split byte to bits
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
49 uint8_t bit1:1; ///< split byte to bits
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
50 uint8_t bit2:1; ///< split byte to bits
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
51 uint8_t bit3:1; ///< split byte to bits
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
52 uint8_t bit4:1; ///< split byte to bits
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
53 uint8_t bit5:1; ///< split byte to bits
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
54 uint8_t bit6:1; ///< split byte to bits
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
55 uint8_t bit7:1; ///< split byte to bits
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
56 } ubit8_t;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
57
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
58
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
59 /// split byte to bits
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
60 typedef union{
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
61 ubit8_t ub; ///< split byte to bits
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
62 uint8_t uw; ///< split byte to bits
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
63 } bit8_Type;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
64
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
65
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
66 /// split word to 2 bytes
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
67 typedef struct{
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
68 uint8_t low; ///< split word to 2 bytes
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
69 uint8_t hi; ///< split word to 2 bytes
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
70 } two_byte;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
71
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
72
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
73 /// split word to 2 bytes
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
74 typedef union{
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
75 two_byte Byte; ///< split word to 2 bytes
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
76 uint16_t Word; ///< split word to 2 bytes
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
77 } tword;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
78
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
79
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
80 /// split signed word to 2 bytes
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
81 typedef union{
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
82 two_byte Byte; ///< split signed word to 2 bytes
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
83 int16_t Word; ///< split signed word to 2 bytes
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
84 } signed_tword;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
85
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
86
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
87 /// split full32 to 2 words
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
88 typedef struct{
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
89 uint16_t low16; ///< split word to 2 bytes
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
90 uint16_t hi16; ///< split word to 2 bytes
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
91 } two_word;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
92
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
93 typedef union{
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
94 two_word Word16; ///< split word to 2 bytes
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
95 uint32_t Full32; ///< split word to 2 bytes
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
96 } tfull32;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
97
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
98
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
99 /// crazy compass calibration stuff
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
100 typedef struct
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
101 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
102 unsigned short int compass_N;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
103 float Su, Sv, Sw;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
104 float Suu, Svv, Sww, Suv, Suw, Svw;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
105 float Suuu, Svvv, Swww;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
106 float Suuv, Suuw, Svvu, Svvw, Swwu, Swwv;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
107 } SCompassCalib;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
108
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
109
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
110 #define Q_PI (18000)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
111 #define Q_PIO2 (9000)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
112
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
113 #define HMC5883L (1) ///< id used with hardwareCompass
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
114 #define LSM303D (2) ///< id used with hardwareCompass
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
115 #define LSM303DLHC (3) ///< id used with hardwareCompass
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
116 #define COMPASS_NOT_RECOGNIZED (4) ///< id used with hardwareCompass
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
117
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
118
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
119 //////////////////////////////////////////////////////////////////////////////
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
120 // fifth order of polynomial approximation of atan(), giving 0.05 deg max error
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
121 //
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
122 #define K1 (5701) // Needs K1/2**16
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
123 #define K2 (1645) // Needs K2/2**48 WAS NEGATIV
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
124 #define K3 ( 446) // Needs K3/2**80
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
125
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
126 const float PI = 3.14159265; ///< pi, used in compass_calc()
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
127
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
128 typedef short int Int16;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
129 typedef signed char Int8;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
130 typedef Int16 Angle;
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 /// The (filtered) components of the magnetometer sensor
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
134 int16_t compass_DX_f; ///< output from sensor
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
135 int16_t compass_DY_f; ///< output from sensor
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
136 int16_t compass_DZ_f; ///< output from sensor
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 /// Found soft-iron calibration values, deduced from already filtered values
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
140 int16_t compass_CX_f; ///< calibration value
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
141 int16_t compass_CY_f; ///< calibration value
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
142 int16_t compass_CZ_f; ///< calibration value
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 (filtered) components of the accelerometer sensor
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
146 int16_t accel_DX_f; ///< output from sensor
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
147 int16_t accel_DY_f; ///< output from sensor
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
148 int16_t accel_DZ_f; ///< output from sensor
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 /// The compass result values
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
152 float compass_heading; ///< the final result calculated in compass_calc()
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
153 float compass_roll; ///< the final result calculated in compass_calc()
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
154 float compass_pitch; ///< the final result calculated in compass_calc()
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
155
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
156
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
157 uint8_t compass_gain; ///< 7 on start, can be reduced during calibration
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
158
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
159 uint8_t hardwareCompass = 0; ///< either HMC5883L or LSM303D or not defined yet ( = 0 )
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
160
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
161 /// LSM303D variables
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
162 uint8_t magDataBuffer[6]; ///< here raw data from LSM303D is stored, can be local
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
163 uint8_t accDataBuffer[6]; ///< here raw data from LSM303D is stored, can be local
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
164
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
165 //uint16_t velMag = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
166 //uint16_t velAcc = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
167
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
168 //uint16_t magODR[] = {31,62,125,250,500,1000,2000};
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
169 //uint16_t accODR[] = {0,31,62,125,250,500,1000,2000,4000,8000,16000};
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
170 //uint8_t fastest = 10; //no sensor is the fastest
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
171 //uint8_t datas1 = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
172 //uint8_t zoffFlag = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
173 //uint8_t sendFlag = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
174
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
175
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
176 // all by pixhawk code:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
177
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
178 // struct accel_scale _accel_scale;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
179 unsigned _accel_range_m_s2;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
180 float _accel_range_scale;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
181 unsigned _accel_samplerate;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
182 unsigned _accel_onchip_filter_bandwith;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
183
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
184 // struct mag_scale _mag_scale;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
185 unsigned _mag_range_ga;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
186 float _mag_range_scale;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
187 unsigned _mag_samplerate;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
188
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
189 // default scale factors
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
190 float _accel_scale_x_offset = 0.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
191 float _accel_scale_x_scale = 1.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
192 float _accel_scale_y_offset = 0.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
193 float _accel_scale_y_scale = 1.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
194 float _accel_scale_z_offset = 0.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
195 float _accel_scale_z_scale = 1.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
196
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
197 float _mag_scale_x_offset = 0.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
198 float _mag_scale_x_scale = 1.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
199 float _mag_scale_y_offset = 0.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
200 float _mag_scale_y_scale = 1.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
201 float _mag_scale_z_offset = 0.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
202 float _mag_scale_z_scale = 1.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
203
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
204
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
205 /* External function prototypes ----------------------------------------------*/
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
206
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
207 extern void copyCompassDataDuringCalibration(int16_t dx, int16_t dy, int16_t dz);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
208
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
209 /* Private function prototypes -----------------------------------------------*/
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
210
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
211 void compass_reset_calibration(SCompassCalib *g);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
212 void compass_add_calibration(SCompassCalib *g);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
213 void compass_solve_calibration(SCompassCalib *g);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
214
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
215 void compass_init_HMC5883L(uint8_t fast, uint8_t gain);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
216 void compass_sleep_HMC5883L(void);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
217 void compass_read_HMC5883L(void);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
218
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
219 void accelerator_init_MMA8452Q(void);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
220 void accelerator_sleep_MMA8452Q(void);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
221 void acceleration_read_MMA8452Q(void);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
222
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
223 void compass_init_LSM303D(uint8_t fast, uint8_t gain);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
224 void compass_sleep_LSM303D(void);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
225 void compass_read_LSM303D(void);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
226 void acceleration_read_LSM303D(void);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
227
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
228 void compass_init_LSM303DLHC(uint8_t fast, uint8_t gain);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
229 void compass_sleep_LSM303DLHC(void);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
230 void compass_read_LSM303DLHC(void);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
231 void acceleration_read_LSM303DLHC(void);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
232
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
233
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
234 int LSM303D_accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
235 int compass_calib_common(void);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
236
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
237 void compass_calc_roll_pitch_only(void);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
238
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
239 void rotate_mag_3f(float *x, float *y, float *z);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
240 void rotate_accel_3f(float *x, float *y, float *z);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
241
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
242
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
243 /* Exported functions --------------------------------------------------------*/
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
244
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
245
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
246 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
247 // compass_init
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
248 /// @brief This might be called several times with different gain values during calibration
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
249 /// On first call it figures out which hardware is integrated
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
250 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
251 /// @param gain: 7 is max gain, compass_calib() might reduce it
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
252 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
253
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
254 uint8_t testCompassTypeDebug = 0xFF;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
255
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
256 void compass_init(uint8_t fast, uint8_t gain)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
257 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
258 // quick off
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
259 #ifdef COMPASS_DEACTIVATE
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
260 hardwareCompass = COMPASS_NOT_RECOGNIZED;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
261 #endif
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
262
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
263 // don't call again with fast, gain in calib mode etc.
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
264 // if unknown
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
265 if(hardwareCompass == COMPASS_NOT_RECOGNIZED)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
266 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
267 return;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
268 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
269
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
270 // old code but without else
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
271 if(hardwareCompass == 0)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
272 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
273 uint8_t data = WHO_AM_I;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
274 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
275 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
276 if(data == WHOIAM_VALUE)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
277 hardwareCompass = LSM303D;
70
6a6116d7b5bb old (and new) compass support
heinrichsweikamp
parents: 38
diff changeset
278 else
6a6116d7b5bb old (and new) compass support
heinrichsweikamp
parents: 38
diff changeset
279 hardwareCompass = HMC5883L;
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
280 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
281
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
282
70
6a6116d7b5bb old (and new) compass support
heinrichsweikamp
parents: 38
diff changeset
283 // k�nnte Probleme mit altem Chip machen
6a6116d7b5bb old (and new) compass support
heinrichsweikamp
parents: 38
diff changeset
284 // beim 303D f�hrt dieser Code dazu, dass WHOIAM_VALUE nicht geschickt wird!!!
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
285
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
286 #ifdef MODE_LSM303DLHC
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
287 HAL_StatusTypeDef resultOfOperation = HAL_TIMEOUT;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
288
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
289 if(hardwareCompass == 0)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
290 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
291 uint8_t data = DLHC_CTRL_REG1_A;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
292 resultOfOperation = I2C_Master_Transmit( DEVICE_ACCELARATOR_303DLHC, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
293 if(resultOfOperation == HAL_OK)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
294 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
295 I2C_Master_Receive( DEVICE_ACCELARATOR_303DLHC, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
296 testCompassTypeDebug = data;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
297 if((data & 0x0f) == 0x07)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
298 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
299 hardwareCompass = LSM303DLHC;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
300 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
301 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
302 else
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
303 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
304 testCompassTypeDebug = 0xEE;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
305 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
306 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
307
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
308 #endif
70
6a6116d7b5bb old (and new) compass support
heinrichsweikamp
parents: 38
diff changeset
309
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
310 if(hardwareCompass == 0)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
311 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
312 uint8_t data = WHO_AM_I;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
313 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
314 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
315 if(data == WHOIAM_VALUE)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
316 hardwareCompass = LSM303D;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
317 else
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
318 hardwareCompass = HMC5883L;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
319 }
70
6a6116d7b5bb old (and new) compass support
heinrichsweikamp
parents: 38
diff changeset
320
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
321 // was in else before !
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
322 if(hardwareCompass == 0)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
323 hardwareCompass = HMC5883L;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
324
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
325 #ifdef TEST_IF_HMC5883L
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
326 HAL_StatusTypeDef resultOfOperationHMC_MMA = HAL_TIMEOUT;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
327
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
328 if(hardwareCompass == HMC5883L)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
329 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
330 uint8_t data = 0x2A; // CTRL_REG1 of DEVICE_ACCELARATOR_MMA8452Q
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
331 resultOfOperationHMC_MMA = I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
332 if(resultOfOperationHMC_MMA == HAL_OK)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
333 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
334 hardwareCompass = HMC5883L; // all fine, keep it
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
335 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
336 else
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
337 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
338 hardwareCompass = COMPASS_NOT_RECOGNIZED;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
339 testCompassTypeDebug = 0xEC;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
340 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
341 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
342 #endif
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
343
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
344
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
345 if(hardwareCompass == LSM303DLHC)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
346 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
347 compass_init_LSM303DLHC(fast, gain);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
348 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
349 else
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
350 if(hardwareCompass == LSM303D)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
351 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
352 compass_init_LSM303D(fast, gain);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
353 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
354 else
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
355 if(hardwareCompass == HMC5883L)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
356 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
357 compass_init_HMC5883L(fast, gain);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
358 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
359
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
360 tfull32 dataBlock[4];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
361 if(BFA_readLastDataBlock((uint32_t *)dataBlock) == BFA_OK)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
362 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
363 compass_CX_f = dataBlock[0].Word16.low16;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
364 compass_CY_f = dataBlock[0].Word16.hi16;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
365 compass_CZ_f = dataBlock[1].Word16.low16;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
366 }
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 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
372 // compass_calib
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
373 /// @brief with onchip_lowpass_filter configuration for accelerometer of LSM303D
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
374 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
375 int compass_calib(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
376 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
377 if(hardwareCompass == LSM303DLHC)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
378 {
70
6a6116d7b5bb old (and new) compass support
heinrichsweikamp
parents: 38
diff changeset
379 return compass_calib_common(); // 170821 zur Zeit kein lowpass filtering gefunden, nur high pass auf dem Register ohne Erkl�rung
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
380 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
381 else
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
382 if(hardwareCompass == LSM303D)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
383 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
384 LSM303D_accel_set_onchip_lowpass_filter_bandwidth(773);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
385 int out = compass_calib_common();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
386 LSM303D_accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
387 return out;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
388 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
389 else
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
390 if(hardwareCompass == HMC5883L)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
391 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
392 return compass_calib_common();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
393 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
394 else
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
395 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
396 return 0; // standard answer of compass_calib_common();
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
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
400 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
401
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
402
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
403 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
404 // compass_sleep
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
405 /// @brief low power mode
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
406 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
407 void compass_sleep(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
408 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
409 if(hardwareCompass == LSM303DLHC)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
410 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
411 compass_sleep_LSM303DLHC();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
412 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
413 else
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
414 if(hardwareCompass == LSM303D)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
415 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
416 compass_sleep_LSM303D();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
417 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
418 else
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
419 if(hardwareCompass == HMC5883L)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
420 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
421 compass_sleep_HMC5883L();
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
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
425
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
426 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
427 // compass_read
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
428 /// @brief reads magnetometer and accelerometer for LSM303D,
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
429 /// otherwise magnetometer only
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
430 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
431 void compass_read(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
432 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
433 if(hardwareCompass == LSM303DLHC)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
434 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
435 compass_read_LSM303DLHC();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
436 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
437 else
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
438 if(hardwareCompass == LSM303D)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
439 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
440 compass_read_LSM303D();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
441 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
442 else
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
443 if(hardwareCompass == HMC5883L)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
444 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
445 compass_read_HMC5883L();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
446 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
447 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
448
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
449
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
450 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
451 // accelerator_init
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
452 /// @brief empty for for LSM303D
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
453 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
454 void accelerator_init(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
455 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
456 // if((hardwareCompass != LSM303D) && (hardwareCompass != LSM303DLHC))
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
457 if(hardwareCompass == HMC5883L)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
458 accelerator_init_MMA8452Q();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
459 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
460
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
461
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
462 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
463 // accelerator_sleep
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
464 /// @brief empty for for LSM303D
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
465 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
466 void accelerator_sleep(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
467 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
468 // if((hardwareCompass != LSM303D) && (hardwareCompass != LSM303DLHC))
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
469 if(hardwareCompass == HMC5883L)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
470 accelerator_sleep_MMA8452Q();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
471 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
472
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
473
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
474 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
475 // acceleration_read
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
476 /// @brief empty for for LSM303D
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
477 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
478 void acceleration_read(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
479 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
480 if(hardwareCompass == LSM303DLHC)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
481 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
482 acceleration_read_LSM303DLHC();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
483 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
484 else
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
485 if(hardwareCompass == LSM303D)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
486 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
487 acceleration_read_LSM303D();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
488 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
489 else
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
490 if(hardwareCompass == HMC5883L)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
491 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
492 acceleration_read_MMA8452Q();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
493 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
494 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
495
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
496
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
497 /* Private functions ---------------------------------------------------------*/
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
498
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
499 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
500 // LSM303D_read_reg
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
501 /// @brief tiny helpers by pixhawk
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
502 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
503 uint8_t LSM303D_read_reg(uint8_t addr)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
504 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
505 uint8_t data;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
506
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
507 I2C_Master_Transmit( DEVICE_COMPASS_303D, &addr, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
508 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
509 return data;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
510 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
511
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
512
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
513 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
514 // LSM303D_write_reg
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
515 /// @brief tiny helpers by pixhawk
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
516 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
517 void LSM303D_write_reg(uint8_t addr, uint8_t value)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
518 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
519 uint8_t data[2];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
520
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
521 /* enable accel*/
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
522 data[0] = addr;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
523 data[1] = value;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
524 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
525 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
526
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 // LSM303D_write_checked_reg
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
530 /// @brief tiny helpers by pixhawk. This runs unchecked at the moment.
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
531 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
532 void LSM303D_write_checked_reg(uint8_t addr, uint8_t value)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
533 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
534 LSM303D_write_reg(addr, value);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
535 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
536
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
537
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
538 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
539 // LSM303D_modify_reg
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
540 /// @brief tiny helpers by pixhawk
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
541 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
542 void LSM303D_modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
543 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
544 uint8_t val;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
545
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
546 val = LSM303D_read_reg(reg);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
547 val &= ~clearbits;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
548 val |= setbits;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
549 LSM303D_write_checked_reg(reg, val);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
550 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
551
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
552
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
553
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
554 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
555 // LSM303DLHC_accelerator_read_req
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
556 /// @brief
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
557 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
558 uint8_t LSM303DLHC_accelerator_read_req(uint8_t addr)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
559 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
560 uint8_t data;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
561
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
562 I2C_Master_Transmit( DEVICE_ACCELARATOR_303DLHC, &addr, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
563 I2C_Master_Receive( DEVICE_ACCELARATOR_303DLHC, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
564 return data;
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 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
569 // LSM303DLHC_accelerator_write_req
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
570 /// @brief
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
571 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
572 void LSM303DLHC_accelerator_write_req(uint8_t addr, uint8_t value)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
573 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
574 uint8_t data[2];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
575
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
576 /* enable accel*/
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
577 data[0] = addr;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
578 data[1] = value;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
579 I2C_Master_Transmit( DEVICE_ACCELARATOR_303DLHC, data, 2);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
580 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
581
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
582 /*
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
583 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
584 // LSM303D_accel_set_range
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
585 /// @brief tiny helpers by pixhawk
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
586 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
587 int LSM303D_accel_set_range(unsigned max_g)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
588 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
589 uint8_t setbits = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
590 uint8_t clearbits = REG2_FULL_SCALE_BITS_A;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
591 float new_scale_g_digit = 0.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
592
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
593 if (max_g == 0) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
594 max_g = 16;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
595 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
596
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
597 if (max_g <= 2) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
598 _accel_range_m_s2 = 2.0f * LSM303D_ONE_G;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
599 setbits |= REG2_FULL_SCALE_2G_A;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
600 new_scale_g_digit = 0.061e-3f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
601
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
602 } else if (max_g <= 4) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
603 _accel_range_m_s2 = 4.0f * LSM303D_ONE_G;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
604 setbits |= REG2_FULL_SCALE_4G_A;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
605 new_scale_g_digit = 0.122e-3f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
606
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
607 } else if (max_g <= 6) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
608 _accel_range_m_s2 = 6.0f * LSM303D_ONE_G;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
609 setbits |= REG2_FULL_SCALE_6G_A;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
610 new_scale_g_digit = 0.183e-3f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
611
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
612 } else if (max_g <= 8) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
613 _accel_range_m_s2 = 8.0f * LSM303D_ONE_G;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
614 setbits |= REG2_FULL_SCALE_8G_A;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
615 new_scale_g_digit = 0.244e-3f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
616
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
617 } else if (max_g <= 16) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
618 _accel_range_m_s2 = 16.0f * LSM303D_ONE_G;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
619 setbits |= REG2_FULL_SCALE_16G_A;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
620 new_scale_g_digit = 0.732e-3f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
621
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
622 } else {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
623 return -1;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
624 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
625
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
626 _accel_range_scale = new_scale_g_digit * LSM303D_ONE_G;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
627
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
628
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
629 LSM303D_modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
630
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
631 return 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
632 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
633 */
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
634 /*
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
635 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
636 // LSM303D_mag_set_range
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
637 /// @brief tiny helpers by pixhawk
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
638 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
639 int LSM303D_mag_set_range(unsigned max_ga)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
640 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
641 uint8_t setbits = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
642 uint8_t clearbits = REG6_FULL_SCALE_BITS_M;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
643 float new_scale_ga_digit = 0.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
644
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
645 if (max_ga == 0) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
646 max_ga = 12;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
647 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
648
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
649 if (max_ga <= 2) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
650 _mag_range_ga = 2;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
651 setbits |= REG6_FULL_SCALE_2GA_M;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
652 new_scale_ga_digit = 0.080e-3f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
653
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
654 } else if (max_ga <= 4) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
655 _mag_range_ga = 4;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
656 setbits |= REG6_FULL_SCALE_4GA_M;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
657 new_scale_ga_digit = 0.160e-3f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
658
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
659 } else if (max_ga <= 8) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
660 _mag_range_ga = 8;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
661 setbits |= REG6_FULL_SCALE_8GA_M;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
662 new_scale_ga_digit = 0.320e-3f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
663
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
664 } else if (max_ga <= 12) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
665 _mag_range_ga = 12;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
666 setbits |= REG6_FULL_SCALE_12GA_M;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
667 new_scale_ga_digit = 0.479e-3f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
668
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
669 } else {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
670 return -1;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
671 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
672
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
673 _mag_range_scale = new_scale_ga_digit;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
674
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
675 LSM303D_modify_reg(ADDR_CTRL_REG6, clearbits, setbits);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
676
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
677 return 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
678 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
679 */
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
680
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
681 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
682 // LSM303D_accel_set_onchip_lowpass_filter_bandwidth
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
683 /// @brief tiny helpers by pixhawk
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
684 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
685 int LSM303D_accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
686 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
687 uint8_t setbits = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
688 uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
689
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
690 if (bandwidth == 0) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
691 bandwidth = 773;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
692 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
693
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
694 if (bandwidth <= 50) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
695 setbits |= REG2_AA_FILTER_BW_50HZ_A;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
696 _accel_onchip_filter_bandwith = 50;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
697
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
698 } else if (bandwidth <= 194) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
699 setbits |= REG2_AA_FILTER_BW_194HZ_A;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
700 _accel_onchip_filter_bandwith = 194;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
701
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
702 } else if (bandwidth <= 362) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
703 setbits |= REG2_AA_FILTER_BW_362HZ_A;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
704 _accel_onchip_filter_bandwith = 362;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
705
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
706 } else if (bandwidth <= 773) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
707 setbits |= REG2_AA_FILTER_BW_773HZ_A;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
708 _accel_onchip_filter_bandwith = 773;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
709
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
710 } else {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
711 return -1;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
712 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
713
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
714 LSM303D_modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
715
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
716 return 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
717 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
718
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
719
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
720 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
721 // LSM303D_accel_set_driver_lowpass_filter
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
722 /// @brief tiny helpers by pixhawk. This one is not used at the moment!
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
723 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
724 int LSM303D_accel_set_driver_lowpass_filter(float samplerate, float bandwidth)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
725 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
726 /*
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
727 _accel_filter_x_set_cutoff_frequency(samplerate, bandwidth);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
728 _accel_filter_y_set_cutoff_frequency(samplerate, bandwidth);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
729 _accel_filter_z_set_cutoff_frequency(samplerate, bandwidth);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
730 */
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
731 return 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
732 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
733
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
734 /* unused 170821
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
735 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
736 // LSM303D_accel_set_samplerate
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
737 /// @brief tiny helpers by pixhawk
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
738 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
739 int LSM303D_accel_set_samplerate(unsigned frequency)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
740 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
741 uint8_t setbits = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
742 uint8_t clearbits = REG1_RATE_BITS_A;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
743
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
744 // if (frequency == 0 || frequency == ACCEL_SAMPLERATE_DEFAULT) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
745 frequency = 1600;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
746 // }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
747
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
748 if (frequency <= 3) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
749 setbits |= REG1_RATE_3_125HZ_A;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
750 _accel_samplerate = 3;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
751
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
752 } else if (frequency <= 6) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
753 setbits |= REG1_RATE_6_25HZ_A;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
754 _accel_samplerate = 6;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
755
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
756 } else if (frequency <= 12) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
757 setbits |= REG1_RATE_12_5HZ_A;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
758 _accel_samplerate = 12;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
759
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
760 } else if (frequency <= 25) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
761 setbits |= REG1_RATE_25HZ_A;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
762 _accel_samplerate = 25;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
763
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
764 } else if (frequency <= 50) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
765 setbits |= REG1_RATE_50HZ_A;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
766 _accel_samplerate = 50;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
767
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
768 } else if (frequency <= 100) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
769 setbits |= REG1_RATE_100HZ_A;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
770 _accel_samplerate = 100;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
771
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
772 } else if (frequency <= 200) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
773 setbits |= REG1_RATE_200HZ_A;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
774 _accel_samplerate = 200;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
775
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
776 } else if (frequency <= 400) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
777 setbits |= REG1_RATE_400HZ_A;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
778 _accel_samplerate = 400;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
779
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
780 } else if (frequency <= 800) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
781 setbits |= REG1_RATE_800HZ_A;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
782 _accel_samplerate = 800;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
783
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
784 } else if (frequency <= 1600) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
785 setbits |= REG1_RATE_1600HZ_A;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
786 _accel_samplerate = 1600;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
787
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
788 } else {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
789 return -1;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
790 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
791
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
792 LSM303D_modify_reg(ADDR_CTRL_REG1, clearbits, setbits);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
793 return 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
794 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
795 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
796 // LSM303D_mag_set_samplerate
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
797 /// @brief tiny helpers by pixhawk
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
798 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
799 int LSM303D_mag_set_samplerate(unsigned frequency)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
800 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
801 uint8_t setbits = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
802 uint8_t clearbits = REG5_RATE_BITS_M;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
803
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
804 if (frequency == 0) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
805 frequency = 100;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
806 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
807
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
808 if (frequency <= 3) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
809 setbits |= REG5_RATE_3_125HZ_M;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
810 _mag_samplerate = 25;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
811
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
812 } else if (frequency <= 6) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
813 setbits |= REG5_RATE_6_25HZ_M;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
814 _mag_samplerate = 25;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
815
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
816 } else if (frequency <= 12) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
817 setbits |= REG5_RATE_12_5HZ_M;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
818 _mag_samplerate = 25;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
819
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
820 } else if (frequency <= 25) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
821 setbits |= REG5_RATE_25HZ_M;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
822 _mag_samplerate = 25;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
823
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
824 } else if (frequency <= 50) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
825 setbits |= REG5_RATE_50HZ_M;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
826 _mag_samplerate = 50;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
827
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
828 } else if (frequency <= 100) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
829 setbits |= REG5_RATE_100HZ_M;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
830 _mag_samplerate = 100;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
831
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
832 } else {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
833 return -1;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
834 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
835
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
836 LSM303D_modify_reg(ADDR_CTRL_REG5, clearbits, setbits);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
837 return 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
838 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
839 */
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
840
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
841
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
842 // rotate_mag_3f: nicht genutzt aber praktisch; rotate_accel_3f wird benutzt
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
843 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
844 // rotate_mag_3f
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
845 /// @brief swap axis in convient way, by hw
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
846 /// @param *x raw input is set to *y input
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
847 /// @param *y raw input is set to -*x input
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
848 /// @param *z raw is not touched
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
849 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
850 void rotate_mag_3f(float *x, float *y, float *z)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
851 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
852 return;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
853 /*
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
854 *x = *x; // HMC: *x = -*y
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
855 *y = *y; // HMC: *y = *x // change 20.04.2016: zuvor *y = -*y
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
856 *z = *z; // HMC: *z = *z
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
857 */
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
858 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
859
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
860
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
861 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
862 // rotate_accel_3f
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
863 /// @brief swap axis in convient way, by hw, same as MMA8452Q
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
864 /// @param *x raw input, output is with sign change
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
865 /// @param *y raw input, output is with sign change
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
866 /// @param *z raw input, output is with sign change
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
867 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
868 void rotate_accel_3f(float *x, float *y, float *z)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
869 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
870 *x = -*x;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
871 *y = -*y;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
872 *z = -*z;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
873 /* tested:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
874 x = x, y =-y, z=-z: does not work with roll
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
875 x = x, y =y, z=-z: does not work with pitch
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
876 x = x, y =y, z=z: does not work at all
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
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
881 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
882 // compass_init_LSM303D by PIXhawk (LSM303D::reset())
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
883 // https://raw.githubusercontent.com/PX4/Firmware/master/src/drivers/lsm303d/lsm303d.cpp
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
884 /// @brief The new ST 303D
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
885 /// This might be called several times with different gain values during calibration
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
886 /// but gain change is not supported at the moment.
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
887 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
888 /// @param gain: 7 is max gain and set with here, compass_calib() might reduce it
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
889 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
890
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
891 //uint8_t testCompassLS303D[11];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
892
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
893 void compass_init_LSM303D(uint8_t fast, uint8_t gain)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
894 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
895 // matthias version 160620
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
896 if(fast == 0)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
897 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
898 LSM303D_write_checked_reg(ADDR_CTRL_REG0, 0x00);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
899 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
900 LSM303D_write_checked_reg(ADDR_CTRL_REG2, 0xC0);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
901 LSM303D_write_checked_reg(ADDR_CTRL_REG3, 0x00);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
902 LSM303D_write_checked_reg(ADDR_CTRL_REG4, 0x00);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
903 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
904 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
905 else
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
906 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
907 LSM303D_write_checked_reg(ADDR_CTRL_REG0, 0x00);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
908 LSM303D_write_checked_reg(ADDR_CTRL_REG1, 0x6F); // 100 Hz
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
909 LSM303D_write_checked_reg(ADDR_CTRL_REG2, 0xC0);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
910 LSM303D_write_checked_reg(ADDR_CTRL_REG3, 0x00);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
911 LSM303D_write_checked_reg(ADDR_CTRL_REG4, 0x00);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
912 LSM303D_write_checked_reg(ADDR_CTRL_REG5, 0x74); // 100 Hz
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
913 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
914 LSM303D_write_checked_reg(ADDR_CTRL_REG6, 0x00);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
915 LSM303D_write_checked_reg(ADDR_CTRL_REG7, 0x00);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
916
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
917 /*
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
918 uint8_t data;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
919 for(int i=0;i<11;i++)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
920 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
921 data = ADDR_INT_THS_L_M + i;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
922 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
923 I2C_Master_Receive( DEVICE_COMPASS_303D, &testCompassLS303D[i], 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
924 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
925 */
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
926
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
927 return;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
928 /*
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
929 LSM303D_accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); // modifies ADDR_CTRL_REG2
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
930 LSM303D_accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); // modifies ADDR_CTRL_REG1
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
931
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
932 LSM303D_mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
933 LSM303D_mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
934 */
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
935
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
936 /*
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
937 // my stuff hw
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
938 // enable accel
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
939 LSM303D_write_checked_reg(ADDR_CTRL_REG1,
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
940 REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
941
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
942 // enable mag
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
943 LSM303D_write_checked_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
944 LSM303D_write_checked_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M | REG5_ENABLE_T);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
945 LSM303D_write_checked_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
946 LSM303D_write_checked_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
947
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
948 LSM303D_accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
949 LSM303D_accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
950 LSM303D_accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
951 //LSM303D_accel_set_onchip_lowpass_filter_bandwidth(773); // factory setting
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
952
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
953 // we setup the anti-alias on-chip filter as 50Hz. We believe
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
954 // this operates in the analog domain, and is critical for
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
955 // anti-aliasing. The 2 pole software filter is designed to
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
956 // operate in conjunction with this on-chip filter
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
957 if(fast)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
958 LSM303D_accel_set_onchip_lowpass_filter_bandwidth(773); // factory setting
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
959 else
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
960 LSM303D_accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
961
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
962
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
963 LSM303D_mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
964 LSM303D_mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
965 */
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
966 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
967
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
968
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
969 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
970 // compass_sleep_LSM303D
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
971 /// @brief The new compass chip, hopefully this works!
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
972 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
973 void compass_sleep_LSM303D(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
974 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
975 LSM303D_write_checked_reg(ADDR_CTRL_REG1, 0x00); // CNTRL1: acceleration sensor Power-down mode
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
976 LSM303D_write_checked_reg(ADDR_CTRL_REG7, 0x02); // CNTRL7: magnetic sensor Power-down mode
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
977 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
978
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
979
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
980 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
981 // acceleration_read_LSM303D
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
982 /// @brief The new LSM303D, code by pixhawk
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
983 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
984 /// output is accel_DX_f, accel_DY_f, accel_DZ_f
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
985 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
986 void acceleration_read_LSM303D(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
987 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
988 uint8_t data;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
989 float xraw_f, yraw_f, zraw_f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
990 float accel_report_x, accel_report_y, accel_report_z;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
991
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
992 memset(accDataBuffer,0,6);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
993
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
994 accel_DX_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
995 accel_DY_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
996 accel_DZ_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
997
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
998 for(int i=0;i<6;i++)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
999 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1000 data = ADDR_OUT_X_L_A + i;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1001 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1002 I2C_Master_Receive( DEVICE_COMPASS_303D, &accDataBuffer[i], 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1003 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1004
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1005 xraw_f = ((float)( (int16_t)((accDataBuffer[1] << 8) | (accDataBuffer[0]))));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1006 yraw_f = ((float)( (int16_t)((accDataBuffer[3] << 8) | (accDataBuffer[2]))));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1007 zraw_f = ((float)( (int16_t)((accDataBuffer[5] << 8) | (accDataBuffer[4]))));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1008
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1009 rotate_accel_3f(&xraw_f, &yraw_f, &zraw_f);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1010
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1011 // mh
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1012 accel_report_x = xraw_f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1013 accel_report_y = yraw_f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1014 accel_report_z = zraw_f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1015
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1016 // my stuff
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1017 /*
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1018 accel_report_x = ((xraw_f * _accel_range_scale) - _accel_scale_x_offset) * _accel_scale_x_scale;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1019 accel_report_y = ((yraw_f * _accel_range_scale) - _accel_scale_y_offset) * _accel_scale_y_scale;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1020 accel_report_z = ((zraw_f * _accel_range_scale) - _accel_scale_z_offset) * _accel_scale_z_scale;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1021 */
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1022 accel_DX_f = ((int16_t)(accel_report_x));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1023 accel_DY_f = ((int16_t)(accel_report_y));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1024 accel_DZ_f = ((int16_t)(accel_report_z));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1025 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1026 /* special code after accel_report_z = ...
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1027 * prior to output
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1028 // we have logs where the accelerometers get stuck at a fixed
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1029 // large value. We want to detect this and mark the sensor as
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1030 // being faulty
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1031
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1032 if (fabsf(_last_accel[0] - x_in_new) < 0.001f &&
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1033 fabsf(_last_accel[1] - y_in_new) < 0.001f &&
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1034 fabsf(_last_accel[2] - z_in_new) < 0.001f &&
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1035 fabsf(x_in_new) > 20 &&
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1036 fabsf(y_in_new) > 20 &&
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1037 fabsf(z_in_new) > 20) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1038 _constant_accel_count += 1;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1039
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1040 } else {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1041 _constant_accel_count = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1042 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1043
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1044 if (_constant_accel_count > 100) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1045 // we've had 100 constant accel readings with large
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1046 // values. The sensor is almost certainly dead. We
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1047 // will raise the error_count so that the top level
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1048 // flight code will know to avoid this sensor, but
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1049 // we'll still give the data so that it can be logged
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1050 // and viewed
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1051 perf_count(_bad_values);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1052 _constant_accel_count = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1053 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1054
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1055 _last_accel[0] = x_in_new;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1056 _last_accel[1] = y_in_new;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1057 _last_accel[2] = z_in_new;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1058
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1059 accel_report.x = _accel_filter_x.apply(x_in_new);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1060 accel_report.y = _accel_filter_y.apply(y_in_new);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1061 accel_report.z = _accel_filter_z.apply(z_in_new);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1062
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1063 math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1064 math::Vector<3> aval_integrated;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1065
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1066 bool accel_notify = _accel_int.put(accel_report.timestamp, aval, aval_integrated, accel_report.integral_dt);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1067 accel_report.x_integral = aval_integrated(0);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1068 accel_report.y_integral = aval_integrated(1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1069 accel_report.z_integral = aval_integrated(2);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1070 */
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1071
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1072
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1073 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1074 // compass_read_LSM303D
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1075 /// @brief The new LSM303D, code by pixhawk
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1076 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1077 /// output is compass_DX_f, compass_DY_f, compass_DZ_f
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1078 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1079 void compass_read_LSM303D(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1080 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1081 uint8_t data;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1082 // float xraw_f, yraw_f, zraw_f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1083 // float mag_report_x, mag_report_y, mag_report_z;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1084
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1085 memset(magDataBuffer,0,6);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1086
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1087 compass_DX_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1088 compass_DY_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1089 compass_DZ_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1090
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1091 for(int i=0;i<6;i++)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1092 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1093 data = ADDR_OUT_X_L_M + i;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1094 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1095 I2C_Master_Receive( DEVICE_COMPASS_303D, &magDataBuffer[i], 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1096 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1097
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1098 // mh 160620 flip x and y if flip display
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1099 compass_DX_f = (((int16_t)((magDataBuffer[1] << 8) | (magDataBuffer[0]))));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1100 compass_DY_f = (((int16_t)((magDataBuffer[3] << 8) | (magDataBuffer[2]))));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1101 compass_DZ_f = (((int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4]))));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1102 // no rotation
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1103 return;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1104 /*
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1105 // my stuff
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1106 compass_DX_f = (((int16_t)((magDataBuffer[1] << 8) | (magDataBuffer[0]))) / 10) - 200;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1107 compass_DY_f = (((int16_t)((magDataBuffer[3] << 8) | (magDataBuffer[2]))) / 10) - 200;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1108 compass_DZ_f = (((int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4]))) / 10) - 200;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1109 */
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1110 // old
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1111 /*
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1112 xraw_f = ((float)( (int16_t)((magDataBuffer[1] << 8) | (magDataBuffer[0]))));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1113 yraw_f = ((float)( (int16_t)((magDataBuffer[3] << 8) | (magDataBuffer[2]))));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1114 zraw_f = ((float)( (int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4]))));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1115
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1116 rotate_mag_3f(&xraw_f, &yraw_f, &zraw_f);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1117
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1118 compass_DX_f = (int16_t)((xraw_f * 0.1f) - 200.0f);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1119 compass_DY_f = (int16_t)((yraw_f * 0.1f) - 200.0f);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1120 compass_DZ_f = (int16_t)((zraw_f * 0.1f) - 200.0f);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1121 */
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1122 /*
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1123 mag_report_x = ((xraw_f * _mag_range_scale) - _mag_scale_x_offset) * _mag_scale_x_scale;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1124 mag_report_y = ((yraw_f * _mag_range_scale) - _mag_scale_y_offset) * _mag_scale_y_scale;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1125 mag_report_z = ((zraw_f * _mag_range_scale) - _mag_scale_z_offset) * _mag_scale_z_scale;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1126
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1127 compass_DX_f = (int16_t)(mag_report_x * 1000.0f); // 1000.0 is just a wild guess by hw
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1128 compass_DY_f = (int16_t)(mag_report_y * 1000.0f);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1129 compass_DZ_f = (int16_t)(mag_report_z * 1000.0f);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1130 */
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1131 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1132
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1133
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1134 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1135 // compass_init_LSM303DLHC
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1136 /// @brief The new ST 303DLHC 2017/2018
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1137 /// This might be called several times with different gain values during calibration
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1138 /// but gain change is not supported at the moment.
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1139 /// parts from KOMPASS LSM303DLH-compass-app-note.pdf
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1140 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1141 /// @param gain:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1142 /// @param fast:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1143 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1144
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1145
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1146
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1147 void compass_init_LSM303DLHC(uint8_t fast, uint8_t gain)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1148 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1149 // acceleration
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1150 // todo : BDU an (wie 303D) und high res, beides in REG4
70
6a6116d7b5bb old (and new) compass support
heinrichsweikamp
parents: 38
diff changeset
1151 //LSM303D_write_checked_reg(DLHC_CTRL_REG2_A,0x00); // 0x00 default, hier k�nnte filter sein 0x8?80, cutoff freq. not beschrieben
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1152
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1153 if(fast == 0)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1154 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1155 LSM303DLHC_accelerator_write_req(DLHC_CTRL_REG1_A, 0x27); // 10 hz
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1156 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1157 else
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1158 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1159 LSM303DLHC_accelerator_write_req(DLHC_CTRL_REG1_A, 0x57); // 100 hz
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1160 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1161 // LSM303D_write_checked_reg(DLHC_CTRL_REG4_A, 0x88); // 0x88: BDU + HighRes, BDU ist doof!
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1162 LSM303D_write_checked_reg(DLHC_CTRL_REG4_A, 0x00); // 0x00 little-endian, ist's immer
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1163 // LSM303D_write_checked_reg(DLHC_CTRL_REG4_A, 0x08); // 0x08: HighRes
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1164 //LSM303D_write_checked_reg(DLHC_CTRL_REG4_A, 0x80); //
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1165
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1166
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1167 // compass
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1168 LSM303D_write_checked_reg(DLHC_CRA_REG_M,0x10); // 15 Hz
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1169
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1170 if(fast == 0)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1171 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1172 LSM303D_write_checked_reg(DLHC_CRA_REG_M,0x10); // 15 Hz
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1173 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1174 else
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1175 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1176 LSM303D_write_checked_reg(DLHC_CRA_REG_M,0x18); // 75 Hz
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1177 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1178 LSM303D_write_checked_reg(DLHC_CRB_REG_M,0x20); // 0x60: 2.5 Gauss ,0x40: +/-1.9 Gauss,0x20: +/-1.3 Gauss
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1179 LSM303D_write_checked_reg(DLHC_MR_REG_M,0x00); //continuous conversation
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
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1183 return;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1184
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1185
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1186 // LSM303D_write_checked_reg(,);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1187 // LSM303D_write_checked_reg(DLHC_CTRL_REG1_A, 0x27); // 0x27 = acc. normal mode with ODR 50Hz - passt nicht mit datenblatt!!
70
6a6116d7b5bb old (and new) compass support
heinrichsweikamp
parents: 38
diff changeset
1188 // LSM303D_write_checked_reg(DLHC_CTRL_REG4_A, 0x40); // 0x40 = full scale range �2 gauss in continuous data update mode and change the little-endian to a big-endian structure.
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1189
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1190 if(fast == 0)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1191 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1192 LSM303DLHC_accelerator_write_req(DLHC_CTRL_REG1_A, 0x27); // 0x27 = acc. normal mode, all axes, with ODR 10HZ laut LSM303DLHC, page 25/42
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1193 //
70
6a6116d7b5bb old (and new) compass support
heinrichsweikamp
parents: 38
diff changeset
1194 //LSM303D_write_checked_reg(DLHC_CTRL_REG2_A,0x00); // 0x00 default, hier k�nnte filter sein 0x8?80, cutoff freq. not beschrieben
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1195 //LSM303D_write_checked_reg(DLHC_CTRL_REG3_A,0x00); // 0x00 default
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1196 //
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1197 LSM303DLHC_accelerator_write_req(DLHC_CTRL_REG4_A, 0x00); // 0x00 = ich glaube little-endian ist gut
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1198 // LSM303D_write_checked_reg(DLHC_CTRL_REG4_A, 0x40); // 0x00 = ich glaube little-endian ist gut
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1199 //
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1200 //LSM303D_write_checked_reg(DLHC_CTRL_REG5_A,0x00); // 0x00 default
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1201 //LSM303D_write_checked_reg(DLHC_CTRL_REG6_A,0x00); // 0x00 default
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1202 // magnetic sensor
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1203 LSM303D_write_checked_reg(DLHC_CRA_REG_M,0x10); // 15 Hz
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1204 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1205 else
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1206 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1207 LSM303DLHC_accelerator_write_req(DLHC_CTRL_REG1_A, 0x57); // 0x57 = acc. normal mode, all axes, with ODR 100HZ, LSM303DLHC, page 25/42
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1208 //
70
6a6116d7b5bb old (and new) compass support
heinrichsweikamp
parents: 38
diff changeset
1209 //LSM303D_write_checked_reg(DLHC_CTRL_REG2_A,0x00); // 0x00 default, hier k�nnte filter sein 0x8?80, cutoff freq. not beschrieben
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1210 //LSM303D_write_checked_reg(DLHC_CTRL_REG3_A,0x00); // 0x00 default
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1211 //
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1212 LSM303DLHC_accelerator_write_req(DLHC_CTRL_REG4_A, 0x00); // 0x00 = ich glaube little-endian ist gut
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1213 // LSM303D_write_checked_reg(DLHC_CTRL_REG4_A, 0x40); // 0x00 = ich glaube little-endian ist gut
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1214 //
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1215 //LSM303D_write_checked_reg(DLHC_CTRL_REG5_A,0x00); // 0x00 default
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1216 //LSM303D_write_checked_reg(DLHC_CTRL_REG6_A,0x00); // 0x00 default
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1217 // magnetic sensor
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1218 LSM303D_write_checked_reg(DLHC_CRA_REG_M,0x18); // 75 Hz
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1219 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1220 LSM303D_write_checked_reg(DLHC_CRB_REG_M,0x02); // +/-1.9 Gauss
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1221 LSM303D_write_checked_reg(DLHC_MR_REG_M,0x00); //continuous conversation
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1222
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1223
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1224 /*
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1225 // matthias version 160620
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1226 if(fast == 0)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1227 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1228 LSM303D_write_checked_reg(ADDR_CTRL_REG0, 0x00);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1229 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
1230 LSM303D_write_checked_reg(ADDR_CTRL_REG2, 0xC0); // anti alias 50 Hz (minimum)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1231 LSM303D_write_checked_reg(ADDR_CTRL_REG3, 0x00);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1232 LSM303D_write_checked_reg(ADDR_CTRL_REG4, 0x00);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1233 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
1234 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1235 else
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1236 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1237 LSM303D_write_checked_reg(ADDR_CTRL_REG0, 0x00);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1238 LSM303D_write_checked_reg(ADDR_CTRL_REG1, 0x6F); // 100 Hz
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1239 LSM303D_write_checked_reg(ADDR_CTRL_REG2, 0xC0);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1240 LSM303D_write_checked_reg(ADDR_CTRL_REG3, 0x00);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1241 LSM303D_write_checked_reg(ADDR_CTRL_REG4, 0x00);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1242 LSM303D_write_checked_reg(ADDR_CTRL_REG5, 0x74); // 100 Hz
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1243 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1244 LSM303D_write_checked_reg(ADDR_CTRL_REG6, 0x00);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1245 LSM303D_write_checked_reg(ADDR_CTRL_REG7, 0x00);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1246 */
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1247 return;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1248 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1249
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1250 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1251 // compass_sleep_LSM303DLHC
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1252 /// @brief The new 2017/2018 compass chip.
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1253 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1254 void compass_sleep_LSM303DLHC(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1255 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1256 LSM303DLHC_accelerator_write_req(DLHC_CTRL_REG1_A, 0x07); // CTRL_REG1_A: linear acceleration Power-down mode
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1257 LSM303D_write_checked_reg(DLHC_MR_REG_M, 0x02); // MR_REG_M: magnetic sensor Power-down mode
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1258 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1259
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1260
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1261 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1262 // compass_read_LSM303DLHC
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1263 /// @brief The new 2017/2018 compass chip.
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1264 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1265 void compass_read_LSM303DLHC(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1266 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1267 uint8_t data;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1268
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1269 memset(magDataBuffer,0,6);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1270
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1271 compass_DX_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1272 compass_DY_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1273 compass_DZ_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1274
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1275 for(int i=0;i<6;i++)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1276 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1277 data = DLHC_OUT_X_L_M + i;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1278 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1279 I2C_Master_Receive( DEVICE_COMPASS_303D, &magDataBuffer[i], 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1280 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1281
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1282 // 303DLHC new order
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1283 compass_DX_f = (((int16_t)((magDataBuffer[0] << 8) | (magDataBuffer[1]))));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1284 compass_DZ_f = (((int16_t)((magDataBuffer[2] << 8) | (magDataBuffer[3]))));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1285 compass_DY_f = (((int16_t)((magDataBuffer[4] << 8) | (magDataBuffer[5]))));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1286
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1287 // no rotation, otherwise see compass_read_LSM303D()
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1288 return;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1289 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1290
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1291
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1292 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1293 // acceleration_read_LSM303DLHC
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1294 /// @brief The new 2017/2018 compass chip.
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1295 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1296 void acceleration_read_LSM303DLHC(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1297 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1298 uint8_t data;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1299 float xraw_f, yraw_f, zraw_f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1300 float accel_report_x, accel_report_y, accel_report_z;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1301
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1302 memset(accDataBuffer,0,6);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1303
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1304 accel_DX_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1305 accel_DY_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1306 accel_DZ_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1307
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1308 for(int i=0;i<6;i++)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1309 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1310 data = DLHC_OUT_X_L_A + i;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1311 I2C_Master_Transmit( DEVICE_ACCELARATOR_303DLHC, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1312 I2C_Master_Receive( DEVICE_ACCELARATOR_303DLHC, &accDataBuffer[i], 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1313 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1314
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1315 xraw_f = ((float)( (int16_t)((accDataBuffer[1] << 8) | (accDataBuffer[0]))));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1316 yraw_f = ((float)( (int16_t)((accDataBuffer[3] << 8) | (accDataBuffer[2]))));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1317 zraw_f = ((float)( (int16_t)((accDataBuffer[5] << 8) | (accDataBuffer[4]))));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1318
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1319 rotate_accel_3f(&xraw_f, &yraw_f, &zraw_f);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1320
70
6a6116d7b5bb old (and new) compass support
heinrichsweikamp
parents: 38
diff changeset
1321 // mh f�r 303D
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1322 accel_report_x = xraw_f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1323 accel_report_y = yraw_f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1324 accel_report_z = zraw_f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1325
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1326 accel_DX_f = ((int16_t)(accel_report_x));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1327 accel_DY_f = ((int16_t)(accel_report_y));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1328 accel_DZ_f = ((int16_t)(accel_report_z));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1329 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1330
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1331
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1332 // --------------------------------------------------------------------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1333 // ----------EARLIER COMPONENTS ---------------------------------------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1334 // --------------------------------------------------------------------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1335
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1336 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1337 // compass_init_HMC5883L
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1338 /// @brief The horrible Honeywell compass chip
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1339 /// This might be called several times during calibration
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1340 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1341 /// @param fast: 1 is fast mode, 0 is normal mode
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1342 /// @param gain: 7 is max gain and set with here, compass_calib() might reduce it
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1343 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1344 void compass_init_HMC5883L(uint8_t fast, uint8_t gain)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1345 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1346 uint8_t write_buffer[4];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1347
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1348 compass_gain = gain;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1349 uint16_t length = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1350 write_buffer[0] = 0x00; // 00 = config Register A
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1351
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1352 if( fast )
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1353 write_buffer[1] = 0x38; // 0b 0011 1000; // ConfigA: 75Hz, 2 Samples averaged
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1354 else
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1355 write_buffer[1] = 0x68; // 0b 0110 1000; // ConfigA: 3Hz, 8 Samples averaged
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1356
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1357 switch(gain)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1358 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1359 case 7:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1360 write_buffer[2] = 0xE0; //0b 1110 0000; // ConfigB: gain
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1361 break;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1362 case 6:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1363 write_buffer[2] = 0xC0; //0b 1100 0000; // ConfigB: gain
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1364 break;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1365 case 5:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1366 write_buffer[2] = 0xA0; //0b 1010 0000; // ConfigB: gain
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1367 break;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1368 case 4:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1369 write_buffer[2] = 0x80; //0b 1000 0000; // ConfigB: gain
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1370 break;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1371 case 3:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1372 write_buffer[2] = 0x60; //0b 0110 0000; // ConfigB: gain
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1373 break;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1374 case 2:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1375 write_buffer[2] = 0x40; //0b 01000 0000; // ConfigB: gain
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1376 break;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1377 case 1:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1378 write_buffer[2] = 0x20; //0b 00100 0000; // ConfigB: gain
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1379 break;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1380 case 0:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1381 write_buffer[2] = 0x00; //0b 00000 0000; // ConfigB: gain
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1382 break;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1383 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1384 write_buffer[3] = 0x00; // Mode: continuous mode
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1385 length = 4;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1386 //hmc_twi_write(0);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1387 I2C_Master_Transmit( DEVICE_COMPASS_HMC5883L, write_buffer, length);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1388 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1389
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1390
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1391
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1392 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1393 // compass_sleep_HMC5883L
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1394 /// @brief Power-down mode for Honeywell compass chip
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1395 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1396 void compass_sleep_HMC5883L(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1397 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1398 uint8_t write_buffer[4];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1399 uint16_t length = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1400
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1401 write_buffer[0] = 0x00; // 00 = config Register A
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1402 write_buffer[1] = 0x68; // 0b 0110 1000; // ConfigA
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1403 write_buffer[2] = 0x20; // 0b 0010 0000; // ConfigB
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1404 write_buffer[3] = 0x02; // 0b 0000 0010; // Idle Mode
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1405 length = 4;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1406 I2C_Master_Transmit( DEVICE_COMPASS_HMC5883L, write_buffer, length);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1407 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1408
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1409
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1410 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1411 // accelerator_init_MMA8452Q
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1412 /// @brief Power-down mode for acceleration chip used in combination with Honeywell compass
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1413 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1414 void accelerator_init_MMA8452Q(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1415 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1416 uint8_t write_buffer[4];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1417 uint16_t length = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1418 //HAL_Delay(1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1419 //return;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1420 write_buffer[0] = 0x0E; // XYZ_DATA_CFG
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1421 write_buffer[1] = 0x00;//0b00000000; // High pass Filter=0 , +/- 2g range
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1422 length = 2;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1423 I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, write_buffer, length);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1424 //HAL_Delay(1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1425 write_buffer[0] = 0x2A; // CTRL_REG1
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1426 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
1427 write_buffer[2] = 0x02; //0b00000010; // CTRL_REG2: High Res in Active mode
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1428 length = 3;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1429 I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, write_buffer, length);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1430
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1431 //HAL_Delay(1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1432 //hw_delay_us(100);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1433 write_buffer[0] = 0x2A; // CTRL_REG1
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1434 write_buffer[1] = 0x35; //0b00110101; // CTRL_REG1: ... Active Mode
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1435 length = 2;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1436 I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, write_buffer, length);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1437 /*
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1438 HAL_Delay(6);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1439 compass_read();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1440 HAL_Delay(1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1441 acceleration_read();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1442
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1443 compass_calc();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1444 */
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1445 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1446
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1447
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1448 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1449 // accelerator_sleep_MMA8452Q
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1450 /// @brief compass_sleep_HMC5883L
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1451 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1452 void accelerator_sleep_MMA8452Q(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1453 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1454 uint16_t length = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1455 uint8_t write_buffer[4];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1456
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1457 write_buffer[0] = 0x2A; // CTRL_REG1
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1458 write_buffer[1] = 0x00; //0b00000000; // CTRL_REG1: Standby Mode
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1459 length = 2;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1460 I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, write_buffer, length);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1461 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1462
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1463
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1464 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1465 // compass_read_HMC5883L
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1466 /// @brief The new ST 303D - get ALL data and store in static variables
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1467 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1468 /// output is compass_DX_f, compass_DY_f, compass_DZ_f
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1469 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1470 void compass_read_HMC5883L(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1471 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1472 uint8_t buffer[20];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1473 compass_DX_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1474 compass_DY_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1475 compass_DZ_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1476 uint8_t length = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1477 uint8_t read_buffer[6];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1478 signed_tword data;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1479 for(int i = 0; i<6;i++)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1480 read_buffer[i] = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1481 buffer[0] = 0x03; // 03 = Data Output X MSB Register
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1482 length = 1;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1483 I2C_Master_Transmit( DEVICE_COMPASS_HMC5883L, buffer, length);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1484 length = 6;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1485 I2C_Master_Receive( DEVICE_COMPASS_HMC5883L, read_buffer, length);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1486
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1487
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1488 data.Byte.hi = read_buffer[0];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1489 data.Byte.low = read_buffer[1];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1490 //Y = Z
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1491 compass_DY_f = - data.Word;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1492
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1493 data.Byte.hi = read_buffer[2];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1494 data.Byte.low = read_buffer[3];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1495 compass_DZ_f = data.Word;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1496
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1497 data.Byte.hi = read_buffer[4];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1498 data.Byte.low = read_buffer[5];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1499 //X = -Y
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1500 compass_DX_f = data.Word;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1501 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1502
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1503
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1504 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1505 // acceleration_read_MMA8452Q
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1506 /// @brief The old MMA8452Q used with the Honeywell compass
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1507 /// get the acceleration data and store in static variables
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1508 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1509 /// output is accel_DX_f, accel_DY_f, accel_DZ_f
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1510 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1511 void acceleration_read_MMA8452Q(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1512 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1513 uint8_t buffer[20];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1514 accel_DX_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1515 accel_DY_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1516 accel_DZ_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1517 uint8_t length = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1518 // bit8_Type status ;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1519 uint8_t read_buffer[7];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1520 signed_tword data;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1521 for(int i = 0; i<6;i++)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1522 read_buffer[i] = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1523 buffer[0] = 0x00; // 03 = Data Output X MSB Register
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1524 length = 1;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1525 I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, buffer, length);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1526 length = 7;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1527 I2C_Master_Receive( DEVICE_ACCELARATOR_MMA8452Q, read_buffer, length);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1528
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1529 // status.uw = read_buffer[0];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1530 data.Byte.hi = read_buffer[1];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1531 data.Byte.low = read_buffer[2];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1532 accel_DX_f =data.Word/16;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1533 data.Byte.hi = read_buffer[3];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1534 data.Byte.low = read_buffer[4];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1535 accel_DY_f =data.Word/16;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1536 data.Byte.hi = read_buffer[5];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1537 data.Byte.low = read_buffer[6];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1538 accel_DZ_f =data.Word/16;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1539
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1540 accel_DX_f *= -1;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1541 accel_DY_f *= -1;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1542 accel_DZ_f *= -1;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1543 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1544
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1545
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1546 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1547 // compass_calc_roll_pitch_only
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1548 /// @brief only the roll and pitch parts of compass_calc()
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1549 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1550 /// input is accel_DX_f, accel_DY_f, accel_DZ_f
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1551 /// output is compass_pitch and compass_roll
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1552 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1553 void compass_calc_roll_pitch_only(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1554 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1555 float sinPhi, cosPhi;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1556 float Phi, Teta;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1557
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1558 //---- Calculate sine and cosine of roll angle Phi -----------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1559 Phi= atan2f(accel_DY_f, accel_DZ_f) ;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1560 compass_roll = Phi * 180.0f /PI;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1561 sinPhi = sinf(Phi);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1562 cosPhi = cosf(Phi);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1563
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1564 //---- calculate sin and cosine of pitch angle Theta ---------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1565 Teta = atanf(-(float)accel_DX_f/(accel_DY_f * sinPhi + accel_DZ_f * cosPhi));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1566 compass_pitch = Teta * 180.0f /PI;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1567 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1568
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1569
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1570 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1571 // compass_calc
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1572 /// @brief all the fancy stuff first implemented in OSTC3
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1573 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1574 /// 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
1575 /// and compass_CX_f, compass_CY_f, compass_CZ_f
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1576 /// output is compass_heading, compass_pitch and compass_roll
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1577 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1578 void compass_calc(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1579 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1580 float sinPhi, cosPhi, sinTeta, cosTeta;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1581 float Phi, Teta, Psi;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1582 int16_t iBfx, iBfy;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1583 int16_t iBpx, iBpy, iBpz;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1584
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1585 //---- Make hard iron correction -----------------------------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1586 // Measured magnetometer orientation, measured ok.
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1587 // From matthias drawing: (X,Y,Z) --> (X,Y,Z) : no rotation.
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1588 iBpx = compass_DX_f - compass_CX_f; // X
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1589 iBpy = compass_DY_f - compass_CY_f; // Y
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1590 iBpz = compass_DZ_f - compass_CZ_f; // Z
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1591
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1592 //---- Calculate sine and cosine of roll angle Phi -----------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1593 //sincos(accel_DZ_f, accel_DY_f, &sin, &cos);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1594 Phi= atan2f(accel_DY_f, accel_DZ_f) ;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1595 compass_roll = Phi * 180.0f /PI;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1596 sinPhi = sinf(Phi);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1597 cosPhi = cosf(Phi);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1598
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1599 //---- rotate by roll angle (-Phi) ---------------------------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1600 iBfy = iBpy * cosPhi - iBpz * sinPhi;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1601 iBpz = iBpy * sinPhi + iBpz * cosPhi;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1602 //Gz = imul(accel_DY_f, sin) + imul(accel_DZ_f, cos);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1603
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1604 //---- calculate sin and cosine of pitch angle Theta ---------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1605 //sincos(Gz, -accel_DX_f, &sin, &cos); // NOTE: changed sin sign.
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1606 // 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
1607 Teta = atanf(-(float)accel_DX_f/(accel_DY_f * sinPhi + accel_DZ_f * cosPhi));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1608 compass_pitch = Teta * 180.0f /PI;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1609 sinTeta = sinf(Teta);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1610 cosTeta = cosf(Teta);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1611 /* correct cosine if pitch not in range -90 to 90 degrees */
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1612 if( cosTeta < 0 ) cosTeta = -cosTeta;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1613
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1614 ///---- de-rotate by pitch angle Theta -----------------------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1615 iBfx = iBpx * cosTeta + iBpz * sinTeta;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1616
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1617 //---- Detect uncalibrated compass ---------------------------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1618 if( !compass_CX_f && !compass_CY_f && !compass_CZ_f )
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1619 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1620 compass_heading = -1;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1621 return;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1622 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1623
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1624 //---- calculate current yaw = e-compass angle Psi -----------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1625 // Result in degree (no need of 0.01 deg precision...
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1626 Psi = atan2f(-iBfy,iBfx);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1627 compass_heading = Psi * 180.0f /PI;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1628 // Result in 0..360 range:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1629 if( compass_heading < 0 )
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1630 compass_heading += 360;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1631 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1632
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1633
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1634 /*
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1635 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1636 // compass_calc_mini_during_calibration
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1637 /// @brief all the fancy stuff first implemented in OSTC3
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1638 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1639 /// input is accel_DX_f, accel_DY_f, accel_DZ_f
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1640 /// output is compass_pitch and compass_roll
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1641 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1642 void compass_calc_mini_during_calibration(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1643 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1644 float sinPhi, cosPhi;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1645 float Phi, Teta;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1646
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1647 //---- Calculate sine and cosine of roll angle Phi -----------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1648 //sincos(accel_DZ_f, accel_DY_f, &sin, &cos);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1649 Phi= atan2f(accel_DY_f, accel_DZ_f) ;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1650 compass_roll = Phi * 180.0f /PI;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1651 sinPhi = sinf(Phi);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1652 cosPhi = cosf(Phi);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1653
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1654 //---- calculate sin and cosine of pitch angle Theta ---------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1655 //sincos(Gz, -accel_DX_f, &sin, &cos); // NOTE: changed sin sign.
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1656 Teta = atanf(-(float)accel_DX_f/(accel_DY_f * sinPhi + accel_DZ_f * cosPhi));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1657 compass_pitch = Teta * 180.0f /PI;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1658 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1659 */
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1660
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1661
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1662 // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1663 // // - Calibration - ///////////////////////////////////////////////////////////////////////////////////////////////////////
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1664 // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1665
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1666 /* can be lost during sleep as those are reset with compass_reset_calibration() */
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1667
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1668 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1669 // compass_reset_calibration
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1670 /// @brief all the fancy stuff first implemented in OSTC3
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1671 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1672 /// output is struct g and compass_CX_f, compass_CY_f, compass_CZ_f
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1673 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1674 /// @param g: is a struct with crazy stuff like Suuu, Svvv, Svvu, etc.
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1675 /// all is set to zero here
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1676 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1677 void compass_reset_calibration(SCompassCalib *g)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1678 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1679 g->compass_N = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1680 g->Su = g->Sv = g->Sw = 0.0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1681 g->Suu = g->Svv = g->Sww = g->Suv = g->Suw = g->Svw = 0.0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1682 g->Suuu = g->Svvv = g->Swww = 0.0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1683 g->Suuv = g->Suuw = g->Svvu = g->Svvw = g->Swwu = g->Swwv = 0.0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1684 compass_CX_f = compass_CY_f = compass_CZ_f = 0.0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1685 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1686
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1687
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1688 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1689 // compass_add_calibration
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1690 /// @brief all the fancy stuff first implemented in OSTC3
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1691 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1692 /// input is compass_DX_f, compass_DY_f, compass_DZ_f
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1693 /// and compass_CX_f, compass_CY_f, compass_CZ_f
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1694 /// output is struct g
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1695 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1696 /// @param g: is a struct with crazy stuff like Suuu, Svvv, Svvu, etc.
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1697 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1698 void compass_add_calibration(SCompassCalib *g)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1699 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1700 float u, v, w;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1701
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1702 u = (compass_DX_f - compass_CX_f) / 32768.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1703 v = (compass_DY_f - compass_CY_f) / 32768.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1704 w = (compass_DZ_f - compass_CZ_f) / 32768.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1705
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1706 g->compass_N++;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1707 g->Su += u;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1708 g->Sv += v;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1709 g->Sw += w;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1710 g->Suv += u*v;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1711 g->Suw += u*w;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1712 g->Svw += v*w;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1713 g->Suu += u*u;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1714 g->Suuu += u*u*u;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1715 g->Suuv += v*u*u;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1716 g->Suuw += w*u*u;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1717 g->Svv += v*v;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1718 g->Svvv += v*v*v;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1719 g->Svvu += u*v*v;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1720 g->Svvw += w*v*v;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1721 g->Sww += w*w;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1722 g->Swww += w*w*w;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1723 g->Swwu += u*w*w;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1724 g->Swwv += v*w*w;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1725 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1726
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1727 //////////////////////////////////////////////////////////////////////////////
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1728
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1729 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1730 // compass_solve_calibration
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1731 /// @brief all the fancy stuff first implemented in OSTC3
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1732 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1733 /// input is compass_CX_f, compass_CY_f, compass_CZ_f and g
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1734 /// output is struct g
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1735 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1736 /// @param g: is a struct with crazy stuff like Suuu, Svvv, Svvu, etc.
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1737 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1738 void compass_solve_calibration(SCompassCalib *g)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1739 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1740 float yu, yv, yw;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1741 float delta;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1742 float uc, vc, wc;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1743
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1744
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1745 //---- Normalize partial sums --------------------------------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1746 //
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1747 // u, v, w should be centered on the mean value um, vm, wm:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1748 // x = u + um, with um = Sx/N
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1749 //
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1750 // So:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1751 // (u + um)**2 = u**2 + 2u*um + um**2
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1752 // Su = 0, um = Sx/N
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1753 // Sxx = Suu + 2 um Su + N*(Sx/N)**2 = Suu + Sx**2/N
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1754 // Suu = Sxx - Sx**2/N
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1755 yu = g->Su/g->compass_N;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1756 yv = g->Sv/g->compass_N;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1757 yw = g->Sw/g->compass_N;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1758
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1759 g->Suu -= g->Su*yu;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1760 g->Svv -= g->Sv*yv;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1761 g->Sww -= g->Sw*yw;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1762
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1763 // (u + um)(v + vm) = uv + u vm + v um + um vm
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1764 // Sxy = Suv + N * um vm
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1765 // Suv = Sxy - N * (Sx/N)(Sy/N);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1766 g->Suv -= g->Su*yv;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1767 g->Suw -= g->Su*yw;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1768 g->Svw -= g->Sv*yw;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1769
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1770 // (u + um)**3 = u**3 + 3 u**2 um + 3 u um**2 + um**3
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1771 // Sxxx = Suuu + 3 um Suu + 3 um**2 Su + N.um**3
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1772 // Su = 0, um = Sx/N:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1773 // Suuu = Sxxx - 3 Sx*Suu/N - N.(Sx/N)**3
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1774 // = Sxxx - 3 Sx*Suu/N - Sx**3/N**2
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1775
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1776 // (u + um)**2 (v + vm) = (u**2 + 2 u um + um**2)(v + vm)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1777 // Sxxy = Suuv + vm Suu + 2 um (Suv + vm Su) + um**2 (Sv + N.vm)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1778 //
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1779 // Su = 0, Sv = 0, vm = Sy/N:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1780 // Sxxy = Suuv + vm Suu + 2 um Suv + N um**2 vm
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1781 //
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1782 // Suuv = Sxxy - (Sy/N) Suu - 2 (Sx/N) Suv - (Sx/N)**2 Sy
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1783 // = Sxxy - Suu*Sy/N - 2 Suv*Sx/N - Sx*Sx*Sy/N/N
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1784 // = Sxxy - (Suu + Sx*Sx/N)*Sy/N - 2 Suv*Sx/N
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1785 g->Suuu -= (3*g->Suu + g->Su*yu)*yu;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1786 g->Suuv -= (g->Suu + g->Su*yu)*yv + 2*g->Suv*yu;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1787 g->Suuw -= (g->Suu + g->Su*yu)*yw + 2*g->Suw*yu;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1788
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1789 g->Svvu -= (g->Svv + g->Sv*yv)*yu + 2*g->Suv*yv;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1790 g->Svvv -= (3*g->Svv + g->Sv*yv)*yv;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1791 g->Svvw -= (g->Svv + g->Sv*yv)*yw + 2*g->Svw*yv;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1792
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1793 g->Swwu -= (g->Sww + g->Sw*yw)*yu + 2*g->Suw*yw;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1794 g->Swwv -= (g->Sww + g->Sw*yw)*yv + 2*g->Svw*yw;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1795 g->Swww -= (3*g->Sww + g->Sw*yw)*yw;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1796
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1797 //---- Solve the system --------------------------------------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1798 // uc Suu + vc Suv + wc Suw = (Suuu + Svvu + Swwu) / 2
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1799 // uc Suv + vc Svv + wc Svw = (Suuv + Svvv + Swwv) / 2
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1800 // uc Suw + vc Svw + wc Sww = (Suuw + Svvw + Swww) / 2
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1801 // Note this is symetric, with a positiv diagonal, hence
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1802 // it always have a uniq solution.
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1803 yu = 0.5f * (g->Suuu + g->Svvu + g->Swwu);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1804 yv = 0.5f * (g->Suuv + g->Svvv + g->Swwv);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1805 yw = 0.5f * (g->Suuw + g->Svvw + g->Swww);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1806 delta = g->Suu * (g->Svv * g->Sww - g->Svw * g->Svw)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1807 - g->Suv * (g->Suv * g->Sww - g->Svw * g->Suw)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1808 + g->Suw * (g->Suv * g->Svw - g->Svv * g->Suw);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1809
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1810 uc = (yu * (g->Svv * g->Sww - g->Svw * g->Svw)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1811 - yv * (g->Suv * g->Sww - g->Svw * g->Suw)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1812 + yw * (g->Suv * g->Svw - g->Svv * g->Suw) )/delta;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1813 vc = (g->Suu * ( yv * g->Sww - yw * g->Svw)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1814 - g->Suv * ( yu * g->Sww - yw * g->Suw)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1815 + g->Suw * ( yu * g->Svw - yv * g->Suw) )/delta;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1816 wc = (g->Suu * (g->Svv * yw - g->Svw * yv )
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1817 - g->Suv * (g->Suv * yw - g->Svw * yu )
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1818 + g->Suw * (g->Suv * yv - g->Svv * yu ) )/delta;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1819
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1820 // Back to uncentered coordinates:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1821 // xc = um + uc
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1822 uc = g->Su/g->compass_N + compass_CX_f/32768.0f + uc;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1823 vc = g->Sv/g->compass_N + compass_CY_f/32768.0f + vc;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1824 wc = g->Sw/g->compass_N + compass_CZ_f/32768.0f + wc;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1825
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1826 // Then save the new calibrated center:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1827 compass_CX_f = (short)(32768 * uc);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1828 compass_CY_f = (short)(32768 * vc);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1829 compass_CZ_f = (short)(32768 * wc);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1830 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1831
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1832
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1833 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1834 // compass_calib
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1835 /// @brief the main loop for calibration
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1836 /// output is compass_CX_f, compass_CY_f, compass_CZ_f and g
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1837 /// 160704 removed -4096 limit for LSM303D
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1838 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1839 /// @return always 0
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1840 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1841 int compass_calib_common(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1842 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1843 SCompassCalib g;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1844
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1845 // Starts with no calibration at all:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1846 compass_reset_calibration(&g);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1847
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1848 int64_t tickstart = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1849 uint32_t ticks = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1850 uint32_t lasttick = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1851 tickstart = HAL_GetTick();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1852 // Eine Minute kalibrieren
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1853 while((ticks) < 60 * 1000)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1854 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1855 compass_read();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1856 acceleration_read();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1857 compass_calc_roll_pitch_only();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1858
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1859 if((hardwareCompass == HMC5883L)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1860 &&((compass_DX_f == -4096) ||
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1861 (compass_DY_f == -4096) ||
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1862 (compass_DZ_f == -4096) ))
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1863 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1864 if(compass_gain == 0)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1865 return -1;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1866 compass_gain--;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1867
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1868 compass_init(1, compass_gain);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1869 compass_reset_calibration(&g);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1870 //tickstart = HAL_GetTick();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1871 continue;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1872 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1873
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1874 copyCompassDataDuringCalibration(compass_DX_f,compass_DY_f,compass_DZ_f);
104
22a1094545f3 Tested and alive.
Dmitry Romanov <kitt@bk.ru>
parents: 70
diff changeset
1875 compass_add_calibration(&g);
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1876 HAL_Delay(1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1877 lasttick = HAL_GetTick();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1878 if(lasttick == 0)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1879 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1880 tickstart = -ticks;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1881 }
104
22a1094545f3 Tested and alive.
Dmitry Romanov <kitt@bk.ru>
parents: 70
diff changeset
1882 HAL_Delay(1);
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1883 ticks = lasttick - tickstart;
147
14e4c83a7559 Forward compass data during calibration mode
Ideenmodellierer
parents: 104
diff changeset
1884 SPI_Evaluate_RX_Data();
104
22a1094545f3 Tested and alive.
Dmitry Romanov <kitt@bk.ru>
parents: 70
diff changeset
1885 }
38
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1886
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1887 compass_solve_calibration(&g);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1888
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1889 tfull32 dataBlock[4];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1890 dataBlock[0].Word16.low16 = compass_CX_f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1891 dataBlock[0].Word16.hi16 = compass_CY_f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1892 dataBlock[1].Word16.low16 = compass_CZ_f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1893 dataBlock[1].Word16.hi16 = 0xFFFF;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1894 dataBlock[2].Full32 = 0x7FFFFFFF;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1895 dataBlock[3].Full32 = 0x7FFFFFFF;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1896 BFA_writeDataBlock((uint32_t *)dataBlock);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1897
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1898 return 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1899 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1900
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1901 // //////////////////////////// TEST CODE /////////////////////////////////////
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1902
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1903
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1904
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1905 //#include <QtDebug>
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1906 //#include <stdio.h>
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1907 //#include <math.h>
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1908 /*#include <stdlib.h>
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1909
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1910 short compass_DX_f, compass_DY_f, compass_DZ_f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1911 short compass_CX_f, compass_CY_f, compass_CZ_f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1912
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1913 inline float uniform(void) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1914 return (rand() & 0xFFFF) / 65536.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1915 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1916 inline float sqr(float x) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1917 return x*x;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1918 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1919
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1920 static const float radius = 0.21f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1921 static const float cx = 0.79f, cy = -0.46f, cz = 0.24f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1922 // const float cx = 0, cy = 0, cz = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1923
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1924 float check_compass_calib(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1925 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1926
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1927 // Starts with no calibration at all:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1928 compass_CX_f = compass_CY_f = compass_CZ_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1929
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1930 // Try 10 recalibration passes:
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1931 for(int p=0; p<10; ++p)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1932 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1933 compass_reset_calibration();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1934
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1935 //---- Generates random points on a sphere -------------------------------
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1936 // of radius,center (cx, cy, cz):
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1937 for(int i=0; i<100; ++i)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1938 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1939 float theta = uniform()*360.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1940 float phi = uniform()*180.0f - 90.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1941
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1942 float x = cx + radius * cosf(phi)*cosf(theta);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1943 float y = cy + radius * cosf(phi)*sinf(theta);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1944 float z = cz + radius * sinf(phi);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1945
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1946 compass_DX_f = (short)(32768 * x);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1947 compass_DY_f = (short)(32768 * y);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1948 compass_DZ_f = (short)(32768 * z);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1949 compass_add_calibration();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1950 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1951
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1952 compass_solve_calibration();
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1953 //qDebug() << "Center ="
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1954 // << compass_CX_f/32768.0f
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1955 // << compass_CY_f/32768.0f
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1956 // << compass_CZ_f/32768.0f;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1957
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1958 float r2 = sqr(compass_CX_f/32768.0f - cx)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1959 + sqr(compass_CY_f/32768.0f - cy)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1960 + sqr(compass_CZ_f/32768.0f - cz);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1961 if( r2 > 0.01f*0.01f )
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1962 return sqrtf(r2);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1963 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1964 return 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1965 }*/
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1966
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1967
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1968
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1969 /*
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1970 void compass_read_LSM303D_v3(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1971 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1972 uint8_t data;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1973
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1974 memset(magDataBuffer,0,6);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1975
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1976 compass_DX_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1977 compass_DY_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1978 compass_DZ_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1979
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1980 //magnetometer multi read, order xl,xh, yl,yh, zl, zh
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1981 data = REG_MAG_DATA_ADDR;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1982 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1983 I2C_Master_Receive( DEVICE_COMPASS_303D, magDataBuffer, 6);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1984
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1985 compass_DX_f = ((int16_t)( (int16_t)((magDataBuffer[1] << 8) | (magDataBuffer[0]))));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1986 compass_DY_f = ((int16_t)( (int16_t)((magDataBuffer[3] << 8) | (magDataBuffer[2]))));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1987 compass_DZ_f = ((int16_t)( (int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4]))));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1988
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1989 // compass_DX_f = compass_DX_f * stat->sensitivity_mag;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1990 // compass_DY_f = compass_DY_f * stat->sensitivity_mag;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1991 // compass_DZ_f = compass_DZ_f * stat->sensitivity_mag;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1992 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1993
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1994
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1995 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1996 // compass_init_LSM303D by STMicroelectronics 2013 V1.0.5 2013/Oct/23
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1997 /// @brief The new ST 303D
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1998 /// This might be called several times with different gain values during calibration
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
1999 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2000 /// @param gain: 7 is max gain and set with here, compass_calib() might reduce it
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2001 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2002
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2003 void compass_init_LSM303D_v3(uint8_t gain)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2004 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2005 uint8_t data[10];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2006
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2007 // CNTRL1
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2008 // 0011 acceleration data rate 0011 = 12.5 Hz (3.125 Hz - 1600 Hz)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2009 // 0xxx block data update off
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2010 // x111 enable all three axes
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2011
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2012 // CNTRL5
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2013 // 0xxx xxxx temp sensor off
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2014 // x00x xxxx magnetic resolution
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2015 // xxx0 1xxx magentic data rate 01 = 6,25 Hz (3.125 Hz - 50 Hz (100 Hz))
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2016 // xxxx xx00 latch irq requests off
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2017
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2018 // CNTRL7
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2019 // 00xx high pass filter mode, 00 normal mode
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2020 // xx0x filter for acceleration data bypassed
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2021 // xxx0 temperature sensor mode only off
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2022 // x0xx magnetic data low-power mode off
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2023 // xx00 magnetic sensor mode 00 = continous-conversion mode (default 10 power-down)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2024
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2025 data[0] = CNTRL0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2026 data[1] = 0x00;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2027 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2028
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2029 // acc
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2030 data[0] = CNTRL1;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2031 data[1] = 0x00;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2032 data[2] = 0x0F;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2033 data[3] = 0x00;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2034 data[4] = 0x00;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2035 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 5);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2036
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2037 // mag
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2038 data[0] = CNTRL3;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2039 data[1] = 0x00;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2040 data[2] = 0x00;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2041 data[3] = 0x18;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2042 data[4] = 0x20;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2043 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 5);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2044
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2045 data[0] = CNTRL7;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2046 data[1] = ((MSMS_MASK & CONTINUOS_CONVERSION) |
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2047 ((~MSMS_MASK) & CNTRL7_RESUME_VALUE));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2048 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2049
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2050 HAL_Delay(100);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2051 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2052
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2053
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2054 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2055 // compass_init_LSM303D by nordevx for arduion
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2056 /// @brief The new ST 303D
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2057 /// This might be called several times with different gain values during calibration
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2058 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2059 /// @param gain: 7 is max gain and set with here, compass_calib() might reduce it
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2060 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2061 void compass_init_LSM303D_v2(uint8_t gain)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2062 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2063 uint8_t data[2];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2064
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2065 // CNTRL1
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2066 // 0011 acceleration data rate 0011 = 12.5 Hz (3.125 Hz - 1600 Hz)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2067 // 0xxx block data update off
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2068 // x111 enable all three axes
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2069
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2070 // CNTRL5
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2071 // 0xxx xxxx temp sensor off
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2072 // x00x xxxx magnetic resolution
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2073 // xxx0 1xxx magentic data rate 01 = 6,25 Hz (3.125 Hz - 50 Hz (100 Hz))
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2074 // xxxx xx00 latch irq requests off
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2075
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2076 // CNTRL7
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2077 // 00xx high pass filter mode, 00 normal mode
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2078 // xx0x filter for acceleration data bypassed
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2079 // xxx0 temperature sensor mode only off
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2080 // x0xx magnetic data low-power mode off
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2081 // xx00 magnetic sensor mode 00 = continous-conversion mode (default 10 power-down)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2082
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2083 data[0] = CNTRL1;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2084 data[1] = 0x37; //0b 0011 0111
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2085 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2086
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2087 data[0] = CNTRL5;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2088 data[1] = 0x08; // 0b 0000 1000
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2089 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2090
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2091 data[0] = CNTRL7;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2092 data[1] = 0x00; // 0b 0000 0000
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2093 I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2094
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2095 HAL_Delay(100);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2096 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2097
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2098
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2099 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2100 // compass_init_LSM303D_v1 by ST lsm303d.c
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2101 /// @brief The new ST 303D
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2102 /// This might be called several times with different gain values during calibration
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2103 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2104 /// @param gain: 7 is max gain and set with here, compass_calib() might reduce it
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2105 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2106 void compass_init_LSM303D_v1(uint8_t gain)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2107 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2108 uint8_t data;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2109
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2110 compass_gain = gain;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2111
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2112 memset(magDataBuffer,0,6);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2113 memset(accDataBuffer,0,6);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2114
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2115 data = CNTRL5;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2116 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2117 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2118 data = (data & 0x1c) >> 2;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2119 velMag = magODR[data];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2120
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2121 data = CNTRL1;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2122 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2123 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2124 data = (data & 0xf0) >> 4;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2125 velAcc = accODR[data];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2126
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2127 data = CNTRL7;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2128 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2129 I2C_Master_Receive( DEVICE_COMPASS_303D, &datas1, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2130 datas1 = (datas1 & 0x02);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2131
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2132 //if mag is not pd
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2133 //mag is bigger than gyro
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2134 if( (velMag < velAcc) || datas1 != 0 ) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2135 //acc is the biggest
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2136 fastest = ACC_IS_FASTEST;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2137 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2138 else {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2139 //acc is the biggest
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2140 fastest = MAG_IS_FASTEST;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2141 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2142
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2143 zoffFlag = 1;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2144
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2145 if( fastest == MAG_IS_FASTEST)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2146 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2147 data = STATUS_REG_M;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2148 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2149 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2150
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2151 // if(ValBit(data, ZYXMDA)) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2152 sendFlag = 1;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2153 // }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2154
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2155 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2156 else if(fastest == ACC_IS_FASTEST)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2157 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2158 data = STATUS_REG_A;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2159 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2160 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2161 // if(ValBit(data, DATAREADY_BIT)) {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2162 sendFlag = 1;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2163 // }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2164 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2165 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2166
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2167 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2168 // compass_read_LSM303D
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2169 /// @brief The new LSM303D :-)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2170 ///
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2171 /// output 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
2172 // ===============================================================================
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2173 void compass_read_LSM303D_v2(void)
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2174 {
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2175 uint8_t data;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2176
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2177 memset(magDataBuffer,0,6);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2178 memset(accDataBuffer,0,6);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2179
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2180 compass_DX_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2181 compass_DY_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2182 compass_DZ_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2183
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2184 accel_DX_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2185 accel_DY_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2186 accel_DZ_f = 0;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2187
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2188 //Accelerometer multi read, order xl,xh, yl,yh, zl, zh
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2189 data = REG_ACC_DATA_ADDR;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2190 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2191 I2C_Master_Receive( DEVICE_COMPASS_303D, accDataBuffer, 6);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2192
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2193 //magnetometer multi read, order xl,xh, yl,yh, zl, zh
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2194 data = OUT_X_L_M;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2195 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2196 I2C_Master_Receive( DEVICE_COMPASS_303D, magDataBuffer, 6);
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2197
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2198 accel_DX_f = ((int16_t)( (int16_t)((accDataBuffer[1] << 8) | (accDataBuffer[0]))));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2199 accel_DY_f = ((int16_t)( (int16_t)((accDataBuffer[3] << 8) | (accDataBuffer[2]))));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2200 accel_DZ_f = ((int16_t)( (int16_t)((accDataBuffer[5] << 8) | (accDataBuffer[4]))));
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2201
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2202 // accel_DX_f = accel_DX_f * stat->sensitivity_acc;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2203 // accel_DY_f = accel_DY_f * stat->sensitivity_acc;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2204 // accel_DZ_f = accel_DZ_f * stat->sensitivity_acc;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2205
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2206
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2207 compass_DX_f = magDataBuffer[1];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2208 compass_DX_f *= 256;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2209 compass_DX_f += magDataBuffer[0];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2210
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2211 compass_DY_f = magDataBuffer[3];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2212 compass_DY_f *= 256;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2213 compass_DY_f += magDataBuffer[2];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2214
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2215 compass_DY_f = magDataBuffer[5];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2216 compass_DY_f *= 256;
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2217 compass_DY_f += magDataBuffer[4];
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2218
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2219 }
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2220
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2221
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2222 */
5f11787b4f42 include in ostc4 repository
heinrichsweikamp
parents:
diff changeset
2223