1 /*
2  * Copyright (C) 2015-2021 Alibaba Group Holding Limited
3  */
4 
5 #ifndef FIS210X_H
6 #define FIS210X_H
7 #include "k_api.h"
8 #include <math.h>
9 #include <stdbool.h>
10 #include "aos/hal/i2c.h"
11 #include "hal_iomux_haas1000.h"
12 
13 #ifndef M_PI
14 #define M_PI    (3.14159265358979323846f)
15 #endif
16 #ifndef ONE_G
17 #define ONE_G   (9.80665f)
18 #endif
19 
20 #define FISIMU_CTRL7_DISABLE_ALL        (0x0)
21 #define FISIMU_CTRL7_ACC_ENABLE         (0x1)
22 #define FISIMU_CTRL7_GYR_ENABLE         (0x2)
23 #define FISIMU_CTRL7_MAG_ENABLE         (0x4)
24 #define FISIMU_CTRL7_AE_ENABLE          (0x8)
25 #define FISIMU_CTRL7_ENABLE_MASK        (0xF)
26 
27 #define FISIMU_CONFIG_ACC_ENABLE        FISIMU_CTRL7_ACC_ENABLE
28 #define FISIMU_CONFIG_GYR_ENABLE        FISIMU_CTRL7_GYR_ENABLE
29 #define FISIMU_CONFIG_MAG_ENABLE        FISIMU_CTRL7_MAG_ENABLE
30 #define FISIMU_CONFIG_AE_ENABLE         FISIMU_CTRL7_AE_ENABLE
31 #define FISIMU_CONFIG_ACCGYR_ENABLE     (FISIMU_CONFIG_ACC_ENABLE | FISIMU_CONFIG_GYR_ENABLE)
32 #define FISIMU_CONFIG_ACCGYRMAG_ENABLE  (FISIMU_CONFIG_ACC_ENABLE | FISIMU_CONFIG_GYR_ENABLE | FISIMU_CONFIG_MAG_ENABLE)
33 #define FISIMU_CONFIG_AEMAG_ENABLE      (FISIMU_CONFIG_AE_ENABLE | FISIMU_CONFIG_MAG_ENABLE)
34 
35 #define FISIMU_STATUS1_CMD_DONE         (0x01)
36 #define FISIMU_STATUS1_WAKEUP_EVENT     (0x04)
37 
38 enum FIS210xRegister {
39     /*! \brief FIS device identifier register. */
40     FisRegister_WhoAmI = 0, // 0
41     /*! \brief FIS hardware revision register. */
42     FisRegister_Revision, // 1
43     /*! \brief General and power management modes. */
44     FisRegister_Ctrl1, // 2
45     /*! \brief Accelerometer control. */
46     FisRegister_Ctrl2, // 3
47     /*! \brief Gyroscope control. */
48     FisRegister_Ctrl3, // 4
49     /*! \brief Magnetometer control. */
50     FisRegister_Ctrl4, // 5
51     /*! \brief Data processing settings. */
52     FisRegister_Ctrl5, // 6
53     /*! \brief AttitudeEngine control. */
54     FisRegister_Ctrl6, // 7
55     /*! \brief Sensor enabled status. */
56     FisRegister_Ctrl7, // 8
57     /*! \brief Reserved - do not write. */
58     FisRegister_Ctrl8, // 9
59     /*! \brief Host command register. */
60     FisRegister_Ctrl9,
61     /*! \brief Calibration register 1 least significant byte. */
62     FisRegister_Cal1_L,
63     /*! \brief Calibration register 1 most significant byte. */
64     FisRegister_Cal1_H,
65     /*! \brief Calibration register 2 least significant byte. */
66     FisRegister_Cal2_L,
67     /*! \brief Calibration register 2 most significant byte. */
68     FisRegister_Cal2_H,
69     /*! \brief Calibration register 3 least significant byte. */
70     FisRegister_Cal3_L,
71     /*! \brief Calibration register 3 most significant byte. */
72     FisRegister_Cal3_H,
73     /*! \brief Calibration register 4 least significant byte. */
74     FisRegister_Cal4_L,
75     /*! \brief Calibration register 4 most significant byte. */
76     FisRegister_Cal4_H,
77     /*! \brief FIFO control register. */
78     FisRegister_FifoCtrl,
79     /*! \brief FIFO data register. */
80     FisRegister_FifoData,
81     /*! \brief FIFO status register. */
82     FisRegister_FifoStatus,
83     /*! \brief Output data overrun and availability. */
84     FisRegister_Status0,
85     /*! \brief Miscellaneous status register. */
86     FisRegister_Status1,
87     /*! \brief Sample counter. */
88     FisRegister_CountOut,
89     /*! \brief Accelerometer X axis least significant byte. */
90     FisRegister_Ax_L,
91     /*! \brief Accelerometer X axis most significant byte. */
92     FisRegister_Ax_H,
93     /*! \brief Accelerometer Y axis least significant byte. */
94     FisRegister_Ay_L,
95     /*! \brief Accelerometer Y axis most significant byte. */
96     FisRegister_Ay_H,
97     /*! \brief Accelerometer Z axis least significant byte. */
98     FisRegister_Az_L,
99     /*! \brief Accelerometer Z axis most significant byte. */
100     FisRegister_Az_H,
101     /*! \brief Gyroscope X axis least significant byte. */
102     FisRegister_Gx_L,
103     /*! \brief Gyroscope X axis most significant byte. */
104     FisRegister_Gx_H,
105     /*! \brief Gyroscope Y axis least significant byte. */
106     FisRegister_Gy_L,
107     /*! \brief Gyroscope Y axis most significant byte. */
108     FisRegister_Gy_H,
109     /*! \brief Gyroscope Z axis least significant byte. */
110     FisRegister_Gz_L,
111     /*! \brief Gyroscope Z axis most significant byte. */
112     FisRegister_Gz_H,
113     /*! \brief Magnetometer X axis least significant byte. */
114     FisRegister_Mx_L,
115     /*! \brief Magnetometer X axis most significant byte. */
116     FisRegister_Mx_H,
117     /*! \brief Magnetometer Y axis least significant byte. */
118     FisRegister_My_L,
119     /*! \brief Magnetometer Y axis most significant byte. */
120     FisRegister_My_H,
121     /*! \brief Magnetometer Z axis least significant byte. */
122     FisRegister_Mz_L,
123     /*! \brief Magnetometer Z axis most significant byte. */
124     FisRegister_Mz_H,
125     /*! \brief Quaternion increment W least significant byte. */
126     FisRegister_Q1_L = 45,
127     /*! \brief Quaternion increment W most significant byte. */
128     FisRegister_Q1_H,
129     /*! \brief Quaternion increment X least significant byte. */
130     FisRegister_Q2_L,
131     /*! \brief Quaternion increment X most significant byte. */
132     FisRegister_Q2_H,
133     /*! \brief Quaternion increment Y least significant byte. */
134     FisRegister_Q3_L,
135     /*! \brief Quaternion increment Y most significant byte. */
136     FisRegister_Q3_H,
137     /*! \brief Quaternion increment Z least significant byte. */
138     FisRegister_Q4_L,
139     /*! \brief Quaternion increment Z most significant byte. */
140     FisRegister_Q4_H,
141     /*! \brief Velocity increment X least significant byte. */
142     FisRegister_Dvx_L,
143     /*! \brief Velocity increment X most significant byte. */
144     FisRegister_Dvx_H,
145     /*! \brief Velocity increment Y least significant byte. */
146     FisRegister_Dvy_L,
147     /*! \brief Velocity increment Y most significant byte. */
148     FisRegister_Dvy_H,
149     /*! \brief Velocity increment Z least significant byte. */
150     FisRegister_Dvz_L,
151     /*! \brief Velocity increment Z most significant byte. */
152     FisRegister_Dvz_H,
153     /*! \brief Temperature output. */
154     FisRegister_Temperature,
155     /*! \brief AttitudeEngine clipping flags. */
156     FisRegister_AeClipping,
157     /*! \brief AttitudeEngine overflow flags. */
158     FisRegister_AeOverflow,
159 };
160 
161 enum FisImu_Ctrl9Command {
162     /*! \brief No operation. */
163     Ctrl9_Nop = 0,
164     /*! \brief Reset FIFO. */
165     Ctrl9_ResetFifo = 0x2,
166     /*! \brief Set magnetometer X calibration values. */
167     Ctrl9_SetMagXCalibration = 0x6,
168     /*! \brief Set magnetometer Y calibration values. */
169     Ctrl9_SetMagYCalibration = 0x7,
170     /*! \brief Set magnetometer Z calibration values. */
171     Ctrl9_SetMagZCalibration = 0x8,
172     /*! \brief Set accelerometer offset correction value. */
173     Ctrl9_SetAccelOffset = 0x12,
174     /*! \brief Set gyroscope offset correction value. */
175     Ctrl9_SetGyroOffset = 0x13,
176     /*! \brief Set accelerometer sensitivity. */
177     Ctrl9_SetAccelSensitivity = 0x14,
178     /*! \brief Set gyroscope sensitivity. */
179     Ctrl9_SetGyroSensitivity = 0x15,
180     /*! \brief Update magnemoter bias compensation. */
181     Ctrl9_UpdateMagBias = 0xB,
182     /*! \brief Trigger motion on demand sample. */
183     Ctrl9_TriggerMotionOnDemand = 0x0c,
184     /*! \brief Update gyroscope bias compensation. */
185     Ctrl9_UpdateAttitudeEngineGyroBias = 0xE,
186     /*! \brief Read frequency correction value. */
187     Ctrl9_ReadTrimmedFrequencyValue = 0x18,
188     /*! \brief Prepare for FIFO read sequence. */
189     Ctrl9_ReadFifo = 0x0D,
190     /*! \brief Set wake on motion parameters. */
191     Ctrl9_ConfigureWakeOnMotion = 0x19,
192 };
193 
194 enum FisImu_LpfConfig {
195     Lpf_Disable, /*!< \brief Disable low pass filter. */
196     Lpf_Enable   /*!< \brief Enable low pass filter. */
197 };
198 
199 enum FisImu_HpfConfig {
200     Hpf_Disable, /*!< \brief Disable high pass filter. */
201     Hpf_Enable   /*!< \brief Enable high pass filter. */
202 };
203 
204 enum FisImu_AccRange {
205     AccRange_2g = 0 << 3, /*!< \brief +/- 2g range */
206     AccRange_4g = 1 << 3, /*!< \brief +/- 4g range */
207     AccRange_8g = 2 << 3, /*!< \brief +/- 8g range */
208     AccRange_16g = 3 << 3 /*!< \brief +/- 16g range */
209 };
210 
211 
212 enum FisImu_AccOdr {
213     AccOdr_1024Hz = 0,  /*!< \brief High resolution 1024Hz output rate. */
214     AccOdr_256Hz = 1, /*!< \brief High resolution 256Hz output rate. */
215     AccOdr_128Hz = 2, /*!< \brief High resolution 128Hz output rate. */
216     AccOdr_32Hz = 3,  /*!< \brief High resolution 32Hz output rate. */
217     AccOdr_LowPower_128Hz = 4, /*!< \brief Low power 128Hz output rate. */
218     AccOdr_LowPower_64Hz = 5,  /*!< \brief Low power 64Hz output rate. */
219     AccOdr_LowPower_25Hz = 6,  /*!< \brief Low power 25Hz output rate. */
220     AccOdr_LowPower_3Hz = 7    /*!< \brief Low power 3Hz output rate. */
221 };
222 
223 enum FisImu_GyrRange {
224     GyrRange_32dps = 0 << 3,   /*!< \brief +-32 degrees per second. */
225     GyrRange_64dps = 1 << 3,   /*!< \brief +-64 degrees per second. */
226     GyrRange_128dps = 2 << 3,  /*!< \brief +-128 degrees per second. */
227     GyrRange_256dps = 3 << 3,  /*!< \brief +-256 degrees per second. */
228     GyrRange_512dps = 4 << 3,  /*!< \brief +-512 degrees per second. */
229     GyrRange_1024dps = 5 << 3, /*!< \brief +-1024 degrees per second. */
230     GyrRange_2048dps = 6 << 3, /*!< \brief +-2048 degrees per second. */
231     GyrRange_2560dps = 7 << 3  /*!< \brief +-2560 degrees per second. */
232 };
233 
234 /*!
235  * \brief Gyroscope output rate configuration.
236  */
237 enum FisImu_GyrOdr {
238     GyrOdr_1024Hz           = 0,    /*!< \brief High resolution 1024Hz output rate. */
239     GyrOdr_256Hz            = 1,    /*!< \brief High resolution 256Hz output rate. */
240     GyrOdr_128Hz            = 2,    /*!< \brief High resolution 128Hz output rate. */
241     GyrOdr_32Hz             = 3,    /*!< \brief High resolution 32Hz output rate. */
242     GyrOdr_OIS_8192Hz       = 6,    /*!< \brief OIS Mode 8192Hz output rate. */
243     GyrOdr_OIS_LL_8192Hz    = 7     /*!< \brief OIS LL Mode 8192Hz output rate. */
244 };
245 
246 enum FisImu_AeOdr {
247     AeOdr_1Hz = 0,  /*!< \brief 1Hz output rate. */
248     AeOdr_2Hz = 1,  /*!< \brief 2Hz output rate. */
249     AeOdr_4Hz = 2,  /*!< \brief 4Hz output rate. */
250     AeOdr_8Hz = 3,  /*!< \brief 8Hz output rate. */
251     AeOdr_16Hz = 4, /*!< \brief 16Hz output rate. */
252     AeOdr_32Hz = 5, /*!< \brief 32Hz output rate. */
253     AeOdr_64Hz = 6,  /*!< \brief 64Hz output rate. */
254     /*!
255      * \brief Motion on demand mode.
256      *
257      * In motion on demand mode the application can trigger AttitudeEngine
258      * output samples as necessary. This allows the AttitudeEngine to be
259      * synchronized with external data sources.
260      *
261      * When in Motion on Demand mode the application should request new data
262      * by calling the FisImu_requestAttitudeEngineData() function. The
263      * AttitudeEngine will respond with a data ready event (INT2) when the
264      * data is available to be read.
265      */
266     AeOdr_motionOnDemand = 128
267 };
268 
269 enum FisImu_MagOdr {
270     MagOdr_32Hz = 2   /*!< \brief 32Hz output rate. */
271 };
272 
273 enum FisImu_MagDev {
274     MagDev_AK8975 = (0 << 4), /*!< \brief AKM AK8975. */
275     MagDev_AK8963 = (1 << 4) /*!< \brief AKM AK8963. */
276 };
277 
278 enum FisImu_AccUnit {
279     AccUnit_g,  /*!< \brief Accelerometer output in terms of g (9.81m/s^2). */
280     AccUnit_ms2 /*!< \brief Accelerometer output in terms of m/s^2. */
281 };
282 
283 enum FisImu_GyrUnit {
284     GyrUnit_dps, /*!< \brief Gyroscope output in degrees/s. */
285     GyrUnit_rads /*!< \brief Gyroscope output in rad/s. */
286 };
287 
288 struct FisImuConfig {
289     /*! \brief Sensor fusion input selection. */
290     uint8_t inputSelection;
291     /*! \brief Accelerometer dynamic range configuration. */
292     enum FisImu_AccRange accRange;
293     /*! \brief Accelerometer output rate. */
294     enum FisImu_AccOdr accOdr;
295     /*! \brief Gyroscope dynamic range configuration. */
296     enum FisImu_GyrRange gyrRange;
297     /*! \brief Gyroscope output rate. */
298     enum FisImu_GyrOdr gyrOdr;
299     /*! \brief AttitudeEngine output rate. */
300     enum FisImu_AeOdr aeOdr;
301     /*!
302      * \brief Magnetometer output data rate.
303      *
304      * \remark This parameter is not used when using an external magnetometer.
305      * In this case the external magnetometer is sampled at the FIS output
306      * data rate, or at an integer divisor thereof such that the maximum
307      * sample rate is not exceeded.
308      */
309     enum FisImu_MagOdr magOdr;
310 
311     /*!
312      * \brief Magnetometer device to use.
313      *
314      * \remark This parameter is not used when using an external magnetometer.
315      */
316     enum FisImu_MagDev magDev;
317 };
318 
319 
320 #define FISIMU_SAMPLE_SIZE (3 * sizeof(int16_t))
321 #define FISIMU_AE_SAMPLE_SIZE ((4 + 3 + 1) * sizeof(int16_t) + sizeof(uint8_t))
322 struct FisImuRawSample {
323     /*! \brief The sample counter of the sample. */
324     uint8_t sampleCounter;
325     /*!
326      * \brief Pointer to accelerometer data in the sample buffer.
327      *
328      * \c NULL if no accelerometer data is available in the buffer.
329      */
330     uint8_t const *accelerometerData;
331     /*!
332      * \brief Pointer to gyroscope data in the sample buffer.
333      *
334      * \c NULL if no gyroscope data is available in the buffer.
335      */
336     uint8_t const *gyroscopeData;
337     /*!
338      * \brief Pointer to magnetometer data in the sample buffer.
339      *
340      * \c NULL if no magnetometer data is available in the buffer.
341      */
342     uint8_t const *magnetometerData;
343     /*!
344      * \brief Pointer to AttitudeEngine data in the sample buffer.
345      *
346      * \c NULL if no AttitudeEngine data is available in the buffer.
347      */
348     uint8_t const *attitudeEngineData;
349     /*! \brief Raw sample buffer. */
350     uint8_t sampleBuffer[FISIMU_SAMPLE_SIZE + FISIMU_AE_SAMPLE_SIZE];
351     /*! \brief Contents of the FIS status 1 register. */
352     uint8_t status1;
353     // uint8_t status0;
354     // uint32_t durT;
355 };
356 
357 struct FisImu_offsetCalibration {
358     enum FisImu_AccUnit accUnit;
359     float accOffset[3];
360     enum FisImu_GyrUnit gyrUnit;
361     float gyrOffset[3];
362 };
363 
364 struct FisImu_sensitivityCalibration {
365     float accSensitivity[3];
366     float gyrSensitivity[3];
367 };
368 
369 enum FisImu_Interrupt {
370     /*! \brief FIS INT1 line. */
371     Fis_Int1 = (0 << 6),
372     /*! \brief FIS INT2 line. */
373     Fis_Int2 = (1 << 6)
374 };
375 
376 enum FisImu_InterruptInitialState {
377     InterruptInitialState_high = (1 << 7), /*!< Interrupt high. */
378     InterruptInitialState_low  = (0 << 7)  /*!< Interrupt low. */
379 };
380 
381 enum FisImu_WakeOnMotionThreshold {
382     WomThreshold_high = 128, /*!< High threshold - large motion needed to wake. */
383     WomThreshold_low  = 32   /*!< Low threshold - small motion needed to wake. */
384 };
385 
386 
387 extern uint8_t FisImu_write_reg(uint8_t reg, uint8_t value);
388 extern uint8_t FisImu_read_reg(uint8_t reg, uint8_t *buf, uint16_t len);
389 extern uint8_t FisImu_init(void);
390 extern void FisImu_deinit(void);
391 extern void FisImu_Config_apply(struct FisImuConfig const *config);
392 extern void FisImu_enableSensors(uint8_t enableFlags);
393 extern void FisImu_read_acc_xyz(float acc_xyz[3]);
394 extern void FisImu_read_gyro_xyz(float gyro_xyz[3]);
395 extern void FisImu_read_xyz(float acc[3], float gyro[3]);
396 extern uint8_t FisImu_readStatus1(void);
397 extern int8_t FisImu_readTemp(void);
398 extern void FisImu_enableWakeOnMotion(void);
399 extern void FisImu_disableWakeOnMotion(void);
400 // for XKF3
401 extern void FisImu_processAccelerometerData(uint8_t const *rawData, float *calibratedData);
402 extern void FisImu_processGyroscopeData(uint8_t const *rawData, float *calibratedData);
403 extern void FisImu_read_rawsample(struct FisImuRawSample *sample);
404 extern void FisImu_applyOffsetCalibration(struct FisImu_offsetCalibration const *cal);
405 // for XKF3
406 // fis210x
407 #endif
408