1 /*
2 * Copyright (c) 2015, Freescale Semiconductor, Inc.
3 * Copyright 2016-2017 NXP
4 * All rights reserved.
5 *
6 * SPDX-License-Identifier: BSD-3-Clause
7 */
8
9 /*! \file driver_MAG3110.c
10 \brief Provides init() and read() functions for the MAG3110 magnetometer.
11 */
12
13 #include "board.h" // generated by Kinetis Expert. Long term - merge sensor_board.h into this file
14 #include "sensor_fusion.h" // Sensor fusion structures and types
15 #include "sensor_drv.h"
16 #include "sensor_io_i2c.h" // Required for registerreadlist_t / registerwritelist_t declarations
17 #include "drivers.h" // Device specific drivers supplied by NXP (can be replaced with user drivers)
18 #include "mag3110.h"
19 #define MAG3110_COUNTSPERUT 10 // fixed range for MAG3110 magnetometer
20
21 #if F_USING_MAG
22
23 // Command definition to read the WHO_AM_I value.
24 const registerreadlist_t MAG3110_WHO_AM_I_READ[] =
25 {
26 { .readFrom = MAG3110_WHO_AM_I, .numBytes = 1 }, __END_READ_DATA__
27 };
28
29 // Command definition to read the number of entries in the accel FIFO.
30 const registerreadlist_t MAG3110_DR_STATUS_READ[] =
31 {
32 { .readFrom = MAG3110_DR_STATUS, .numBytes = 1 }, __END_READ_DATA__
33 };
34
35 // Command definition to read the converted result
36 registerreadlist_t MAG3110_DATA_READ[] =
37 {
38 { .readFrom = MAG3110_OUT_X_MSB, .numBytes = 6 }, __END_READ_DATA__
39 };
40
41 // Each entry in a RegisterWriteList is composed of: register address, value to write, bit-mask to apply to write (0 enables)
42 const registerwritelist_t MAG3110_Initialization[] =
43 {
44 // write 0000 0000 = 0x00 to CTRL_REG1 to place MAG3110 into standby
45 // [7-1] = 0000 000
46 // [0]: AC=0 for standby
47 { MAG3110_CTRL_REG1, 0x00, 0x00 },
48
49 // write 1001 0000 = 0x90 to CTRL_REG2
50 // [7]: AUTO_MRST_EN=1: enable degaussing
51 // [6]: unused=0
52 // [5]: RAW=0: normal mode
53 // [4]: Mag_RST=1: enable a single degauss
54 // [3-0]: unused=0
55 { MAG3110_CTRL_REG2, 0x90, 0x00 },
56
57 // write 000X X001 to CTRL_REG1 to set ODR and take MAG3110 out of standby
58 // [7-5]: DR=000 for 1280Hz ADC (to give best noise performance)
59 // [4-3]: OS=11 for 10Hz ODR giving 0x19
60 // [4-3]: OS=10 for 20Hz ODR giving 0x11
61 // [4-3]: OS=01 for 40Hz ODR giving 0x09
62 // [4-3]: OS=00 for 80Hz ODR giving 0x01
63 // [2]: FT=0 for normal reads
64 // [1]: TM=0 to not trigger immediate measurement
65 // [0]: AC=1 for active mode
66 #if (MAG_ODR_HZ <= 10) // select 10Hz ODR
67 { MAG3110_CTRL_REG1, 0x19, 0x00 },
68 #elif (MAG_ODR_HZ <= 30) // select 20Hz ODR (to give lower noise with standard 25Hz build)
69 { MAG3110_CTRL_REG1, 0x11, 0x00 },
70 #elif (MAG_ODR_HZ <= 40) // select 40Hz ODR
71 { MAG3110_CTRL_REG1, 0x09, 0x00 },
72 #else // select 80Hz ODR
73 { MAG3110_CTRL_REG1, 0x01, 0x00 },
74 #endif
75 __END_WRITE_DATA__
76 };
77
78 // All sensor drivers and initialization functions have the same prototype.
79 // sfg is a pointer to the master "global" sensor fusion structure.
80 // sensor = pointer to linked list element used by the sensor fusion subsystem to specify required sensors
81 //#pragma optimize=no_scheduling
MAG3110_Init(struct PhysicalSensor * sensor,SensorFusionGlobals * sfg)82 int8_t MAG3110_Init(struct PhysicalSensor *sensor, SensorFusionGlobals *sfg)
83 {
84 int32_t status;
85 uint8_t reg;
86 status = Register_I2C_Read(sensor->bus_driver, &sensor->deviceInfo, sensor->addr, MAG3110_WHO_AM_I, 1, ®);
87 if (status==SENSOR_ERROR_NONE) {
88 sfg->Mag.iWhoAmI = reg;
89 if (reg!=MAG3110_WHOAMI_VALUE) {
90 return(SENSOR_ERROR_INIT);
91 }
92 } else {
93 // iWhoAmI will return default value of zero
94 // return with error
95 return(SENSOR_ERROR_INIT);
96 }
97
98 // Configure and start the MAG3110 sensor. This does multiple register writes
99 // (see MAG3110_Initialization definition above)
100 status = Sensor_I2C_Write(sensor->bus_driver, &sensor->deviceInfo, sensor->addr, MAG3110_Initialization );
101
102 // Stash some needed constants in the SF data structure for this sensor
103 sfg->Mag.iCountsPeruT = MAG3110_COUNTSPERUT;
104 sfg->Mag.fCountsPeruT = (float)MAG3110_COUNTSPERUT; // IAR optimized this out without the #pragma before the function
105 sfg->Mag.fuTPerCount = 1.0F / MAG3110_COUNTSPERUT; // IAR optimized this out without the #pragma before the function
106
107 sensor->isInitialized = F_USING_MAG; // IAR optimized this out without the #pragma before the function
108 sfg->Mag.isEnabled = true; // IAR optimized this out without the #pragma before the function
109
110 return (status);
111 }
112
MAG3110_Read(struct PhysicalSensor * sensor,SensorFusionGlobals * sfg)113 int8_t MAG3110_Read(struct PhysicalSensor *sensor, SensorFusionGlobals *sfg)
114 {
115 uint8_t I2C_Buffer[6]; // I2C read buffer
116 int8_t status; // I2C transaction status
117 int16_t sample[3]; // Reconstructed sample
118
119 if(sensor->isInitialized != F_USING_MAG)
120 {
121 return SENSOR_ERROR_INIT;
122 }
123
124 status = Sensor_I2C_Read(sensor->bus_driver, &sensor->deviceInfo, sensor->addr, MAG3110_DATA_READ, I2C_Buffer );
125 sample[CHX] = (I2C_Buffer[0] << 8) | I2C_Buffer[1];
126 sample[CHY] = (I2C_Buffer[2] << 8) | I2C_Buffer[3];
127 sample[CHZ] = (I2C_Buffer[4] << 8) | I2C_Buffer[5];
128 if (status==SENSOR_ERROR_NONE) {
129 conditionSample(sample); // truncate negative values to -32767
130 sample[CHZ] = -sample[CHZ]; // +Z should point up (MAG3110 Z positive is down)
131 addToFifo((union FifoSensor*) &(sfg->Mag), MAG_FIFO_SIZE, sample);
132 }
133
134 return (status);
135 }
136
137
138 // Each entry in a RegisterWriteList is composed of: register address, value to write, bit-mask to apply to write (0 enables)
139 const registerwritelist_t MAG3110_IDLE[] =
140 {
141 // Set ACTIVE = 0
142 { MAG3110_CTRL_REG1, 0x00, 0x00 },
143 __END_WRITE_DATA__
144 };
145
146 // MAG3110_Idle places the sensor into STANDBY mode (wakeup time = 25ms at ODR = 80Hz)
MAG3110_Idle(struct PhysicalSensor * sensor,SensorFusionGlobals * sfg)147 int8_t MAG3110_Idle(struct PhysicalSensor *sensor, SensorFusionGlobals *sfg)
148 {
149 int32_t status;
150 if(sensor->isInitialized == F_USING_MAG) {
151 status = Sensor_I2C_Write(sensor->bus_driver, &sensor->deviceInfo, sensor->addr, MAG3110_IDLE );
152 sensor->isInitialized = 0;
153 sfg->Mag.isEnabled = false;
154 } else {
155 return SENSOR_ERROR_INIT;
156 }
157 return status;
158 }
159
160 #endif
161
