PX4 Firmware
PX4 Autopilot Software http://px4.io
mpu9250.h
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2016-2019 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 #pragma once
35 
38 #include <lib/ecl/geo/geo.h>
39 #include <px4_platform_common/getopt.h>
40 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
43 
44 #include "MPU9250_mag.h"
45 
46 #if defined(PX4_I2C_OBDEV_MPU9250) || defined(PX4_I2C_BUS_EXPANSION)
47 # define USE_I2C
48 #endif
49 
50 
51 // MPU 9250 registers
52 #define MPUREG_WHOAMI 0x75
53 #define MPUREG_SMPLRT_DIV 0x19
54 #define MPUREG_CONFIG 0x1A
55 #define MPUREG_GYRO_CONFIG 0x1B
56 #define MPUREG_ACCEL_CONFIG 0x1C
57 #define MPUREG_ACCEL_CONFIG2 0x1D
58 #define MPUREG_LPACCEL_ODR 0x1E
59 #define MPUREG_WOM_THRESH 0x1F
60 #define MPUREG_FIFO_EN 0x23
61 #define MPUREG_I2C_MST_CTRL 0x24
62 #define MPUREG_I2C_SLV0_ADDR 0x25
63 #define MPUREG_I2C_SLV0_REG 0x26
64 #define MPUREG_I2C_SLV0_CTRL 0x27
65 #define MPUREG_I2C_SLV1_ADDR 0x28
66 #define MPUREG_I2C_SLV1_REG 0x29
67 #define MPUREG_I2C_SLV1_CTRL 0x2A
68 #define MPUREG_I2C_SLV2_ADDR 0x2B
69 #define MPUREG_I2C_SLV2_REG 0x2C
70 #define MPUREG_I2C_SLV2_CTRL 0x2D
71 #define MPUREG_I2C_SLV3_ADDR 0x2E
72 #define MPUREG_I2C_SLV3_REG 0x2F
73 #define MPUREG_I2C_SLV3_CTRL 0x30
74 #define MPUREG_I2C_SLV4_ADDR 0x31
75 #define MPUREG_I2C_SLV4_REG 0x32
76 #define MPUREG_I2C_SLV4_DO 0x33
77 #define MPUREG_I2C_SLV4_CTRL 0x34
78 #define MPUREG_I2C_SLV4_DI 0x35
79 #define MPUREG_I2C_MST_STATUS 0x36
80 #define MPUREG_INT_PIN_CFG 0x37
81 #define MPUREG_INT_ENABLE 0x38
82 #define MPUREG_INT_STATUS 0x3A
83 #define MPUREG_ACCEL_XOUT_H 0x3B
84 #define MPUREG_ACCEL_XOUT_L 0x3C
85 #define MPUREG_ACCEL_YOUT_H 0x3D
86 #define MPUREG_ACCEL_YOUT_L 0x3E
87 #define MPUREG_ACCEL_ZOUT_H 0x3F
88 #define MPUREG_ACCEL_ZOUT_L 0x40
89 #define MPUREG_TEMP_OUT_H 0x41
90 #define MPUREG_TEMP_OUT_L 0x42
91 #define MPUREG_GYRO_XOUT_H 0x43
92 #define MPUREG_GYRO_XOUT_L 0x44
93 #define MPUREG_GYRO_YOUT_H 0x45
94 #define MPUREG_GYRO_YOUT_L 0x46
95 #define MPUREG_GYRO_ZOUT_H 0x47
96 #define MPUREG_GYRO_ZOUT_L 0x48
97 #define MPUREG_EXT_SENS_DATA_00 0x49
98 #define MPUREG_I2C_SLV0_D0 0x63
99 #define MPUREG_I2C_SLV1_D0 0x64
100 #define MPUREG_I2C_SLV2_D0 0x65
101 #define MPUREG_I2C_SLV3_D0 0x66
102 #define MPUREG_I2C_MST_DELAY_CTRL 0x67
103 #define MPUREG_SIGNAL_PATH_RESET 0x68
104 #define MPUREG_MOT_DETECT_CTRL 0x69
105 #define MPUREG_USER_CTRL 0x6A
106 #define MPUREG_PWR_MGMT_1 0x6B
107 #define MPUREG_PWR_MGMT_2 0x6C
108 #define MPUREG_FIFO_COUNTH 0x72
109 #define MPUREG_FIFO_COUNTL 0x73
110 #define MPUREG_FIFO_R_W 0x74
111 
112 // Configuration bits MPU 9250
113 #define BIT_SLEEP 0x40
114 #define BIT_H_RESET 0x80
115 #define MPU_CLK_SEL_AUTO 0x01
116 
117 #define BITS_GYRO_ST_X 0x80
118 #define BITS_GYRO_ST_Y 0x40
119 #define BITS_GYRO_ST_Z 0x20
120 #define BITS_FS_250DPS 0x00
121 #define BITS_FS_500DPS 0x08
122 #define BITS_FS_1000DPS 0x10
123 #define BITS_FS_2000DPS 0x18
124 #define BITS_FS_MASK 0x18
125 
126 #define BITS_DLPF_CFG_250HZ 0x00
127 #define BITS_DLPF_CFG_184HZ 0x01
128 #define BITS_DLPF_CFG_92HZ 0x02
129 #define BITS_DLPF_CFG_41HZ 0x03
130 #define BITS_DLPF_CFG_20HZ 0x04
131 #define BITS_DLPF_CFG_10HZ 0x05
132 #define BITS_DLPF_CFG_5HZ 0x06
133 #define BITS_DLPF_CFG_3600HZ 0x07
134 #define BITS_DLPF_CFG_MASK 0x07
135 
136 #define BITS_ACCEL_CONFIG2_41HZ 0x03
137 
138 #define BIT_RAW_RDY_EN 0x01
139 #define BIT_INT_ANYRD_2CLEAR 0x10
140 #define BIT_INT_BYPASS_EN 0x02
141 
142 #define BIT_I2C_READ_FLAG 0x80
143 
144 #define BIT_I2C_SLV0_NACK 0x01
145 #define BIT_I2C_FIFO_EN 0x40
146 #define BIT_I2C_MST_EN 0x20
147 #define BIT_I2C_IF_DIS 0x10
148 #define BIT_FIFO_RST 0x04
149 #define BIT_I2C_MST_RST 0x02
150 #define BIT_SIG_COND_RST 0x01
151 
152 #define BIT_I2C_SLV0_EN 0x80
153 #define BIT_I2C_SLV0_BYTE_SW 0x40
154 #define BIT_I2C_SLV0_REG_DIS 0x20
155 #define BIT_I2C_SLV0_REG_GRP 0x10
156 
157 #define BIT_I2C_MST_MULT_MST_EN 0x80
158 #define BIT_I2C_MST_WAIT_FOR_ES 0x40
159 #define BIT_I2C_MST_SLV_3_FIFO_EN 0x20
160 #define BIT_I2C_MST_P_NSR 0x10
161 #define BITS_I2C_MST_CLOCK_258HZ 0x08
162 #define BITS_I2C_MST_CLOCK_400HZ 0x0D
163 
164 #define BIT_I2C_SLV0_DLY_EN 0x01
165 #define BIT_I2C_SLV1_DLY_EN 0x02
166 #define BIT_I2C_SLV2_DLY_EN 0x04
167 #define BIT_I2C_SLV3_DLY_EN 0x08
168 
169 #define MPU_WHOAMI_9250 0x71
170 #define MPU_WHOAMI_6500 0x70
171 
172 #define MPU9250_ACCEL_DEFAULT_RATE 1000
173 #define MPU9250_ACCEL_MAX_OUTPUT_RATE 280
174 #define MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30
175 #define MPU9250_GYRO_DEFAULT_RATE 1000
176 /* rates need to be the same between accel and gyro */
177 #define MPU9250_GYRO_MAX_OUTPUT_RATE MPU9250_ACCEL_MAX_OUTPUT_RATE
178 #define MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30
179 
180 #define MPU9250_DEFAULT_ONCHIP_FILTER_FREQ 92
181 
182 #pragma pack(push, 1)
183 /**
184  * Report conversation within the mpu, including command byte and
185  * interrupt status.
186  */
187 struct MPUReport {
188  uint8_t cmd;
189  uint8_t status;
190  uint8_t accel_x[2];
191  uint8_t accel_y[2];
192  uint8_t accel_z[2];
193  uint8_t temp[2];
194  uint8_t gyro_x[2];
195  uint8_t gyro_y[2];
196  uint8_t gyro_z[2];
197  struct ak8963_regs mag;
198 };
199 #pragma pack(pop)
200 
201 /*
202  The MPU9250 can only handle high bus speeds on the sensor and
203  interrupt status registers. All other registers have a maximum 1MHz
204  Communication with all registers of the device is performed using either
205  I2C at 400kHz or SPI at 1MHz. For applications requiring faster communications,
206  the sensor and interrupt registers may be read using SPI at 20MHz
207  */
208 #define MPU9250_LOW_BUS_SPEED 0
209 #define MPU9250_HIGH_BUS_SPEED 0x8000
210 #define MPU9250_REG_MASK 0x00FF
211 # define MPU9250_IS_HIGH_SPEED(r) ((r) & MPU9250_HIGH_BUS_SPEED)
212 # define MPU9250_REG(r) ((r) & MPU9250_REG_MASK)
213 # define MPU9250_SET_SPEED(r, s) ((r)|(s))
214 # define MPU9250_HIGH_SPEED_OP(r) MPU9250_SET_SPEED((r), MPU9250_HIGH_BUS_SPEED)
215 # define MPU9250_LOW_SPEED_OP(r) ((r) &~MPU9250_HIGH_BUS_SPEED)
216 
217 /* interface factories */
218 extern device::Device *MPU9250_SPI_interface(int bus, uint32_t cs);
219 extern device::Device *MPU9250_I2C_interface(int bus, uint32_t address);
220 extern int MPU9250_probe(device::Device *dev);
221 
222 typedef device::Device *(*MPU9250_constructor)(int, uint32_t);
223 
224 class MPU9250_mag;
225 
226 class MPU9250 : public px4::ScheduledWorkItem
227 {
228 public:
229  MPU9250(device::Device *interface, device::Device *mag_interface, enum Rotation rotation);
230  virtual ~MPU9250();
231 
232  virtual int init();
233  uint8_t get_whoami() { return _whoami; }
234 
235  /**
236  * Diagnostics - print some basic information about the driver.
237  */
238  void print_info();
239 
240 protected:
242  uint8_t _whoami{0}; /** whoami result */
243 
244  virtual int probe();
245 
246  friend class MPU9250_mag;
247 
248  void Run() override;
249 
250 private:
251 
254 
256 
257  unsigned _call_interval{1000};
258 
259  unsigned _dlpf_freq{0};
260 
261  unsigned _sample_rate{1000};
262 
268 
269  uint8_t _register_wait{0};
270  uint64_t _reset_wait{0};
271 
272  // this is used to support runtime checking of key
273  // configuration registers to detect SPI bus errors and sensor
274  // reset
275 
276  static constexpr int MPU9250_NUM_CHECKED_REGISTERS{11};
277  static const uint16_t _mpu9250_checked_registers[MPU9250_NUM_CHECKED_REGISTERS];
278 
279  const uint16_t *_checked_registers{nullptr};
280 
281  uint8_t _checked_values[MPU9250_NUM_CHECKED_REGISTERS] {};
282  uint8_t _checked_bad[MPU9250_NUM_CHECKED_REGISTERS] {};
283  unsigned _checked_next{0};
284  unsigned _num_checked_registers{0};
285 
286 
287  // last temperature reading for print_info()
288  float _last_temperature{0.0f};
289 
290  bool check_null_data(uint16_t *data, uint8_t size);
291  bool check_duplicate(uint8_t *accel_data);
292 
293  // keep last accel reading for duplicate detection
294  uint8_t _last_accel_data[6] {};
295  bool _got_duplicate{false};
296 
297  void start();
298  void stop();
299  int reset();
300 
301  /**
302  * Resets the main chip (excluding the magnetometer if any).
303  */
304  int reset_mpu();
305 
306  /**
307  * Fetch measurements from the sensor and update the report buffers.
308  */
309  void measure();
310 
311  /**
312  * Read a register from the mpu
313  *
314  * @param The register to read.
315  * @param The bus speed to read with.
316  * @return The value that was read.
317  */
318  uint8_t read_reg(unsigned reg, uint32_t speed = MPU9250_LOW_BUS_SPEED);
319  uint16_t read_reg16(unsigned reg);
320 
321 
322  /**
323  * Read a register range from the mpu
324  *
325  * @param The start address to read from.
326  * @param The bus speed to read with.
327  * @param The address of the target data buffer.
328  * @param The count of bytes to be read.
329  * @return The value that was read.
330  */
331  uint8_t read_reg_range(unsigned start_reg, uint32_t speed, uint8_t *buf, uint16_t count);
332 
333  /**
334  * Write a register in the mpu
335  *
336  * @param reg The register to write.
337  * @param value The new value to write.
338  */
339  void write_reg(unsigned reg, uint8_t value);
340 
341  /**
342  * Modify a register in the mpu
343  *
344  * Bits are cleared before bits are set.
345  *
346  * @param reg The register to modify.
347  * @param clearbits Bits in the register to clear.
348  * @param setbits Bits in the register to set.
349  */
350  void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
351 
352  /**
353  * Write a register in the mpu, updating _checked_values
354  *
355  * @param reg The register to write.
356  * @param value The new value to write.
357  */
358  void write_checked_reg(unsigned reg, uint8_t value);
359 
360  /**
361  * Modify a checked register in the mpu
362  *
363  * Bits are cleared before bits are set.
364  *
365  * @param reg The register to modify.
366  * @param clearbits Bits in the register to clear.
367  * @param setbits Bits in the register to set.
368  */
369  void modify_checked_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
370 
371  /**
372  * Set the mpu measurement range.
373  *
374  * @param max_g The maximum G value the range must support.
375  * @return OK if the value can be supported, -ERANGE otherwise.
376  */
377  int set_accel_range(unsigned max_g);
378 
379  /**
380  * Swap a 16-bit value read from the mpu to native byte order.
381  */
382  uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); }
383 
384  /**
385  * Get the internal / external state
386  *
387  * @return true if the sensor is not on the main MCU board
388  */
389  bool is_external() { return _interface->external(); }
390 
391  /*
392  set low pass filter frequency
393  */
394  void _set_dlpf_filter(uint16_t frequency_hz);
395 
396  /*
397  set sample rate (approximate) - 1kHz to 5Hz
398  */
399  void _set_sample_rate(unsigned desired_sample_rate_hz);
400 
401  /*
402  check that key registers still have the right value
403  */
404  void check_registers();
405 };
perf_counter_t _good_transfers
Definition: mpu9250.h:266
Definition of geo / math functions to perform geodesic calculations.
MPU9250_mag _mag
Definition: mpu9250.h:255
device::Device * _interface
Definition: mpu9250.h:241
void write_reg(unsigned reg, uint8_t value)
void measure()
Definition: MPU9250_mag.cpp:75
A set of useful macros for enhanced runtime and compile time error detection and warning suppression...
int reset(enum LPS22HB_BUS busid)
Reset the driver.
uint8_t accel_z[2]
Definition: icm20948.h:316
static void stop()
Definition: dataman.cpp:1491
uint8_t temp[2]
Definition: icm20948.h:317
uint8_t status
Definition: icm20948.h:313
uint8_t accel_y[2]
Definition: icm20948.h:315
device::Device * MPU9250_I2C_interface(int bus, uint32_t address)
device::Device * MPU9250_SPI_interface(int bus, uint32_t cs)
Definition: mpu9250_spi.cpp:83
Report conversation within the mpu, including command byte and interrupt status.
Definition: icm20948.h:311
Header common to all counters.
void init()
Activates/configures the hardware registers.
Definition of commonly used conversions.
uint8_t cmd
Definition: icm20948.h:312
uint8_t gyro_z[2]
Definition: icm20948.h:320
perf_counter_t _bad_transfers
Definition: mpu9250.h:264
Rotation
Enum for board and external compass rotations.
Definition: rotation.h:51
uint8_t * data
Definition: dataman.cpp:149
uint8_t read_reg(unsigned reg)
uint16_t swap16(uint16_t val)
Swap a 16-bit value read from the mpu to native byte order.
Definition: mpu9250.h:382
int MPU9250_probe(device::Device *dev)
struct ak09916_regs mag
Definition: icm20948.h:321
uint8_t get_whoami()
Definition: mpu9250.h:233
Helper class implementing the magnetometer driver node.
Definition: MPU9250_mag.h:112
uint8_t accel_x[2]
Definition: icm20948.h:314
static int start()
Definition: dataman.cpp:1452
#define MPU9250_LOW_BUS_SPEED
Definition: mpu9250.h:208
uint8_t gyro_y[2]
Definition: icm20948.h:319
virtual bool external() const
Definition: Device.hpp:237
perf_counter_t _duplicates
Definition: mpu9250.h:267
perf_counter_t _bad_registers
Definition: mpu9250.h:265
Fundamental base class for all physical drivers (I2C, SPI).
Definition: Device.hpp:65
bool is_external()
Get the internal / external state.
Definition: mpu9250.h:389
PX4Accelerometer _px4_accel
Definition: mpu9250.h:252
PX4Gyroscope _px4_gyro
Definition: mpu9250.h:253
uint8_t gyro_x[2]
Definition: icm20948.h:318
perf_counter_t _sample_perf
Definition: mpu9250.h:263
static constexpr uint8_t _checked_registers[]
Definition: FXAS21002C.cpp:178