annotate Small_CPU/Src/compass.c @ 261:cc2406b835ff bm-3

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