PX4 Firmware
PX4 Autopilot Software http://px4.io
icm20948.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 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 icm20948.cpp
36  *
37  * Driver for the Invensense ICM20948 connected via I2C or SPI.
38  *
39  *
40  * based on the icm20948 driver
41  */
42 
43 #include <px4_platform_common/px4_config.h>
44 #include <px4_platform_common/time.h>
45 #include <lib/ecl/geo/geo.h>
46 #include <lib/perf/perf_counter.h>
47 #include <systemlib/conversions.h>
48 #include <systemlib/px4_macros.h>
49 #include <drivers/drv_hrt.h>
50 #include <drivers/device/spi.h>
52 
53 #include "ICM20948_mag.h"
54 #include "icm20948.h"
55 
56 /*
57  we set the timer interrupt to run a bit faster than the desired
58  sample rate and then throw away duplicates by comparing
59  accelerometer values. This time reduction is enough to cope with
60  worst case timing jitter due to other timers
61  */
62 #define ICM20948_TIMER_REDUCTION 200
63 
64 /* Set accel range used */
65 #define ACCEL_RANGE_G 16
66 /*
67  list of registers that will be checked in check_registers(). Note
68  that MPUREG_PRODUCT_ID must be first in the list.
69  */
70 const uint16_t ICM20948::_icm20948_checked_registers[ICM20948_NUM_CHECKED_REGISTERS] = { ICMREG_20948_USER_CTRL,
85  };
86 
87 ICM20948::ICM20948(device::Device *interface, device::Device *mag_interface, enum Rotation rotation) :
88  ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())),
89  _interface(interface),
90  _px4_accel(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_DEFAULT : ORB_PRIO_HIGH), rotation),
91  _px4_gyro(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_DEFAULT : ORB_PRIO_HIGH), rotation),
92  _mag(this, mag_interface, rotation),
93  _selected_bank(0xFF), // invalid/improbable bank value, will be set on first read/write
95  _dlpf_freq_icm_gyro(ICM20948_DEFAULT_ONCHIP_FILTER_FREQ),
96  _dlpf_freq_icm_accel(ICM20948_DEFAULT_ONCHIP_FILTER_FREQ),
97  _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
98  _bad_transfers(perf_alloc(PC_COUNT, MODULE_NAME": bad_trans")),
99  _bad_registers(perf_alloc(PC_COUNT, MODULE_NAME": bad_reg")),
100  _good_transfers(perf_alloc(PC_COUNT, MODULE_NAME": good_trans")),
101  _duplicates(perf_alloc(PC_COUNT, MODULE_NAME": dupe"))
102 {
105 }
106 
108 {
109  // make sure we are truly inactive
110  stop();
111 
112  // delete the perf counter
119 }
120 
121 int
123 {
124  /*
125  * If the MPU is using I2C we should reduce the sample rate to 200Hz and
126  * make the integration autoreset faster so that we integrate just one
127  * sample since the sampling rate is already low.
128  */
130 
131  if (is_i2c) {
132  _sample_rate = 200;
133  }
134 
135  int ret = probe();
136 
137  if (ret != OK) {
138  PX4_DEBUG("probe failed");
139  return ret;
140  }
141 
142  _reset_wait = hrt_absolute_time() + 100000;
143 
144  if (reset_mpu() != OK) {
145  PX4_ERR("Exiting! Device failed to take initialization");
146  return ret;
147  }
148 
149  /* Magnetometer setup */
150 
151 #ifdef USE_I2C
152  px4_usleep(100);
153 
154  if (!_mag.is_passthrough() && _mag._interface->init() != PX4_OK) {
155  PX4_ERR("failed to setup ak09916 interface");
156  }
157 
158 #endif /* USE_I2C */
159 
160  ret = _mag.ak09916_reset();
161 
162  if (ret != OK) {
163  PX4_DEBUG("mag reset failed");
164  return ret;
165  }
166 
167  start();
168 
169  return ret;
170 }
171 
173 {
174  /* When the icm20948 starts from 0V the internal power on circuit
175  * per the data sheet will require:
176  *
177  * Start-up time for register read/write From power-up Typ:11 max:100 ms
178  *
179  */
180  px4_usleep(110000);
181 
182  // Hold off sampling until done (100 MS will be shortened)
183  _reset_wait = hrt_absolute_time() + 100000;
184 
185  int ret = reset_mpu();
186 
187  if (ret == OK && (_whoami == ICM_WHOAMI_20948)) {
188  ret = _mag.ak09916_reset();
189  }
190 
192 
193  return ret;
194 }
195 
196 int
198 {
199  switch (_whoami) {
200  case ICM_WHOAMI_20948:
202  px4_usleep(1000);
203 
205  px4_usleep(200);
207  break;
208  }
209 
210  // Enable I2C bus or Disable I2C bus (recommended on data sheet)
213 
214  // SAMPLE RATE
216 
218 
219  // Gyro scale 2000 deg/s ()
220  switch (_whoami) {
221  case ICM_WHOAMI_20948:
223  break;
224  }
225 
226  // correct gyro scale factors
227  // scale to rad/s in SI units
228  // 2000 deg/s = (2000/180)*PI = 34.906585 rad/s
229  // scaling factor:
230  // 1/(2^15)*(2000/180)*PI
231  _px4_gyro.set_scale(0.0174532 / 16.4); //1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F);
232 
234 
235  // INT CFG => Interrupt on Data Ready
237 
238 #ifdef USE_I2C
239  bool bypass = !_mag.is_passthrough();
240 #else
241  bool bypass = false;
242 #endif
243 
244  /* INT: Clear on any read.
245  * If this instance is for a device is on I2C bus the Mag will have an i2c interface
246  * that it will use to access the either: a) the internal mag device on the internal I2C bus
247  * or b) it could be used to access a downstream I2C devices connected to the chip on
248  * it's AUX_{ASD|SCL} pins. In either case we need to disconnect (bypass) the internal master
249  * controller that chip provides as a SPI to I2C bridge.
250  * so bypass is true if the mag has an i2c non null interfaces.
251  */
252 
254 
256 
257  uint8_t retries = 3;
258  bool all_ok = false;
259 
260  while (!all_ok && retries--) {
261 
262  // Assume all checked values are as expected
263  all_ok = true;
264  uint8_t reg = 0;
265  uint8_t bankcheck = 0;
266 
267  for (uint8_t i = 0; i < _num_checked_registers; i++) {
268  if ((reg = read_reg(_checked_registers[i])) != _checked_values[i]) {
270 
272  PX4_ERR("Reg %d is:%d s/b:%d Tries:%d - bank s/b %d, is %d", _checked_registers[i], reg, _checked_values[i], retries,
273  REG_BANK(_checked_registers[i]), bankcheck);
274  all_ok = false;
275  }
276  }
277  }
278 
279  return all_ok ? OK : -EIO;
280 }
281 
282 int
284 {
285  int ret = PX4_ERROR;
286 
287  // Try first for icm20948/6500
289 
290  // must be an ICM
291  // Make sure selected register bank is bank 0 (which contains WHOAMI)
294 
295  if (_whoami == ICM_WHOAMI_20948) {
300  ret = PX4_OK;
301  }
302 
304  _checked_bad[0] = _whoami;
305 
306  if (ret != PX4_OK) {
307  PX4_DEBUG("unexpected whoami 0x%02x", _whoami);
308  }
309 
310  return ret;
311 }
312 
313 /*
314  set sample rate (approximate) - 1kHz to 5Hz, for both accel and gyro
315  For ICM20948 accel and gyro samplerates are both set to the same value.
316 */
317 void
318 ICM20948::_set_sample_rate(unsigned desired_sample_rate_hz)
319 {
320  uint8_t div = 1;
321 
322  if (desired_sample_rate_hz == 0) {
323  desired_sample_rate_hz = ICM20948_GYRO_DEFAULT_RATE;
324  }
325 
326  switch (_whoami) {
327  case ICM_WHOAMI_20948:
328  div = 1100 / desired_sample_rate_hz;
329  break;
330  }
331 
332  if (div > 200) { div = 200; }
333 
334  if (div < 1) { div = 1; }
335 
336 
337  switch (_whoami) {
338  case ICM_WHOAMI_20948:
340  // There's also an MSB for this allowing much higher dividers for the accelerometer.
341  // For 1 < div < 200 the LSB is sufficient.
343  _sample_rate = 1100 / div;
344  break;
345  }
346 }
347 
348 /*
349  set the DLPF filter frequency. This affects both accel and gyro.
350  */
351 void
352 ICM20948::_set_dlpf_filter(uint16_t frequency_hz)
353 {
354  uint8_t filter;
355 
356  switch (_whoami) {
357  case ICM_WHOAMI_20948:
358 
359  /*
360  choose next highest filter frequency available for gyroscope
361  */
362  if (frequency_hz == 0) {
363  //_dlpf_freq_icm_gyro = 0;
365 
366  } else if (frequency_hz <= 5) {
367  //_dlpf_freq_icm_gyro = 5;
369 
370  } else if (frequency_hz <= 11) {
371  //_dlpf_freq_icm_gyro = 11;
373 
374  } else if (frequency_hz <= 23) {
375  //_dlpf_freq_icm_gyro = 23;
377 
378  } else if (frequency_hz <= 51) {
379  //_dlpf_freq_icm_gyro = 51;
381 
382  } else if (frequency_hz <= 119) {
383  //_dlpf_freq_icm_gyro = 119;
385 
386  } else if (frequency_hz <= 151) {
387  //_dlpf_freq_icm_gyro = 151;
389 
390  } else if (frequency_hz <= 197) {
391  //_dlpf_freq_icm_gyro = 197;
393 
394  } else {
395  //_dlpf_freq_icm_gyro = 0;
397  }
398 
400 
401  /*
402  choose next highest filter frequency available for accelerometer
403  */
404  if (frequency_hz == 0) {
405  //_dlpf_freq_icm_accel = 0;
407 
408  } else if (frequency_hz <= 5) {
409  //_dlpf_freq_icm_accel = 5;
411 
412  } else if (frequency_hz <= 11) {
413  //_dlpf_freq_icm_accel = 11;
415 
416  } else if (frequency_hz <= 23) {
417  //_dlpf_freq_icm_accel = 23;
419 
420  } else if (frequency_hz <= 50) {
421  //_dlpf_freq_icm_accel = 50;
423 
424  } else if (frequency_hz <= 111) {
425  //_dlpf_freq_icm_accel = 111;
427 
428  } else if (frequency_hz <= 246) {
429  //_dlpf_freq_icm_accel = 246;
431 
432  } else {
433  //_dlpf_freq_icm_accel = 0;
435  }
436 
438  break;
439  }
440 }
441 
442 int
444 {
445  uint8_t ret;
446  uint8_t buf;
447  uint8_t retries = 3;
448 
449  if (_selected_bank != bank) {
451 
452  if (ret != OK) {
453  return ret;
454  }
455  }
456 
457  /*
458  * Making sure the right register bank is selected (even if it should be). Observed some
459  * unexpected changes to this, don't risk writing to the wrong register bank.
460  */
462 
463  while (bank != buf && retries > 0) {
464  //PX4_WARN("user bank: expected %d got %d",bank,buf);
466 
467  if (ret != OK) {
468  return ret;
469  }
470 
471  retries--;
472  //PX4_WARN("BANK retries: %d", 4-retries);
473 
475  }
476 
477 
478  _selected_bank = bank;
479 
480  if (bank != buf) {
481  PX4_DEBUG("SELECT FAILED %d %d %d %d", retries, _selected_bank, bank, buf);
482  return PX4_ERROR;
483 
484  } else {
485  return PX4_OK;
486  }
487 }
488 
489 uint8_t
490 ICM20948::read_reg(unsigned reg, uint32_t speed)
491 {
492  uint8_t buf{};
493 
495  _interface->read(ICM20948_SET_SPEED(REG_ADDRESS(reg), speed), &buf, 1);
496 
497  return buf;
498 }
499 
500 uint8_t
501 ICM20948::read_reg_range(unsigned start_reg, uint32_t speed, uint8_t *buf, uint16_t count)
502 {
503  if (buf == NULL) {
504  return PX4_ERROR;
505  }
506 
507  select_register_bank(REG_BANK(start_reg));
508  return _interface->read(ICM20948_SET_SPEED(REG_ADDRESS(start_reg), speed), buf, count);
509 }
510 
511 uint16_t
512 ICM20948::read_reg16(unsigned reg)
513 {
514  uint8_t buf[2] {};
515 
516  // general register transfer at low clock speed
519 
520  return (uint16_t)(buf[0] << 8) | buf[1];
521 }
522 
523 void
524 ICM20948::write_reg(unsigned reg, uint8_t value)
525 {
526  // general register transfer at low clock speed
529 }
530 
531 void
532 ICM20948::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
533 {
534  uint8_t val = read_reg(reg);
535  val &= ~clearbits;
536  val |= setbits;
537  write_reg(reg, val);
538 }
539 
540 void
541 ICM20948::modify_checked_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
542 {
543  uint8_t val = read_reg(reg);
544  val &= ~clearbits;
545  val |= setbits;
546  write_checked_reg(reg, val);
547 }
548 
549 void
550 ICM20948::write_checked_reg(unsigned reg, uint8_t value)
551 {
552  write_reg(reg, value);
553 
554  for (uint8_t i = 0; i < _num_checked_registers; i++) {
555  if (reg == _checked_registers[i]) {
556  _checked_values[i] = value;
557  _checked_bad[i] = value;
558  break;
559  }
560  }
561 }
562 
563 int
564 ICM20948::set_accel_range(unsigned max_g_in)
565 {
566  uint8_t afs_sel;
567  float lsb_per_g;
568  //float max_accel_g;
569 
570  if (max_g_in > 8) { // 16g - AFS_SEL = 3
571  afs_sel = 3;
572  lsb_per_g = 2048;
573  //max_accel_g = 16;
574 
575  } else if (max_g_in > 4) { // 8g - AFS_SEL = 2
576  afs_sel = 2;
577  lsb_per_g = 4096;
578  //max_accel_g = 8;
579 
580  } else if (max_g_in > 2) { // 4g - AFS_SEL = 1
581  afs_sel = 1;
582  lsb_per_g = 8192;
583  //max_accel_g = 4;
584 
585  } else { // 2g - AFS_SEL = 0
586  afs_sel = 0;
587  lsb_per_g = 16384;
588  //max_accel_g = 2;
589  }
590 
591  switch (_whoami) {
592  case ICM_WHOAMI_20948:
594  break;
595  }
596 
597  _px4_accel.set_scale(CONSTANTS_ONE_G / lsb_per_g);
598 
599  return OK;
600 }
601 
602 void
604 {
605  /* make sure we are stopped first */
606  stop();
607 
608  ScheduleOnInterval(_call_interval - ICM20948_TIMER_REDUCTION, 1000);
609 }
610 
611 void
613 {
614  ScheduleClear();
615 }
616 
617 void
619 {
620  /* make another measurement */
621  measure();
622 }
623 
624 void
626 {
627  /*
628  we read the register at full speed, even though it isn't
629  listed as a high speed register. The low speed requirement
630  for some registers seems to be a propagation delay
631  requirement for changing sensor configuration, which should
632  not apply to reading a single register. It is also a better
633  test of SPI bus health to read at the same speed as we read
634  the data registers.
635  */
636  uint8_t v;
637 
639  _checked_values[_checked_next]) {
640 
642 
643  /*
644  if we get the wrong value then we know the SPI bus
645  or sensor is very sick. We set _register_wait to 20
646  and wait until we have seen 20 good values in a row
647  before we consider the sensor to be OK again.
648  */
650 
651  /*
652  try to fix the bad register value. We only try to
653  fix one per loop to prevent a bad sensor hogging the
654  bus.
655  */
656  if (_register_wait == 0 || _checked_next == 0) {
657  // if the product_id is wrong then reset the
658  // sensor completely
659  reset_mpu();
660 
661  // after doing a reset we need to wait a long
662  // time before we do any other register writes
663  // or we will end up with the icm20948 in a
664  // bizarre state where it has all correct
665  // register values but large offsets on the
666  // accel axes
667  _reset_wait = hrt_absolute_time() + 10000;
668  _checked_next = 0;
669 
670  } else {
671  write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]);
672 
673  // waiting 3ms between register writes seems
674  // to raise the chance of the sensor
675  // recovering considerably
676  _reset_wait = hrt_absolute_time() + 3000;
677  }
678 
679  _register_wait = 20;
680  }
681 
682  _checked_next = (_checked_next + 1) % _num_checked_registers;
683 }
684 
685 bool
686 ICM20948::check_null_data(uint16_t *data, uint8_t size)
687 {
688  while (size--) {
689  if (*data++) {
691  return false;
692  }
693  }
694 
695  // all zero data - probably a SPI bus error
698  // note that we don't call reset() here as a reset()
699  // costs 20ms with interrupts disabled. That means if
700  // the icm20948 does go bad it would cause a FMU failure,
701  // regardless of whether another sensor is available,
702  return true;
703 }
704 
705 bool
706 ICM20948::check_duplicate(uint8_t *accel_data)
707 {
708  /*
709  see if this is duplicate accelerometer data. Note that we
710  can't use the data ready interrupt status bit in the status
711  register as that also goes high on new gyro data, and when
712  we run with BITS_DLPF_CFG_256HZ_NOLPF2 the gyro is being
713  sampled at 8kHz, so we would incorrectly think we have new
714  data when we are in fact getting duplicate accelerometer data.
715  */
716  if (!_got_duplicate && memcmp(accel_data, &_last_accel_data, sizeof(_last_accel_data)) == 0) {
717  // it isn't new data - wait for next timer
720  _got_duplicate = true;
721 
722  } else {
723  memcpy(&_last_accel_data, accel_data, sizeof(_last_accel_data));
724  _got_duplicate = false;
725  }
726 
727  return _got_duplicate;
728 }
729 
730 void
732 {
735 
736  if (hrt_absolute_time() < _reset_wait) {
737  // we're waiting for a reset to complete
739  return;
740  }
741 
742  MPUReport mpu_report{};
743  ICMReport icm_report{};
744 
745  struct Report {
746  int16_t accel_x;
747  int16_t accel_y;
748  int16_t accel_z;
749  int16_t temp;
750  int16_t gyro_x;
751  int16_t gyro_y;
752  int16_t gyro_z;
753  } report{};
754 
755  const hrt_abstime timestamp_sample = hrt_absolute_time();
756 
757  // Fetch the full set of measurements from the ICM20948 in one pass
758  if (_mag.is_passthrough() && _register_wait == 0) {
759 
761 
762  if (OK != read_reg_range(ICMREG_20948_ACCEL_XOUT_H, ICM20948_HIGH_BUS_SPEED, (uint8_t *)&icm_report,
763  sizeof(icm_report))) {
765  return;
766  }
767 
768  check_registers();
769 
770  if (check_duplicate(MPU_OR_ICM(&mpu_report.accel_x[0], &icm_report.accel_x[0]))) {
771  return;
772  }
773  }
774 
775  /*
776  * In case of a mag passthrough read, hand the magnetometer data over to _mag. Else,
777  * try to read a magnetometer report.
778  */
779 
780 # ifdef USE_I2C
781 
782  if (_mag.is_passthrough()) {
783 # endif
784 
785  _mag._measure(timestamp_sample, mpu_report.mag);
786 
787 # ifdef USE_I2C
788 
789  } else {
790  _mag.measure();
791  }
792 
793 # endif
794 
795  // Continue evaluating gyro and accelerometer results
796  if (_register_wait == 0) {
797  // Convert from big to little endian
798  report.accel_x = int16_t_from_bytes(icm_report.accel_x);
799  report.accel_y = int16_t_from_bytes(icm_report.accel_y);
800  report.accel_z = int16_t_from_bytes(icm_report.accel_z);
801  report.temp = int16_t_from_bytes(icm_report.temp);
802  report.gyro_x = int16_t_from_bytes(icm_report.gyro_x);
803  report.gyro_y = int16_t_from_bytes(icm_report.gyro_y);
804  report.gyro_z = int16_t_from_bytes(icm_report.gyro_z);
805 
806  if (check_null_data((uint16_t *)&report, sizeof(report) / 2)) {
807  return;
808  }
809  }
810 
811  if (_register_wait != 0) {
812  /*
813  * We are waiting for some good transfers before using the sensor again.
814  * We still increment _good_transfers, but don't return any data yet.
815  */
816  _register_wait--;
817  return;
818  }
819 
820  // Get sensor temperature
821  _last_temperature = (report.temp) / 333.87f + 21.0f;
822 
825 
826 
827  // Swap axes and negate y
828  int16_t accel_xt = report.accel_y;
829  int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x);
830 
831  int16_t gyro_xt = report.gyro_y;
832  int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x);
833 
834  // Apply the swap
835  report.accel_x = accel_xt;
836  report.accel_y = accel_yt;
837  report.gyro_x = gyro_xt;
838  report.gyro_y = gyro_yt;
839 
840  // report the error count as the sum of the number of bad
841  // transfers and bad register reads. This allows the higher
842  // level code to decide if it should use this sensor based on
843  // whether it has had failures
844  const uint64_t error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);
845  _px4_accel.set_error_count(error_count);
846  _px4_gyro.set_error_count(error_count);
847 
848  /* NOTE: Axes have been swapped to match the board a few lines above. */
849  _px4_accel.update(timestamp_sample, report.accel_x, report.accel_y, report.accel_z);
850  _px4_gyro.update(timestamp_sample, report.gyro_x, report.gyro_y, report.gyro_z);
851 
852  /* stop measuring */
854 }
855 
856 void
858 {
864 
867  _mag.print_status();
868 }
#define BIT_INT_ANYRD_2CLEAR
Definition: icm20948.h:139
#define ICM_BITS_GYRO_DLPF_CFG_361HZ
Definition: icm20948.h:252
#define ICMREG_20948_PWR_MGMT_2
Definition: icm20948.h:213
#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
static const uint16_t _icm20948_checked_registers[ICM20948_NUM_CHECKED_REGISTERS]
Definition: icm20948.h:405
void print_status()
Definition: ICM20948_mag.h:128
#define ICM_BITS_GYRO_DLPF_CFG_5HZ
Definition: icm20948.h:251
unsigned _num_checked_registers
Definition: icm20948.h:412
uint64_t _reset_wait
Definition: icm20948.h:398
bool is_passthrough()
Definition: ICM20948_mag.h:141
measure the time elapsed performing an event
Definition: perf_counter.h:56
uint16_t read_reg16(unsigned reg)
Definition: icm20948.cpp:512
perf_counter_t _sample_perf
Definition: icm20948.h:390
int select_register_bank(uint8_t bank)
Select a register bank in ICM20948.
Definition: icm20948.cpp:443
Definition of geo / math functions to perform geodesic calculations.
void print_status()
#define MPU_CLK_SEL_AUTO
Definition: icm20948.h:115
device::Device * _interface
Definition: ICM20948_mag.h:131
#define ACCEL_RANGE_G
Definition: icm20948.cpp:65
int reset()
Definition: icm20948.cpp:172
unsigned _call_interval
Definition: icm20948.h:382
void update(hrt_abstime timestamp, float x, float y, float z)
#define ICMREG_20948_GYRO_CONFIG_2
Definition: icm20948.h:223
uint8_t _register_wait
Definition: icm20948.h:397
#define ICMREG_20948_ACCEL_SMPLRT_DIV_1
Definition: icm20948.h:224
Report conversation within the mpu, including command byte and interrupt status.
Definition: icm20948.h:293
unsigned _sample_rate
Definition: icm20948.h:388
#define ICM20948_LOW_SPEED_OP(r)
Definition: icm20948.h:339
#define ICMREG_20948_USER_CTRL
Definition: icm20948.h:211
virtual ~ICM20948()
Definition: icm20948.cpp:107
#define ICMREG_20948_INT_ENABLE_3
Definition: icm20948.h:219
#define ICM_BITS_DEC3_CFG_32
Definition: icm20948.h:279
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
Modify a register in the mpu.
Definition: icm20948.cpp:532
bool _got_duplicate
Definition: icm20948.h:423
#define ICM_BITS_GYRO_DLPF_CFG_51HZ
Definition: icm20948.h:248
A set of useful macros for enhanced runtime and compile time error detection and warning suppression...
#define ICMREG_20948_ACCEL_SMPLRT_DIV_2
Definition: icm20948.h:225
PX4Gyroscope _px4_gyro
Definition: icm20948.h:377
ICM20948_mag _mag
Definition: icm20948.h:379
#define ICM_BITS_GYRO_DLPF_CFG_151HZ
Definition: icm20948.h:246
#define BIT_I2C_IF_DIS
Definition: icm20948.h:147
uint8_t _whoami
Definition: icm20948.h:366
perf_counter_t _duplicates
Definition: icm20948.h:395
count the number of times an event occurs
Definition: perf_counter.h:55
Vector rotation library.
#define ICMREG_20948_WHOAMI
Definition: icm20948.h:210
High-resolution timer with callouts and timekeeping.
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 ICM_BITS_ACCEL_DLPF_CFG_50HZ
Definition: icm20948.h:263
#define ICM20948_HIGH_BUS_SPEED
Definition: icm20948.h:333
void _set_dlpf_filter(uint16_t frequency_hz)
Definition: icm20948.cpp:352
#define MPUREG_WHOAMI
Definition: icm20948.h:52
#define ICMREG_20948_INT_PIN_CFG
Definition: icm20948.h:214
#define ICM20948_SET_SPEED(r, s)
Definition: icm20948.h:337
device::Device * _interface
Definition: icm20948.h:365
uint8_t _selected_bank
Definition: icm20948.h:380
void update(hrt_abstime timestamp, float x, float y, float z)
void write_reg(unsigned reg, uint8_t value)
Write a register in the mpu.
Definition: icm20948.cpp:524
uint8_t read_reg(unsigned reg, uint32_t speed=ICM20948_LOW_BUS_SPEED)
Read a register from the mpu.
Definition: icm20948.cpp:490
void _measure(hrt_abstime timestamp_sample, ak09916_regs data)
perf_counter_t _interval_perf
Definition: icm20948.h:391
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
#define ICM20948_DEFAULT_ONCHIP_FILTER_FREQ
Definition: icm20948.h:179
void perf_free(perf_counter_t handle)
Free a counter.
void set_temperature(float temperature)
#define ICM_BITS_GYRO_FS_SEL_MASK
Definition: icm20948.h:259
void Run() override
Definition: icm20948.cpp:618
Definition of commonly used conversions.
#define perf_alloc(a, b)
Definition: px4io.h:59
unsigned _checked_next
Definition: icm20948.h:411
#define ICMREG_20948_GYRO_SMPLRT_DIV
Definition: icm20948.h:221
void measure()
Fetch measurements from the sensor and update the report buffers.
Definition: icm20948.cpp:731
Rotation
Enum for board and external compass rotations.
Definition: rotation.h:51
#define ICM_BITS_GYRO_DLPF_CFG_119HZ
Definition: icm20948.h:247
static struct mag_report _mag
mag report
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 ICMREG_20948_ACCEL_XOUT_H
Definition: icm20948.h:217
#define ICMREG_20948_INT_ENABLE_1
Definition: icm20948.h:216
virtual int probe()
whoami result
Definition: icm20948.cpp:283
int reset_mpu()
Resets the main chip (excluding the magnetometer if any).
Definition: icm20948.cpp:197
uint8_t _last_accel_data[6]
Definition: icm20948.h:422
#define ICM_BITS_ACCEL_FS_SEL_MASK
Definition: icm20948.h:274
void stop()
Definition: icm20948.cpp:612
void write_checked_reg(unsigned reg, uint8_t value)
Write a register in the mpu, updating _checked_values.
Definition: icm20948.cpp:550
void set_error_count(uint64_t error_count)
void perf_end(perf_counter_t handle)
End a performance event.
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: icm20948.cpp:501
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
ICM20948(device::Device *interface, device::Device *mag_interface, enum Rotation rotation)
Definition: icm20948.cpp:87
virtual int write(unsigned address, void *data, unsigned count)
Write directly to the device.
Definition: Device.hpp:115
#define ICM_BITS_ACCEL_DLPF_CFG_11HZ
Definition: icm20948.h:265
#define ICM_BITS_ACCEL_DLPF_CFG_23HZ
Definition: icm20948.h:264
#define ICMREG_20948_ACCEL_CONFIG_2
Definition: icm20948.h:227
static constexpr int ICM20948_NUM_CHECKED_REGISTERS
Definition: icm20948.h:404
void set_scale(float scale)
float _last_temperature
Definition: icm20948.h:416
#define ICM_WHOAMI_20948
Definition: icm20948.h:169
const uint16_t * _checked_registers
Definition: icm20948.h:407
#define ICMREG_20948_INT_ENABLE
Definition: icm20948.h:215
#define ICM_BITS_GYRO_DLPF_CFG_11HZ
Definition: icm20948.h:250
void set_temperature(float temperature)
#define ICM_BITS_ACCEL_DLPF_CFG_473HZ
Definition: icm20948.h:267
#define ICM_BITS_ACCEL_DLPF_CFG_246HZ
Definition: icm20948.h:261
#define ICMREG_20948_PWR_MGMT_1
Definition: icm20948.h:212
#define ICMREG_20948_INT_ENABLE_2
Definition: icm20948.h:218
void start()
Definition: icm20948.cpp:603
#define ICM_BITS_GYRO_FS_SEL_2000DPS
Definition: icm20948.h:258
bool check_duplicate(uint8_t *accel_data)
Definition: icm20948.cpp:706
#define ICM_BITS_ACCEL_DLPF_CFG_5HZ
Definition: icm20948.h:266
#define arraySize(a)
int set_accel_range(unsigned max_g)
Set the mpu measurement range.
Definition: icm20948.cpp:564
void modify_checked_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
Modify a checked register in the mpu.
Definition: icm20948.cpp:541
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
Fundamental base class for all physical drivers (I2C, SPI).
Definition: Device.hpp:65
#define ICM_BITS_ACCEL_DLPF_CFG_111HZ
Definition: icm20948.h:262
void _set_sample_rate(unsigned desired_sample_rate_hz)
Definition: icm20948.cpp:318
Definition: bst.cpp:62
PX4Accelerometer _px4_accel
Definition: icm20948.h:376
#define BIT_RAW_RDY_EN
Definition: icm20948.h:138
#define MPU_OR_ICM(m, i)
Definition: icm20948.h:286
#define ICMREG_20948_GYRO_CONFIG_1
Definition: icm20948.h:222
#define ICM_BITS_GYRO_DLPF_CFG_197HZ
Definition: icm20948.h:245
#define OK
Definition: uavcan_main.cpp:71
bool check_null_data(uint16_t *data, uint8_t size)
Definition: icm20948.cpp:686
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
void print_info()
Diagnostics - print some basic information about the driver.
Definition: icm20948.cpp:857
uint8_t _checked_bad[ICM20948_NUM_CHECKED_REGISTERS]
Definition: icm20948.h:410
#define REG_ADDRESS(r)
Definition: icm20948.h:206
void set_scale(float scale)
DeviceBusType get_device_bus_type() const
Return the bus type the device is connected to.
Definition: Device.hpp:168
void set_device_type(uint8_t devtype)
#define ICM20948_GYRO_DEFAULT_RATE
Definition: icm20948.h:174
perf_counter_t _good_transfers
Definition: icm20948.h:394
#define ICMREG_20948_ACCEL_CONFIG
Definition: icm20948.h:226
perf_counter_t _bad_transfers
Definition: icm20948.h:392
#define ICM_BITS_GYRO_DLPF_CFG_23HZ
Definition: icm20948.h:249
void check_registers()
Definition: icm20948.cpp:625
perf_counter_t _bad_registers
Definition: icm20948.h:393
#define REG_BANK(r)
Definition: icm20948.h:205
#define ICM20948_TIMER_REDUCTION
Definition: icm20948.cpp:62
#define ICMREG_20948_BANK_SEL
Definition: icm20948.h:208
#define DRV_DEVTYPE_ICM20948
Definition: drv_sensor.h:64
virtual int init()
Definition: icm20948.cpp:122
void perf_begin(perf_counter_t handle)
Begin a performance event.
uint8_t _checked_values[ICM20948_NUM_CHECKED_REGISTERS]
Definition: icm20948.h:409
__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
Performance measuring tools.