annotate Small_CPU/Src/compass.c @ 328:4fe5400567e7 I2C_Improvment

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