PX4 Firmware
PX4 Autopilot Software http://px4.io
mpu9250.cpp
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 /**
35  * @file mpu9250.cpp
36  *
37  * Driver for the Invensense MPU9250 connected via I2C or SPI.
38  *
39  * @author Andrew Tridgell
40  *
41  * based on the mpu6000 driver
42  */
43 
44 #include "mpu9250.h"
45 
46 /*
47  we set the timer interrupt to run a bit faster than the desired
48  sample rate and then throw away duplicates by comparing
49  accelerometer values. This time reduction is enough to cope with
50  worst case timing jitter due to other timers
51  */
52 #define MPU9250_TIMER_REDUCTION 200
53 
54 /* Set accel range used */
55 #define ACCEL_RANGE_G 16
56 /*
57  list of registers that will be checked in check_registers(). Note
58  that MPUREG_PRODUCT_ID must be first in the list.
59  */
60 const uint16_t MPU9250::_mpu9250_checked_registers[MPU9250_NUM_CHECKED_REGISTERS] = { MPUREG_WHOAMI,
71  };
72 
73 MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, enum Rotation rotation) :
74  ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())),
75  _interface(interface),
76  _px4_accel(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_MAX : ORB_PRIO_HIGH), rotation),
77  _px4_gyro(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_MAX : ORB_PRIO_HIGH), rotation),
78  _mag(this, mag_interface, rotation),
80  _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
81  _bad_transfers(perf_alloc(PC_COUNT, MODULE_NAME": bad_trans")),
82  _bad_registers(perf_alloc(PC_COUNT, MODULE_NAME": bad_reg")),
83  _good_transfers(perf_alloc(PC_COUNT, MODULE_NAME": good_trans")),
84  _duplicates(perf_alloc(PC_COUNT, MODULE_NAME": dupe"))
85 {
88 }
89 
91 {
92  // make sure we are truly inactive
93  stop();
94 
95  // delete the perf counter
101 }
102 
103 int
105 {
106  /*
107  * If the MPU is using I2C we should reduce the sample rate to 200Hz and
108  * make the integration autoreset faster so that we integrate just one
109  * sample since the sampling rate is already low.
110  */
112 
113  if (is_i2c) {
114  _sample_rate = 200;
115  }
116 
117  int ret = probe();
118 
119  if (ret != OK) {
120  PX4_DEBUG("probe failed");
121  return ret;
122  }
123 
124  _reset_wait = hrt_absolute_time() + 100000;
125 
126  if (reset_mpu() != OK) {
127  PX4_ERR("Exiting! Device failed to take initialization");
128  return ret;
129  }
130 
131  /* Magnetometer setup */
132  if (_whoami == MPU_WHOAMI_9250) {
133 
134 #ifdef USE_I2C
135  px4_usleep(100);
136 
137  if (!_mag.is_passthrough() && _mag._interface->init() != PX4_OK) {
138  PX4_ERR("failed to setup ak8963 interface");
139  }
140 
141 #endif /* USE_I2C */
142 
143  ret = _mag.ak8963_reset();
144 
145  if (ret != OK) {
146  PX4_DEBUG("mag reset failed");
147  return ret;
148  }
149  }
150 
151  start();
152 
153  return ret;
154 }
155 
156 int
158 {
159  /* When the mpu9250 starts from 0V the internal power on circuit
160  * per the data sheet will require:
161  *
162  * Start-up time for register read/write From power-up Typ:11 max:100 ms
163  *
164  */
165  px4_usleep(110000);
166 
167  // Hold off sampling until done (100 MS will be shortened)
168  _reset_wait = hrt_absolute_time() + 100000;
169 
170  int ret = reset_mpu();
171 
172  if (ret == OK && (_whoami == MPU_WHOAMI_9250)) {
173  ret = _mag.ak8963_reset();
174  }
175 
177 
178  return ret;
179 }
180 
181 int
183 {
184  switch (_whoami) {
185  case MPU_WHOAMI_9250:
186  case MPU_WHOAMI_6500:
190  px4_usleep(1000);
191  break;
192  }
193 
194  // Enable I2C bus or Disable I2C bus (recommended on data sheet)
197 
198  // SAMPLE RATE
200 
202 
203  // Gyro scale 2000 deg/s ()
204  switch (_whoami) {
205  case MPU_WHOAMI_9250:
206  case MPU_WHOAMI_6500:
208  break;
209  }
210 
211  // correct gyro scale factors
212  // scale to rad/s in SI units
213  // 2000 deg/s = (2000/180)*PI = 34.906585 rad/s
214  // scaling factor:
215  // 1/(2^15)*(2000/180)*PI
216  _px4_gyro.set_scale(0.0174532 / 16.4); //1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F);
217 
219 
220  // INT CFG => Interrupt on Data Ready
221  write_checked_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready
222 
223 #ifdef USE_I2C
224  bool bypass = !_mag.is_passthrough();
225 #else
226  bool bypass = false;
227 #endif
228 
229  /* INT: Clear on any read.
230  * If this instance is for a device is on I2C bus the Mag will have an i2c interface
231  * that it will use to access the either: a) the internal mag device on the internal I2C bus
232  * or b) it could be used to access a downstream I2C devices connected to the chip on
233  * it's AUX_{ASD|SCL} pins. In either case we need to disconnect (bypass) the internal master
234  * controller that chip provides as a SPI to I2C bridge.
235  * so bypass is true if the mag has an i2c non null interfaces.
236  */
237 
239 
241 
242  uint8_t retries = 3;
243  bool all_ok = false;
244 
245  while (!all_ok && retries--) {
246 
247  // Assume all checked values are as expected
248  all_ok = true;
249  uint8_t reg = 0;
250 
251  for (uint8_t i = 0; i < _num_checked_registers; i++) {
252  if ((reg = read_reg(_checked_registers[i])) != _checked_values[i]) {
253 
255  PX4_ERR("Reg %d is:%d s/b:%d Tries:%d", _checked_registers[i], reg, _checked_values[i], retries);
256  all_ok = false;
257  }
258  }
259  }
260 
261  return all_ok ? OK : -EIO;
262 }
263 
264 int
266 {
267  int ret = PX4_ERROR;
268 
269  // Try first for mpu9250/6500
271 
273 
278  ret = PX4_OK;
279  }
280 
282  _checked_bad[0] = _whoami;
283 
284  if (ret != PX4_OK) {
285  PX4_DEBUG("unexpected whoami 0x%02x", _whoami);
286  }
287 
288  return ret;
289 }
290 
291 /*
292  set sample rate (approximate) - 1kHz to 5Hz, for both accel and gyro
293 */
294 void
295 MPU9250::_set_sample_rate(unsigned desired_sample_rate_hz)
296 {
297  uint8_t div = 1;
298 
299  if (desired_sample_rate_hz == 0) {
300  desired_sample_rate_hz = MPU9250_GYRO_DEFAULT_RATE;
301  }
302 
303  switch (_whoami) {
304  case MPU_WHOAMI_9250:
305  case MPU_WHOAMI_6500:
306  div = 1000 / desired_sample_rate_hz;
307  break;
308  }
309 
310  if (div > 200) { div = 200; }
311 
312  if (div < 1) { div = 1; }
313 
314 
315  switch (_whoami) {
316  case MPU_WHOAMI_9250:
317  case MPU_WHOAMI_6500:
319  _sample_rate = 1000 / div;
320  break;
321  }
322 }
323 
324 /*
325  set the DLPF filter frequency. This affects both accel and gyro.
326  */
327 void
328 MPU9250::_set_dlpf_filter(uint16_t frequency_hz)
329 {
330  uint8_t filter;
331 
332  switch (_whoami) {
333  case MPU_WHOAMI_9250:
334  case MPU_WHOAMI_6500:
335 
336  /*
337  choose next highest filter frequency available
338  */
339  if (frequency_hz == 0) {
340  _dlpf_freq = 0;
341  filter = BITS_DLPF_CFG_3600HZ;
342 
343  } else if (frequency_hz <= 5) {
344  _dlpf_freq = 5;
345  filter = BITS_DLPF_CFG_5HZ;
346 
347  } else if (frequency_hz <= 10) {
348  _dlpf_freq = 10;
349  filter = BITS_DLPF_CFG_10HZ;
350 
351  } else if (frequency_hz <= 20) {
352  _dlpf_freq = 20;
353  filter = BITS_DLPF_CFG_20HZ;
354 
355  } else if (frequency_hz <= 41) {
356  _dlpf_freq = 41;
357  filter = BITS_DLPF_CFG_41HZ;
358 
359  } else if (frequency_hz <= 92) {
360  _dlpf_freq = 92;
361  filter = BITS_DLPF_CFG_92HZ;
362 
363  } else if (frequency_hz <= 184) {
364  _dlpf_freq = 184;
365  filter = BITS_DLPF_CFG_184HZ;
366 
367  } else if (frequency_hz <= 250) {
368  _dlpf_freq = 250;
369  filter = BITS_DLPF_CFG_250HZ;
370 
371  } else {
372  _dlpf_freq = 0;
373  filter = BITS_DLPF_CFG_3600HZ;
374  }
375 
377  break;
378  }
379 }
380 
381 uint8_t
382 MPU9250::read_reg(unsigned reg, uint32_t speed)
383 {
384  uint8_t buf{};
385 
386  _interface->read(MPU9250_SET_SPEED(reg, speed), &buf, 1);
387 
388  return buf;
389 }
390 
391 uint8_t
392 MPU9250::read_reg_range(unsigned start_reg, uint32_t speed, uint8_t *buf, uint16_t count)
393 {
394  if (buf == NULL) {
395  return PX4_ERROR;
396  }
397 
398  return _interface->read(MPU9250_SET_SPEED(start_reg, speed), buf, count);
399 }
400 
401 uint16_t
402 MPU9250::read_reg16(unsigned reg)
403 {
404  uint8_t buf[2] {};
405 
406  // general register transfer at low clock speed
407  _interface->read(MPU9250_LOW_SPEED_OP(reg), &buf, arraySize(buf));
408 
409  return (uint16_t)(buf[0] << 8) | buf[1];
410 }
411 
412 void
413 MPU9250::write_reg(unsigned reg, uint8_t value)
414 {
415  // general register transfer at low clock speed
416  _interface->write(MPU9250_LOW_SPEED_OP(reg), &value, 1);
417 }
418 
419 void
420 MPU9250::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
421 {
422  uint8_t val = read_reg(reg);
423  val &= ~clearbits;
424  val |= setbits;
425  write_reg(reg, val);
426 }
427 
428 void
429 MPU9250::modify_checked_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
430 {
431  uint8_t val = read_reg(reg);
432  val &= ~clearbits;
433  val |= setbits;
434  write_checked_reg(reg, val);
435 }
436 
437 void
438 MPU9250::write_checked_reg(unsigned reg, uint8_t value)
439 {
440  write_reg(reg, value);
441 
442  for (uint8_t i = 0; i < _num_checked_registers; i++) {
443  if (reg == _checked_registers[i]) {
444  _checked_values[i] = value;
445  _checked_bad[i] = value;
446  break;
447  }
448  }
449 }
450 
451 int
452 MPU9250::set_accel_range(unsigned max_g_in)
453 {
454  uint8_t afs_sel;
455  float lsb_per_g;
456  //float max_accel_g;
457 
458  if (max_g_in > 8) { // 16g - AFS_SEL = 3
459  afs_sel = 3;
460  lsb_per_g = 2048;
461  //max_accel_g = 16;
462 
463  } else if (max_g_in > 4) { // 8g - AFS_SEL = 2
464  afs_sel = 2;
465  lsb_per_g = 4096;
466  //max_accel_g = 8;
467 
468  } else if (max_g_in > 2) { // 4g - AFS_SEL = 1
469  afs_sel = 1;
470  lsb_per_g = 8192;
471  //max_accel_g = 4;
472 
473  } else { // 2g - AFS_SEL = 0
474  afs_sel = 0;
475  lsb_per_g = 16384;
476  //max_accel_g = 2;
477  }
478 
479  switch (_whoami) {
480  case MPU_WHOAMI_9250:
481  case MPU_WHOAMI_6500:
482  write_checked_reg(MPUREG_ACCEL_CONFIG, afs_sel << 3);
483  break;
484  }
485 
486  _px4_accel.set_scale(CONSTANTS_ONE_G / lsb_per_g);
487 
488  return OK;
489 }
490 
491 void
493 {
494  /* make sure we are stopped first */
495  stop();
496 
497  ScheduleOnInterval(_call_interval - MPU9250_TIMER_REDUCTION, 1000);
498 }
499 
500 void
502 {
503  ScheduleClear();
504 }
505 
506 void
508 {
509  /* make another measurement */
510  measure();
511 }
512 
513 void
515 {
516  /*
517  we read the register at full speed, even though it isn't
518  listed as a high speed register. The low speed requirement
519  for some registers seems to be a propagation delay
520  requirement for changing sensor configuration, which should
521  not apply to reading a single register. It is also a better
522  test of SPI bus health to read at the same speed as we read
523  the data registers.
524  */
525  uint8_t v = 0;
526 
528 
530 
531  /*
532  if we get the wrong value then we know the SPI bus
533  or sensor is very sick. We set _register_wait to 20
534  and wait until we have seen 20 good values in a row
535  before we consider the sensor to be OK again.
536  */
538 
539  /*
540  try to fix the bad register value. We only try to
541  fix one per loop to prevent a bad sensor hogging the
542  bus.
543  */
544  if (_register_wait == 0 || _checked_next == 0) {
545  // if the product_id is wrong then reset the sensor completely
548 
549  // after doing a reset we need to wait a long
550  // time before we do any other register writes
551  // or we will end up with the mpu9250 in a
552  // bizarre state where it has all correct
553  // register values but large offsets on the
554  // accel axes
555  _reset_wait = hrt_absolute_time() + 10000;
556  _checked_next = 0;
557 
558  } else {
559  write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]);
560 
561  // waiting 3ms between register writes seems
562  // to raise the chance of the sensor
563  // recovering considerably
564  _reset_wait = hrt_absolute_time() + 3000;
565  }
566 
567  _register_wait = 20;
568  }
569 
570  _checked_next = (_checked_next + 1) % _num_checked_registers;
571 }
572 
573 bool
574 MPU9250::check_null_data(uint16_t *data, uint8_t size)
575 {
576  while (size--) {
577  if (*data++) {
579  return false;
580  }
581  }
582 
583  // all zero data - probably a SPI bus error
586  // note that we don't call reset() here as a reset()
587  // costs 20ms with interrupts disabled. That means if
588  // the mpu9250 does go bad it would cause a FMU failure,
589  // regardless of whether another sensor is available,
590  return true;
591 }
592 
593 bool
594 MPU9250::check_duplicate(uint8_t *accel_data)
595 {
596  /*
597  see if this is duplicate accelerometer data. Note that we
598  can't use the data ready interrupt status bit in the status
599  register as that also goes high on new gyro data, and when
600  we run with BITS_DLPF_CFG_256HZ_NOLPF2 the gyro is being
601  sampled at 8kHz, so we would incorrectly think we have new
602  data when we are in fact getting duplicate accelerometer data.
603  */
604  if (!_got_duplicate && memcmp(accel_data, &_last_accel_data, sizeof(_last_accel_data)) == 0) {
605  // it isn't new data - wait for next timer
608  _got_duplicate = true;
609 
610  } else {
611  memcpy(&_last_accel_data, accel_data, sizeof(_last_accel_data));
612  _got_duplicate = false;
613  }
614 
615  return _got_duplicate;
616 }
617 
618 void
620 {
622 
623  if (hrt_absolute_time() < _reset_wait) {
624  // we're waiting for a reset to complete
626  return;
627  }
628 
629  MPUReport mpu_report{};
630 
631  struct Report {
632  int16_t accel_x;
633  int16_t accel_y;
634  int16_t accel_z;
635  int16_t temp;
636  int16_t gyro_x;
637  int16_t gyro_y;
638  int16_t gyro_z;
639  } report{};
640 
641  const hrt_abstime timestamp_sample = hrt_absolute_time();
642 
643  // Fetch the full set of measurements from the ICM20948 in one pass
644  if (_mag.is_passthrough() && _register_wait == 0) {
646  if (OK != read_reg_range(MPUREG_INT_STATUS, MPU9250_HIGH_BUS_SPEED, (uint8_t *)&mpu_report, sizeof(mpu_report))) {
648  return;
649  }
650  }
651 
652  check_registers();
653 
654  if (check_duplicate(&mpu_report.accel_x[0])) {
655  return;
656  }
657  }
658 
659  /*
660  * In case of a mag passthrough read, hand the magnetometer data over to _mag. Else,
661  * try to read a magnetometer report.
662  */
663 
664 # ifdef USE_I2C
665 
666  if (_mag.is_passthrough()) {
667 # endif
668 
669  if (_register_wait == 0) {
670  _mag._measure(timestamp_sample, mpu_report.mag);
671  }
672 
673 # ifdef USE_I2C
674 
675  } else {
676  _mag.measure();
677  }
678 
679 # endif
680 
681  // Continue evaluating gyro and accelerometer results
682  if (_register_wait == 0) {
683  // Convert from big to little endian
684  report.accel_x = int16_t_from_bytes(mpu_report.accel_x);
685  report.accel_y = int16_t_from_bytes(mpu_report.accel_y);
686  report.accel_z = int16_t_from_bytes(mpu_report.accel_z);
687  report.temp = int16_t_from_bytes(mpu_report.temp);
688  report.gyro_x = int16_t_from_bytes(mpu_report.gyro_x);
689  report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y);
690  report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z);
691 
692  if (check_null_data((uint16_t *)&report, sizeof(report) / 2)) {
693  return;
694  }
695  }
696 
697  if (_register_wait != 0) {
698  /*
699  * We are waiting for some good transfers before using the sensor again.
700  * We still increment _good_transfers, but don't return any data yet.
701  */
702  _register_wait--;
703  return;
704  }
705 
706  // Get sensor temperature
707  _last_temperature = (report.temp) / 333.87f + 21.0f;
708 
711 
712 
713  // Swap axes and negate y
714  int16_t accel_xt = report.accel_y;
715  int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x);
716 
717  int16_t gyro_xt = report.gyro_y;
718  int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x);
719 
720  // Apply the swap
721  report.accel_x = accel_xt;
722  report.accel_y = accel_yt;
723  report.gyro_x = gyro_xt;
724  report.gyro_y = gyro_yt;
725 
726  // report the error count as the sum of the number of bad
727  // transfers and bad register reads. This allows the higher
728  // level code to decide if it should use this sensor based on
729  // whether it has had failures
730  const uint64_t error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);
731  _px4_accel.set_error_count(error_count);
732  _px4_gyro.set_error_count(error_count);
733 
734  /* NOTE: Axes have been swapped to match the board a few lines above. */
735  _px4_accel.update(timestamp_sample, report.accel_x, report.accel_y, report.accel_z);
736  _px4_gyro.update(timestamp_sample, report.gyro_x, report.gyro_y, report.gyro_z);
737 
738  /* stop measuring */
740 }
741 
742 void
744 {
750 
753  _mag.print_status();
754 }
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
Modify a register in the mpu.
Definition: mpu9250.cpp:420
void write_checked_reg(unsigned reg, uint8_t value)
Write a register in the mpu, updating _checked_values.
Definition: mpu9250.cpp:438
#define BIT_INT_ANYRD_2CLEAR
Definition: icm20948.h:139
#define BITS_DLPF_CFG_20HZ
Definition: icm20948.h:130
#define BIT_INT_BYPASS_EN
Definition: icm20948.h:140
virtual int init()
Initialise the driver and make it ready for use.
Definition: Device.hpp:91
unsigned _call_interval
Definition: mpu9250.h:257
#define MPUREG_INT_STATUS
Definition: icm20948.h:82
#define BITS_DLPF_CFG_92HZ
Definition: icm20948.h:128
#define BITS_DLPF_CFG_250HZ
Definition: icm20948.h:126
perf_counter_t _good_transfers
Definition: mpu9250.h:266
void _measure(hrt_abstime timestamp_sample, ak8963_regs data)
virtual ~MPU9250()
Definition: mpu9250.cpp:90
measure the time elapsed performing an event
Definition: perf_counter.h:56
#define BITS_DLPF_CFG_3600HZ
Definition: icm20948.h:133
device::Device * _interface
Definition: MPU9250_mag.h:132
#define BITS_ACCEL_CONFIG2_41HZ
Definition: icm20948.h:136
void print_status()
#define MPU_WHOAMI_9250
Definition: mpu9250.h:169
#define MPU_CLK_SEL_AUTO
Definition: icm20948.h:115
#define MPU9250_HIGH_BUS_SPEED
Definition: mpu9250.h:209
int reset_mpu()
Resets the main chip (excluding the magnetometer if any).
Definition: mpu9250.cpp:182
MPU9250_mag _mag
Definition: mpu9250.h:255
device::Device * _interface
Definition: mpu9250.h:241
uint8_t _register_wait
Definition: mpu9250.h:269
void update(hrt_abstime timestamp, float x, float y, float z)
uint8_t read_reg(unsigned reg, uint32_t speed=MPU9250_LOW_BUS_SPEED)
Read a register from the mpu.
Definition: mpu9250.cpp:382
#define MPUREG_CONFIG
Definition: icm20948.h:54
#define BITS_DLPF_CFG_10HZ
Definition: icm20948.h:131
uint8_t _checked_values[MPU9250_NUM_CHECKED_REGISTERS]
Definition: mpu9250.h:281
void measure()
Definition: MPU9250_mag.cpp:75
MPU9250(device::Device *interface, device::Device *mag_interface, enum Rotation rotation)
Definition: mpu9250.cpp:73
int set_accel_range(unsigned max_g)
Set the mpu measurement range.
Definition: mpu9250.cpp:452
#define DRV_GYR_DEVTYPE_MPU9250
Definition: drv_sensor.h:74
void print_info()
Diagnostics - print some basic information about the driver.
Definition: mpu9250.cpp:743
unsigned _dlpf_freq
Definition: mpu9250.h:259
virtual int probe()
whoami result
Definition: mpu9250.cpp:265
#define BIT_I2C_IF_DIS
Definition: icm20948.h:147
void modify_checked_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
Modify a checked register in the mpu.
Definition: mpu9250.cpp:429
count the number of times an event occurs
Definition: perf_counter.h:55
#define BITS_DLPF_CFG_41HZ
Definition: icm20948.h:129
uint16_t read_reg16(unsigned reg)
Definition: mpu9250.cpp:402
uint8_t _whoami
Definition: mpu9250.h:242
const uint16_t * _checked_registers
Definition: mpu9250.h:279
void set_device_type(uint8_t devtype)
static constexpr float CONSTANTS_ONE_G
Definition: geo.h:51
void set_error_count(uint64_t error_count)
#define MPUREG_ACCEL_CONFIG
Definition: icm20948.h:56
#define ACCEL_RANGE_G
Definition: mpu9250.cpp:55
#define MPUREG_WHOAMI
Definition: icm20948.h:52
virtual int init()
Definition: mpu9250.cpp:104
#define MPU9250_TIMER_REDUCTION
Definition: mpu9250.cpp:52
uint8_t read_reg_range(unsigned start_reg, uint32_t speed, uint8_t *buf, uint16_t count)
Read a register range from the mpu.
Definition: mpu9250.cpp:392
void Run() override
Definition: mpu9250.cpp:507
#define MPUREG_SMPLRT_DIV
Definition: icm20948.h:53
#define MPU9250_SET_SPEED(r, s)
Definition: mpu9250.h:213
void update(hrt_abstime timestamp, float x, float y, float z)
void perf_count(perf_counter_t handle)
Count a performance event.
Report conversation within the mpu, including command byte and interrupt status.
Definition: icm20948.h:311
void perf_free(perf_counter_t handle)
Free a counter.
#define MPUREG_INT_ENABLE
Definition: icm20948.h:81
void set_temperature(float temperature)
#define MPUREG_INT_PIN_CFG
Definition: icm20948.h:80
#define MPU9250_GYRO_DEFAULT_RATE
#define perf_alloc(a, b)
Definition: px4io.h:59
#define MPUREG_PWR_MGMT_1
Definition: icm20948.h:106
perf_counter_t _bad_transfers
Definition: mpu9250.h:264
Rotation
Enum for board and external compass rotations.
Definition: rotation.h:51
static struct mag_report _mag
mag report
uint8_t _checked_bad[MPU9250_NUM_CHECKED_REGISTERS]
Definition: mpu9250.h:282
uint8_t * data
Definition: dataman.cpp:149
void check_registers()
Definition: mpu9250.cpp:514
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
unsigned _num_checked_registers
Definition: mpu9250.h:284
unsigned _checked_next
Definition: mpu9250.h:283
void set_error_count(uint64_t error_count)
bool check_duplicate(uint8_t *accel_data)
Definition: mpu9250.cpp:594
void perf_end(perf_counter_t handle)
End a performance event.
static constexpr int MPU9250_NUM_CHECKED_REGISTERS
Definition: mpu9250.h:276
__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
bool check_null_data(uint16_t *data, uint8_t size)
Definition: mpu9250.cpp:574
#define MPU_WHOAMI_6500
Definition: mpu9250.h:170
void print_status()
Definition: MPU9250_mag.h:129
void set_scale(float scale)
uint8_t _last_accel_data[6]
Definition: mpu9250.h:294
void set_temperature(float temperature)
unsigned _sample_rate
Definition: mpu9250.h:261
#define MPUREG_PWR_MGMT_2
Definition: icm20948.h:107
#define MPU9250_DEFAULT_ONCHIP_FILTER_FREQ
Definition: mpu9250.h:180
void measure()
Fetch measurements from the sensor and update the report buffers.
Definition: mpu9250.cpp:619
float _last_temperature
Definition: mpu9250.h:288
perf_counter_t _duplicates
Definition: mpu9250.h:267
#define MPUREG_ACCEL_CONFIG2
Definition: icm20948.h:57
#define arraySize(a)
#define BITS_DLPF_CFG_5HZ
Definition: icm20948.h:132
uint64_t _reset_wait
Definition: mpu9250.h:270
perf_counter_t _bad_registers
Definition: mpu9250.h:265
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
uint64_t perf_event_count(perf_counter_t handle)
Return current event_count.
#define BIT_H_RESET
Definition: icm20948.h:114
bool is_passthrough()
Definition: MPU9250_mag.h:142
bool _got_duplicate
Definition: mpu9250.h:295
Fundamental base class for all physical drivers (I2C, SPI).
Definition: Device.hpp:65
#define MPUREG_GYRO_CONFIG
Definition: icm20948.h:55
void _set_dlpf_filter(uint16_t frequency_hz)
Definition: mpu9250.cpp:328
Definition: bst.cpp:62
#define BIT_RAW_RDY_EN
Definition: icm20948.h:138
void write_reg(unsigned reg, uint8_t value)
Write a register in the mpu.
Definition: mpu9250.cpp:413
int ak8963_reset()
int reset()
Definition: mpu9250.cpp:157
#define OK
Definition: uavcan_main.cpp:71
int16_t int16_t_from_bytes(uint8_t bytes[])
Converts a signed 16 bit integer from big endian to little endian.
Definition: conversions.c:46
PX4Accelerometer _px4_accel
Definition: mpu9250.h:252
void set_scale(float scale)
void stop()
Definition: mpu9250.cpp:501
DeviceBusType get_device_bus_type() const
Return the bus type the device is connected to.
Definition: Device.hpp:168
#define MPU9250_LOW_SPEED_OP(r)
Definition: mpu9250.h:215
void set_device_type(uint8_t devtype)
#define MPUREG_USER_CTRL
Definition: icm20948.h:105
static const uint16_t _mpu9250_checked_registers[MPU9250_NUM_CHECKED_REGISTERS]
Definition: mpu9250.h:277
void _set_sample_rate(unsigned desired_sample_rate_hz)
Definition: mpu9250.cpp:295
#define BITS_FS_2000DPS
Definition: icm20948.h:123
void perf_begin(perf_counter_t handle)
Begin a performance event.
PX4Gyroscope _px4_gyro
Definition: mpu9250.h:253
void start()
Definition: mpu9250.cpp:492
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
perf_counter_t _sample_perf
Definition: mpu9250.h:263
#define DRV_ACC_DEVTYPE_MPU9250
Definition: drv_sensor.h:69
virtual int read(unsigned address, void *data, unsigned count)
Read directly from the device.
Definition: Device.hpp:103
#define BITS_DLPF_CFG_184HZ
Definition: icm20948.h:127