annotate Small_CPU/Src/compass.c @ 909:6902bb4b6b2e Evo_2_23

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