2 * This file is part of Cleanflight and Betaflight.
4 * Cleanflight and Betaflight are free software. You can redistribute
5 * this software and/or modify this software under the terms of the
6 * GNU General Public License as published by the Free Software
7 * Foundation, either version 3 of the License, or (at your option)
10 * Cleanflight and Betaflight are distributed in the hope that they
11 * will be useful, but WITHOUT ANY WARRANTY; without even the implied
12 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13 * See the GNU General Public License for more details.
15 * You should have received a copy of the GNU General Public License
16 * along with this software.
18 * If not, see <http://www.gnu.org/licenses/>.
30 #include "common/axis.h"
31 #include "common/maths.h"
32 #include "common/utils.h"
34 #include "drivers/bus.h"
35 #include "drivers/bus_i2c.h"
36 #include "drivers/bus_i2c_busdev.h"
37 #include "drivers/sensor.h"
38 #include "drivers/time.h"
41 #include "compass_ak8975.h"
43 // This sensor is available in MPU-9150.
44 // This driver only support I2C mode, direct and in bypass configuration.
46 // AK8975, mag sensor address
47 #define AK8975_MAG_I2C_ADDRESS 0x0C
48 #define AK8975_DEVICE_ID 0x48
51 #define AK8975_MAG_REG_WIA 0x00
52 #define AK8975_MAG_REG_INFO 0x01
53 #define AK8975_MAG_REG_ST1 0x02
54 #define AK8975_MAG_REG_HXL 0x03
55 #define AK8975_MAG_REG_HXH 0x04
56 #define AK8975_MAG_REG_HYL 0x05
57 #define AK8975_MAG_REG_HYH 0x06
58 #define AK8975_MAG_REG_HZL 0x07
59 #define AK8975_MAG_REG_HZH 0x08
60 #define AK8975_MAG_REG_ST2 0x09
61 #define AK8975_MAG_REG_CNTL 0x0A
62 #define AK8975_MAG_REG_ASTC 0x0C // self test
63 #define AK8975_MAG_REG_I2CDIS 0x0F
64 #define AK8975_MAG_REG_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value
65 #define AK8975_MAG_REG_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value
66 #define AK8975_MAG_REG_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value
68 #define ST1_REG_DATA_READY 0x01
70 #define ST2_REG_DATA_ERROR 0x04
71 #define ST2_REG_MAG_SENSOR_OVERFLOW 0x08
73 #define CNTL_MODE_POWER_DOWN 0x00
74 #define CNTL_MODE_ONCE 0x01
75 #define CNTL_MODE_CONT1 0x02
76 #define CNTL_MODE_FUSE_ROM 0x0F
77 #define CNTL_BIT_14_BIT 0x00
78 #define CNTL_BIT_16_BIT 0x10
80 static bool ak8975Init(magDev_t
*mag
)
85 const busDevice_t
*busdev
= &mag
->busdev
;
87 busDeviceRegister(busdev
);
89 busWriteRegister(busdev
, AK8975_MAG_REG_CNTL
, CNTL_MODE_POWER_DOWN
); // power down before entering fuse mode
92 busWriteRegister(busdev
, AK8975_MAG_REG_CNTL
, CNTL_MODE_FUSE_ROM
); // Enter Fuse ROM access mode
95 busReadRegisterBuffer(busdev
, AK8975_MAG_REG_ASAX
, asa
, sizeof(asa
)); // Read the x-, y-, and z-axis asa values
98 mag
->magGain
[X
] = asa
[X
] + 128;
99 mag
->magGain
[Y
] = asa
[Y
] + 128;
100 mag
->magGain
[Z
] = asa
[Z
] + 128;
102 busWriteRegister(busdev
, AK8975_MAG_REG_CNTL
, CNTL_MODE_POWER_DOWN
); // power down after reading.
105 // Clear status registers
106 busReadRegisterBuffer(busdev
, AK8975_MAG_REG_ST1
, &status
, 1);
107 busReadRegisterBuffer(busdev
, AK8975_MAG_REG_ST2
, &status
, 1);
109 // Trigger first measurement
110 busWriteRegister(busdev
, AK8975_MAG_REG_CNTL
, CNTL_BIT_16_BIT
| CNTL_MODE_ONCE
);
114 static int16_t parseMag(uint8_t *raw
, int16_t gain
) {
115 int ret
= (int16_t)(raw
[1] << 8 | raw
[0]) * gain
/ 256;
116 return constrain(ret
, INT16_MIN
, INT16_MAX
);
119 static bool ak8975Read(magDev_t
*mag
, int16_t *magData
)
125 const busDevice_t
*busdev
= &mag
->busdev
;
127 ack
= busReadRegisterBuffer(busdev
, AK8975_MAG_REG_ST1
, &status
, 1);
128 if (!ack
|| (status
& ST1_REG_DATA_READY
) == 0) {
132 busReadRegisterBuffer(busdev
, AK8975_MAG_REG_HXL
, buf
, 6); // read from AK8975_MAG_REG_HXL to AK8975_MAG_REG_HZH
134 ack
= busReadRegisterBuffer(busdev
, AK8975_MAG_REG_ST2
, &status
, 1);
139 busWriteRegister(busdev
, AK8975_MAG_REG_CNTL
, CNTL_BIT_16_BIT
| CNTL_MODE_ONCE
); // start reading again uint8_t status2 = buf[6];
141 if (status
& ST2_REG_DATA_ERROR
) {
145 if (status
& ST2_REG_MAG_SENSOR_OVERFLOW
) {
149 magData
[X
] = -parseMag(buf
+ 0, mag
->magGain
[X
]);
150 magData
[Y
] = -parseMag(buf
+ 2, mag
->magGain
[Y
]);
151 magData
[Z
] = -parseMag(buf
+ 4, mag
->magGain
[Z
]);
156 bool ak8975Detect(magDev_t
*mag
)
160 busDevice_t
*busdev
= &mag
->busdev
;
162 if (busdev
->bustype
== BUSTYPE_I2C
&& busdev
->busdev_u
.i2c
.address
== 0) {
163 busdev
->busdev_u
.i2c
.address
= AK8975_MAG_I2C_ADDRESS
;
166 bool ack
= busReadRegisterBuffer(busdev
, AK8975_MAG_REG_WIA
, &sig
, 1);
168 if (!ack
|| sig
!= AK8975_DEVICE_ID
) // 0x48 / 01001000 / 'H'
171 mag
->init
= ak8975Init
;
172 mag
->read
= ak8975Read
;