annotate Small_CPU/Src/compass.c @ 558:66aef216828b

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