PX4 Firmware
PX4 Autopilot Software http://px4.io
MPU9250_mag.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file mag.cpp
36  *
37  * Driver for the ak8963 magnetometer within the Invensense mpu9250
38  *
39  * @author Robert Dickenson
40  *
41  */
42 
43 #include <px4_platform_common/px4_config.h>
44 #include <px4_platform_common/log.h>
45 #include <px4_platform_common/time.h>
46 #include <lib/perf/perf_counter.h>
47 #include <drivers/drv_hrt.h>
48 
49 #include "MPU9250_mag.h"
50 #include "mpu9250.h"
51 
52 // If interface is non-null, then it will used for interacting with the device.
53 // Otherwise, it will passthrough the parent MPU9250
54 MPU9250_mag::MPU9250_mag(MPU9250 *parent, device::Device *interface, enum Rotation rotation) :
55  _interface(interface),
56  _px4_mag(parent->_interface->get_device_id(), (parent->_interface->external() ? ORB_PRIO_MAX : ORB_PRIO_HIGH),
57  rotation),
58  _parent(parent),
59  _mag_overruns(perf_alloc(PC_COUNT, MODULE_NAME": mag_overruns")),
60  _mag_overflows(perf_alloc(PC_COUNT, MODULE_NAME": mag_overflows")),
61  _mag_errors(perf_alloc(PC_COUNT, MODULE_NAME": mag_errors"))
62 {
65 }
66 
68 {
72 }
73 
74 void
76 {
77  const hrt_abstime timestamp_sample = hrt_absolute_time();
78 
79  uint8_t st1 = 0;
80  int ret = _interface->read(AK8963REG_ST1, &st1, sizeof(st1));
81 
82  if (ret != OK) {
84  return;
85  }
86 
87  /* Check if data ready is set.
88  * This is not described to be set in continuous mode according to the
89  * MPU9250 datasheet. However, the datasheet of the 8963 recommends to
90  * check data ready before doing the read and before triggering the
91  * next measurement by reading ST2. */
92  if (!(st1 & AK09916_ST1_DRDY)) {
93  return;
94  }
95 
96  /* Monitor if data overrun flag is ever set. */
97  if (st1 & 0x02) {
99  }
100 
101  ak8963_regs data{};
102  ret = _interface->read(AK8963REG_ST1, &data, sizeof(data));
103 
104  if (ret != OK) {
106  return;
107  }
108 
109  /* Monitor magnetic sensor overflow flag. */
110  if (data.st2 & 0x08) {
112  }
113 
114  _measure(timestamp_sample, data);
115 }
116 
117 void
119 {
120  /* Check if data ready is set.
121  * This is not described to be set in continuous mode according to the
122  * MPU9250 datasheet. However, the datasheet of the 8963 recommends to
123  * check data ready before doing the read and before triggering the
124  * next measurement by reading ST2.
125  *
126  * If _measure is used in passthrough mode, all the data is already
127  * fetched, however, we should still not use the data if the data ready
128  * is not set. This has lead to intermittent spikes when the data was
129  * being updated while getting read.
130  */
131  if (!(data.st1 & AK09916_ST1_DRDY)) {
132  return;
133  }
134 
137 
138  /*
139  * Align axes - note the accel & gyro are also re-aligned so this
140  * doesn't look obvious with the datasheet
141  */
142  _px4_mag.update(timestamp_sample, data.x, -data.y, -data.z);
143 }
144 
145 void
146 MPU9250_mag::set_passthrough(uint8_t reg, uint8_t size, uint8_t *out)
147 {
148  uint8_t addr;
149 
150  _parent->write_reg(MPUREG_I2C_SLV0_CTRL, 0); // ensure slave r/w is disabled before changing the registers
151 
152  if (out) {
154  addr = AK8963_I2C_ADDR;
155 
156  } else {
158  }
159 
163 }
164 
165 void
166 MPU9250_mag::read_block(uint8_t reg, uint8_t *val, uint8_t count)
167 {
168  _parent->_interface->read(reg, val, count);
169 }
170 
171 void
172 MPU9250_mag::passthrough_read(uint8_t reg, uint8_t *buf, uint8_t size)
173 {
174  set_passthrough(reg, size);
175  px4_usleep(25 + 25 * size); // wait for the value to be read from slave
177  _parent->write_reg(MPUREG_I2C_SLV0_CTRL, 0); // disable new reads
178 }
179 
180 uint8_t
181 MPU9250_mag::read_reg(unsigned int reg)
182 {
183  uint8_t buf{};
184 
185  if (_interface == nullptr) {
186  passthrough_read(reg, &buf, 0x01);
187 
188  } else {
189  _interface->read(reg, &buf, 1);
190  }
191 
192  return buf;
193 }
194 
195 bool
197 {
198  deviceid = read_reg(AK8963REG_WIA);
199 
200  return (AK8963_DEVICE_ID == deviceid);
201 }
202 
203 /*
204  * 400kHz I2C bus speed = 2.5us per bit = 25us per byte
205  */
206 void
207 MPU9250_mag::passthrough_write(uint8_t reg, uint8_t val)
208 {
209  set_passthrough(reg, 1, &val);
210  px4_usleep(50); // wait for the value to be written to slave
211  _parent->write_reg(MPUREG_I2C_SLV0_CTRL, 0); // disable new writes
212 }
213 
214 void
215 MPU9250_mag::write_reg(unsigned reg, uint8_t value)
216 {
217  // general register transfer at low clock speed
218  if (_interface == nullptr) {
219  passthrough_write(reg, value);
220 
221  } else {
222  _interface->write(MPU9250_LOW_SPEED_OP(reg), &value, 1);
223  }
224 }
225 
226 int
228 {
229  // First initialize it to use the bus
230  int rv = ak8963_setup();
231 
232  if (rv == OK) {
233  // Now reset the mag
235 
236  // Then re-initialize the bus/mag
237  rv = ak8963_setup();
238  }
239 
240  return rv;
241 }
242 
243 bool
245 {
246  uint8_t response[3];
247  float ak8963_ASA[3];
248 
250  px4_usleep(50);
251 
252  if (_interface != nullptr) {
253  _interface->read(AK8963REG_ASAX, response, 3);
254 
255  } else {
256  passthrough_read(AK8963REG_ASAX, response, 3);
257  }
258 
260 
261  for (int i = 0; i < 3; i++) {
262  if (0 != response[i] && 0xff != response[i]) {
263  ak8963_ASA[i] = ((float)(response[i] - 128) / 256.0f) + 1.0f;
264 
265  } else {
266  return false;
267  }
268  }
269 
270  _px4_mag.set_sensitivity(ak8963_ASA[0], ak8963_ASA[1], ak8963_ASA[2]);
271 
272  return true;
273 }
274 
275 int
277 {
278  /* When _interface is null we are using SPI and must
279  * use the parent interface to configure the device to act
280  * in master mode (SPI to I2C bridge)
281  */
282  if (_interface == nullptr) {
283  if (_parent->_whoami == MPU_WHOAMI_9250) {
286  }
287 
288  } else {
290  }
291 
292  return OK;
293 }
294 int
296 {
297  int retries = 10;
298 
299  do {
302 
303  uint8_t id = 0;
304 
305  if (ak8963_check_id(id)) {
306  break;
307  }
308 
309  retries--;
310  PX4_WARN("AK8963: bad id %d retries %d", id, retries);
312  px4_usleep(100);
313  } while (retries > 0);
314 
315  if (retries > 0) {
316  retries = 10;
317 
318  while (!ak8963_read_adjustments() && retries) {
319  retries--;
320  PX4_ERR("AK8963: failed to read adjustment data. Retries %d", retries);
321 
323  px4_usleep(100);
326  }
327  }
328 
329  if (retries == 0) {
330  PX4_ERR("AK8963: failed to initialize, disabled!");
333 
334  return -EIO;
335  }
336 
337  if (_parent->_whoami == MPU_WHOAMI_9250) {
339  }
340 
341  if (_interface == nullptr) {
342  // Configure mpu' I2C Master interface to read ak8963 data into to fifo
343  if (_parent->_whoami == MPU_WHOAMI_9250) {
345  }
346  }
347 
348  return OK;
349 }
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
Modify a register in the mpu.
Definition: mpu9250.cpp:420
#define AK8963_16BIT_ADC
Definition: MPU9250_mag.h:60
#define MPUREG_I2C_SLV0_D0
Definition: icm20948.h:98
#define AK8963REG_ASAX
Definition: MPU9250_mag.h:50
void _measure(hrt_abstime timestamp_sample, ak8963_regs data)
#define BIT_I2C_SLV0_EN
Definition: icm20948.h:152
#define AK8963_DEVICE_ID
Definition: MPU9250_mag.h:45
MPU9250_mag(MPU9250 *parent, device::Device *interface, enum Rotation rotation)
Definition: MPU9250_mag.cpp:54
device::Device * _interface
Definition: MPU9250_mag.h:132
#define MPU_WHOAMI_9250
Definition: mpu9250.h:169
void set_device_type(uint8_t devtype)
void set_error_count(uint64_t error_count)
static constexpr float MPU9250_MAG_RANGE_GA
Definition: MPU9250_mag.h:41
#define BIT_I2C_MST_WAIT_FOR_ES
Definition: icm20948.h:158
device::Device * _interface
Definition: mpu9250.h:241
void read_block(uint8_t reg, uint8_t *val, uint8_t count)
#define MPUREG_I2C_SLV0_CTRL
Definition: icm20948.h:64
uint8_t st1
Definition: MPU9250_mag.h:97
void write_reg(unsigned reg, uint8_t value)
void measure()
Definition: MPU9250_mag.cpp:75
#define AK8963_CONTINUOUS_MODE2
Definition: MPU9250_mag.h:56
#define AK8963REG_WIA
Definition: MPU9250_mag.h:47
void modify_checked_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
Modify a checked register in the mpu.
Definition: mpu9250.cpp:429
int16_t y
Definition: MPU9250_mag.h:99
count the number of times an event occurs
Definition: perf_counter.h:55
#define AK09916_ST1_DRDY
Definition: ICM20948_mag.h:88
uint8_t _whoami
Definition: mpu9250.h:242
int ak8963_setup_master_i2c()
perf_counter_t _mag_errors
Definition: MPU9250_mag.h:153
High-resolution timer with callouts and timekeeping.
#define MPUREG_I2C_SLV0_REG
Definition: icm20948.h:63
void passthrough_write(uint8_t reg, uint8_t val)
void set_external(bool external)
#define AK8963_I2C_ADDR
Definition: MPU9250_mag.h:44
int16_t x
Definition: MPU9250_mag.h:98
MPU9250 * _parent
Definition: MPU9250_mag.h:147
#define BITS_I2C_MST_CLOCK_400HZ
Definition: icm20948.h:162
perf_counter_t _mag_overruns
Definition: MPU9250_mag.h:151
void perf_count(perf_counter_t handle)
Count a performance event.
#define AK8963_POWERDOWN_MODE
Definition: MPU9250_mag.h:57
void set_sensitivity(float x, float y, float z)
void perf_free(perf_counter_t handle)
Free a counter.
void set_temperature(float temperature)
#define perf_alloc(a, b)
Definition: px4io.h:59
void update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z)
Rotation
Enum for board and external compass rotations.
Definition: rotation.h:51
uint8_t * data
Definition: dataman.cpp:149
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
#define MPUREG_EXT_SENS_DATA_00
Definition: icm20948.h:97
#define AK8963_FUZE_MODE
Definition: MPU9250_mag.h:59
uint8_t read_reg(unsigned reg)
#define MPUREG_I2C_MST_CTRL
Definition: icm20948.h:61
void passthrough_read(uint8_t reg, uint8_t *buf, uint8_t size)
#define BIT_I2C_MST_EN
Definition: icm20948.h:146
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
virtual int write(unsigned address, void *data, unsigned count)
Write directly to the device.
Definition: Device.hpp:115
#define AK8963REG_CNTL1
Definition: MPU9250_mag.h:51
void set_scale(float scale)
#define MPUREG_I2C_SLV0_ADDR
Definition: icm20948.h:62
perf_counter_t _mag_overflows
Definition: MPU9250_mag.h:152
float _last_temperature
Definition: mpu9250.h:288
#define DRV_MAG_DEVTYPE_MPU9250
Definition: drv_sensor.h:58
#define AK8963REG_CNTL2
Definition: MPU9250_mag.h:52
bool ak8963_read_adjustments()
#define AK8963_RESET
Definition: MPU9250_mag.h:62
uint64_t perf_event_count(perf_counter_t handle)
Return current event_count.
Fundamental base class for all physical drivers (I2C, SPI).
Definition: Device.hpp:65
PX4Magnetometer _px4_mag
Definition: MPU9250_mag.h:145
bool is_external()
Get the internal / external state.
Definition: mpu9250.h:389
void set_passthrough(uint8_t reg, uint8_t size, uint8_t *out=NULL)
#define BIT_I2C_READ_FLAG
Definition: icm20948.h:142
void write_reg(unsigned reg, uint8_t value)
Write a register in the mpu.
Definition: mpu9250.cpp:413
int ak8963_setup()
int ak8963_reset()
#define AK8963REG_ST1
Definition: MPU9250_mag.h:48
#define OK
Definition: uavcan_main.cpp:71
bool ak8963_check_id(uint8_t &id)
#define MPU9250_LOW_SPEED_OP(r)
Definition: mpu9250.h:215
#define BIT_I2C_MST_P_NSR
Definition: icm20948.h:160
#define BIT_I2C_MST_RST
Definition: icm20948.h:149
#define MPUREG_USER_CTRL
Definition: icm20948.h:105
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
virtual int read(unsigned address, void *data, unsigned count)
Read directly from the device.
Definition: Device.hpp:103
int16_t z
Definition: MPU9250_mag.h:100
Performance measuring tools.