| 
    PX4 Firmware
    
   PX4 Autopilot Software http://px4.io 
   | 
 
#include <icm20948.h>
Public Member Functions | |
| ICM20948 (device::Device *interface, device::Device *mag_interface, enum Rotation rotation) | |
| virtual | ~ICM20948 () | 
| virtual int | init () | 
| uint8_t | get_whoami () | 
| void | print_info () | 
| Diagnostics - print some basic information about the driver.  More... | |
Protected Member Functions | |
| virtual int | probe () | 
| whoami result  More... | |
| void | Run () override | 
Protected Attributes | |
| device::Device * | _interface | 
| uint8_t | _whoami {0} | 
Private Member Functions | |
| bool | check_null_data (uint16_t *data, uint8_t size) | 
| bool | check_duplicate (uint8_t *accel_data) | 
| void | start () | 
| void | stop () | 
| int | reset () | 
| int | reset_mpu () | 
| Resets the main chip (excluding the magnetometer if any).  More... | |
| void | measure () | 
| Fetch measurements from the sensor and update the report buffers.  More... | |
| int | select_register_bank (uint8_t bank) | 
| Select a register bank in ICM20948.  More... | |
| uint8_t | read_reg (unsigned reg, uint32_t speed=ICM20948_LOW_BUS_SPEED) | 
| Read a register from the mpu.  More... | |
| uint16_t | read_reg16 (unsigned reg) | 
| uint8_t | read_reg_range (unsigned start_reg, uint32_t speed, uint8_t *buf, uint16_t count) | 
| Read a register range from the mpu.  More... | |
| void | write_reg (unsigned reg, uint8_t value) | 
| Write a register in the mpu.  More... | |
| void | modify_reg (unsigned reg, uint8_t clearbits, uint8_t setbits) | 
| Modify a register in the mpu.  More... | |
| void | write_checked_reg (unsigned reg, uint8_t value) | 
| Write a register in the mpu, updating _checked_values.  More... | |
| void | modify_checked_reg (unsigned reg, uint8_t clearbits, uint8_t setbits) | 
| Modify a checked register in the mpu.  More... | |
| int | set_accel_range (unsigned max_g) | 
| Set the mpu measurement range.  More... | |
| uint16_t | swap16 (uint16_t val) | 
| Swap a 16-bit value read from the mpu to native byte order.  More... | |
| bool | is_external () | 
| Get the internal / external state.  More... | |
| void | _set_dlpf_filter (uint16_t frequency_hz) | 
| void | _set_sample_rate (unsigned desired_sample_rate_hz) | 
| void | check_registers () | 
Private Attributes | |
| PX4Accelerometer | _px4_accel | 
| PX4Gyroscope | _px4_gyro | 
| ICM20948_mag | _mag | 
| uint8_t | _selected_bank {0} | 
| unsigned | _call_interval {1000} | 
| unsigned | _dlpf_freq {0} | 
| unsigned | _dlpf_freq_icm_gyro {0} | 
| unsigned | _dlpf_freq_icm_accel {0} | 
| unsigned | _sample_rate {1000} | 
| perf_counter_t | _sample_perf | 
| perf_counter_t | _interval_perf | 
| perf_counter_t | _bad_transfers | 
| perf_counter_t | _bad_registers | 
| perf_counter_t | _good_transfers | 
| perf_counter_t | _duplicates | 
| uint8_t | _register_wait {0} | 
| uint64_t | _reset_wait {0} | 
| const uint16_t * | _checked_registers {nullptr} | 
| uint8_t | _checked_values [ICM20948_NUM_CHECKED_REGISTERS] {} | 
| uint8_t | _checked_bad [ICM20948_NUM_CHECKED_REGISTERS] {} | 
| unsigned | _checked_next {0} | 
| unsigned | _num_checked_registers {0} | 
| float | _last_temperature {0.0f} | 
| uint8_t | _last_accel_data [6] {} | 
| bool | _got_duplicate {false} | 
Static Private Attributes | |
| static constexpr int | ICM20948_NUM_CHECKED_REGISTERS {15} | 
| static const uint16_t | _icm20948_checked_registers [ICM20948_NUM_CHECKED_REGISTERS] | 
Friends | |
| class | ICM20948_mag | 
Definition at line 350 of file icm20948.h.
| ICM20948::ICM20948 | ( | device::Device * | interface, | 
| device::Device * | mag_interface, | ||
| enum Rotation | rotation | ||
| ) | 
Definition at line 87 of file icm20948.cpp.
References _px4_accel, _px4_gyro, DRV_DEVTYPE_ICM20948, PX4Accelerometer::set_device_type(), and PX4Gyroscope::set_device_type().
      
  | 
  virtual | 
Definition at line 107 of file icm20948.cpp.
References _bad_registers, _bad_transfers, _duplicates, _good_transfers, _interval_perf, _sample_perf, perf_free(), and stop().
      
  | 
  private | 
Definition at line 352 of file icm20948.cpp.
References _whoami, ICM_BITS_ACCEL_DLPF_CFG_111HZ, ICM_BITS_ACCEL_DLPF_CFG_11HZ, ICM_BITS_ACCEL_DLPF_CFG_23HZ, ICM_BITS_ACCEL_DLPF_CFG_246HZ, ICM_BITS_ACCEL_DLPF_CFG_473HZ, ICM_BITS_ACCEL_DLPF_CFG_50HZ, ICM_BITS_ACCEL_DLPF_CFG_5HZ, ICM_BITS_GYRO_DLPF_CFG_119HZ, ICM_BITS_GYRO_DLPF_CFG_11HZ, ICM_BITS_GYRO_DLPF_CFG_151HZ, ICM_BITS_GYRO_DLPF_CFG_197HZ, ICM_BITS_GYRO_DLPF_CFG_23HZ, ICM_BITS_GYRO_DLPF_CFG_361HZ, ICM_BITS_GYRO_DLPF_CFG_51HZ, ICM_BITS_GYRO_DLPF_CFG_5HZ, ICM_WHOAMI_20948, ICMREG_20948_ACCEL_CONFIG, ICMREG_20948_GYRO_CONFIG_1, and write_checked_reg().
Referenced by reset_mpu().
      
  | 
  private | 
Definition at line 318 of file icm20948.cpp.
References _sample_rate, _whoami, ICM20948_GYRO_DEFAULT_RATE, ICM_WHOAMI_20948, ICMREG_20948_ACCEL_SMPLRT_DIV_2, ICMREG_20948_GYRO_SMPLRT_DIV, and write_checked_reg().
Referenced by reset_mpu().
      
  | 
  private | 
Definition at line 706 of file icm20948.cpp.
References _duplicates, _got_duplicate, _last_accel_data, _sample_perf, perf_count(), and perf_end().
Referenced by measure().
      
  | 
  private | 
Definition at line 686 of file icm20948.cpp.
References _bad_transfers, _good_transfers, _sample_perf, perf_count(), and perf_end().
Referenced by measure().
      
  | 
  private | 
Definition at line 625 of file icm20948.cpp.
References _bad_registers, _checked_bad, _checked_next, _checked_registers, _checked_values, _num_checked_registers, _register_wait, _reset_wait, hrt_absolute_time(), ICM20948_HIGH_BUS_SPEED, perf_count(), read_reg(), reset_mpu(), and write_reg().
Referenced by measure().
      
  | 
  inline | 
Definition at line 357 of file icm20948.h.
      
  | 
  virtual | 
Definition at line 122 of file icm20948.cpp.
References ICM20948_mag::_interface, _interface, _mag, _reset_wait, _sample_rate, ICM20948_mag::ak09916_reset(), device::Device::DeviceBusType_I2C, device::Device::get_device_bus_type(), hrt_absolute_time(), device::Device::init(), ICM20948_mag::is_passthrough(), OK, probe(), reset_mpu(), and start().
      
  | 
  inlineprivate | 
Get the internal / external state.
Definition at line 528 of file icm20948.h.
References device::Device::external().
Referenced by ICM20948_mag::_measure().
      
  | 
  private | 
Fetch measurements from the sensor and update the report buffers.
Definition at line 731 of file icm20948.cpp.
References _bad_registers, _bad_transfers, _interval_perf, _last_temperature, _mag, ICM20948_mag::_measure(), _px4_accel, _px4_gyro, _register_wait, _reset_wait, _sample_perf, check_duplicate(), check_null_data(), check_registers(), f(), hrt_absolute_time(), hrt_abstime, ICM20948_HIGH_BUS_SPEED, ICMREG_20948_ACCEL_XOUT_H, int16_t_from_bytes(), ICM20948_mag::is_passthrough(), ICM20948_mag::measure(), MPU_OR_ICM, OK, perf_begin(), perf_count(), perf_end(), perf_event_count(), read_reg_range(), REG_BANK, select_register_bank(), PX4Accelerometer::set_error_count(), PX4Gyroscope::set_error_count(), PX4Accelerometer::set_temperature(), PX4Gyroscope::set_temperature(), PX4Accelerometer::update(), and PX4Gyroscope::update().
Referenced by Run().
      
  | 
  private | 
Modify a checked register in the mpu.
Bits are cleared before bits are set.
| reg | The register to modify. | 
| clearbits | Bits in the register to clear. | 
| setbits | Bits in the register to set. | 
Definition at line 541 of file icm20948.cpp.
References read_reg(), and write_checked_reg().
Referenced by ICM20948_mag::ak09916_setup(), ICM20948_mag::ak09916_setup_master_i2c(), reset_mpu(), and set_accel_range().
      
  | 
  private | 
Modify a register in the mpu.
Bits are cleared before bits are set.
| reg | The register to modify. | 
| clearbits | Bits in the register to clear. | 
| setbits | Bits in the register to set. | 
Definition at line 532 of file icm20948.cpp.
References read_reg(), and write_reg().
Referenced by ICM20948_mag::ak09916_setup().
| void ICM20948::print_info | ( | ) | 
Diagnostics - print some basic information about the driver.
Definition at line 857 of file icm20948.cpp.
References _bad_registers, _bad_transfers, _duplicates, _good_transfers, _mag, _px4_accel, _px4_gyro, _sample_perf, perf_print_counter(), PX4Accelerometer::print_status(), PX4Gyroscope::print_status(), and ICM20948_mag::print_status().
Referenced by icm20948::info().
      
  | 
  protectedvirtual | 
whoami result
Definition at line 283 of file icm20948.cpp.
References _checked_bad, _checked_registers, _checked_values, _icm20948_checked_registers, _num_checked_registers, _whoami, ICM20948_NUM_CHECKED_REGISTERS, ICM_WHOAMI_20948, ICMREG_20948_WHOAMI, MPUREG_WHOAMI, read_reg(), REG_BANK, and select_register_bank().
Referenced by init().
      
  | 
  private | 
Read a register from the mpu.
| The | register to read. | 
| The | bus speed to read with. | 
Definition at line 490 of file icm20948.cpp.
References _interface, ICM20948_SET_SPEED, device::Device::read(), REG_ADDRESS, REG_BANK, and select_register_bank().
Referenced by check_registers(), modify_checked_reg(), modify_reg(), probe(), and reset_mpu().
      
  | 
  private | 
Definition at line 512 of file icm20948.cpp.
References _interface, arraySize, ICM20948_LOW_SPEED_OP, device::Device::read(), REG_ADDRESS, REG_BANK, and select_register_bank().
      
  | 
  private | 
Read a register range from the mpu.
| The | start address to read from. | 
| The | bus speed to read with. | 
| The | address of the target data buffer. | 
| The | count of bytes to be read. | 
Definition at line 501 of file icm20948.cpp.
References _interface, ICM20948_SET_SPEED, device::Device::read(), REG_ADDRESS, REG_BANK, and select_register_bank().
Referenced by measure().
      
  | 
  private | 
Definition at line 172 of file icm20948.cpp.
References _mag, _reset_wait, _whoami, ICM20948_mag::ak09916_reset(), hrt_absolute_time(), ICM_WHOAMI_20948, OK, and reset_mpu().
      
  | 
  private | 
Resets the main chip (excluding the magnetometer if any).
Definition at line 197 of file icm20948.cpp.
References _checked_registers, _checked_values, _interface, _mag, _num_checked_registers, _px4_gyro, _sample_rate, _set_dlpf_filter(), _set_sample_rate(), _whoami, ACCEL_RANGE_G, BIT_H_RESET, BIT_I2C_IF_DIS, BIT_INT_ANYRD_2CLEAR, BIT_INT_BYPASS_EN, BIT_RAW_RDY_EN, device::Device::DeviceBusType_I2C, device::Device::get_device_bus_type(), ICM20948_DEFAULT_ONCHIP_FILTER_FREQ, ICM20948_LOW_SPEED_OP, ICM_BITS_DEC3_CFG_32, ICM_BITS_GYRO_FS_SEL_2000DPS, ICM_BITS_GYRO_FS_SEL_MASK, ICM_WHOAMI_20948, ICMREG_20948_ACCEL_CONFIG_2, ICMREG_20948_BANK_SEL, ICMREG_20948_GYRO_CONFIG_1, ICMREG_20948_INT_ENABLE_1, ICMREG_20948_INT_PIN_CFG, ICMREG_20948_PWR_MGMT_1, ICMREG_20948_PWR_MGMT_2, ICMREG_20948_USER_CTRL, ICM20948_mag::is_passthrough(), modify_checked_reg(), MPU_CLK_SEL_AUTO, OK, device::Device::read(), read_reg(), REG_BANK, set_accel_range(), PX4Gyroscope::set_scale(), write_checked_reg(), and write_reg().
Referenced by check_registers(), init(), and reset().
      
  | 
  overrideprotected | 
Definition at line 618 of file icm20948.cpp.
References measure().
      
  | 
  private | 
Select a register bank in ICM20948.
Only actually switches if the remembered bank is different from the requested one
| The | index of the register bank to switch to (0-3) | 
Definition at line 443 of file icm20948.cpp.
References _interface, _selected_bank, ICM20948_LOW_SPEED_OP, ICMREG_20948_BANK_SEL, OK, device::Device::read(), and device::Device::write().
Referenced by measure(), probe(), read_reg(), read_reg16(), read_reg_range(), and write_reg().
      
  | 
  private | 
Set the mpu measurement range.
| max_g | The maximum G value the range must support. | 
Definition at line 564 of file icm20948.cpp.
References _px4_accel, _whoami, CONSTANTS_ONE_G, ICM_BITS_ACCEL_FS_SEL_MASK, ICM_WHOAMI_20948, ICMREG_20948_ACCEL_CONFIG, modify_checked_reg(), OK, and PX4Accelerometer::set_scale().
Referenced by reset_mpu().
      
  | 
  private | 
Definition at line 603 of file icm20948.cpp.
References _call_interval, ICM20948_TIMER_REDUCTION, and stop().
Referenced by init().
      
  | 
  private | 
Definition at line 612 of file icm20948.cpp.
Referenced by start(), and ~ICM20948().
      
  | 
  inlineprivate | 
Swap a 16-bit value read from the mpu to native byte order.
Definition at line 521 of file icm20948.h.
      
  | 
  private | 
Write a register in the mpu, updating _checked_values.
| reg | The register to write. | 
| value | The new value to write. | 
Definition at line 550 of file icm20948.cpp.
References _checked_bad, _checked_registers, _checked_values, _num_checked_registers, and write_reg().
Referenced by _set_dlpf_filter(), _set_sample_rate(), modify_checked_reg(), and reset_mpu().
      
  | 
  private | 
Write a register in the mpu.
| reg | The register to write. | 
| value | The new value to write. | 
Definition at line 524 of file icm20948.cpp.
References _interface, ICM20948_LOW_SPEED_OP, REG_ADDRESS, REG_BANK, select_register_bank(), and device::Device::write().
Referenced by ICM20948_mag::ak09916_setup(), ICM20948_mag::ak09916_setup_master_i2c(), check_registers(), modify_reg(), ICM20948_mag::passthrough_read(), ICM20948_mag::passthrough_write(), reset_mpu(), ICM20948_mag::set_passthrough(), and write_checked_reg().
      
  | 
  friend | 
Definition at line 370 of file icm20948.h.
      
  | 
  private | 
Definition at line 393 of file icm20948.h.
Referenced by check_registers(), measure(), print_info(), and ~ICM20948().
      
  | 
  private | 
Definition at line 392 of file icm20948.h.
Referenced by check_null_data(), measure(), print_info(), and ~ICM20948().
      
  | 
  private | 
Definition at line 382 of file icm20948.h.
Referenced by start().
      
  | 
  private | 
Definition at line 410 of file icm20948.h.
Referenced by check_registers(), probe(), and write_checked_reg().
      
  | 
  private | 
Definition at line 411 of file icm20948.h.
Referenced by check_registers().
      
  | 
  private | 
Definition at line 407 of file icm20948.h.
Referenced by check_registers(), probe(), reset_mpu(), and write_checked_reg().
      
  | 
  private | 
Definition at line 409 of file icm20948.h.
Referenced by check_registers(), probe(), reset_mpu(), and write_checked_reg().
      
  | 
  private | 
Definition at line 384 of file icm20948.h.
      
  | 
  private | 
Definition at line 386 of file icm20948.h.
      
  | 
  private | 
Definition at line 385 of file icm20948.h.
      
  | 
  private | 
Definition at line 395 of file icm20948.h.
Referenced by check_duplicate(), print_info(), and ~ICM20948().
      
  | 
  private | 
Definition at line 394 of file icm20948.h.
Referenced by check_null_data(), print_info(), and ~ICM20948().
      
  | 
  private | 
Definition at line 423 of file icm20948.h.
Referenced by check_duplicate().
      
  | 
  staticprivate | 
      
  | 
  protected | 
Definition at line 365 of file icm20948.h.
Referenced by init(), ICM20948_mag::read_block(), read_reg(), read_reg16(), read_reg_range(), reset_mpu(), select_register_bank(), and write_reg().
      
  | 
  private | 
Definition at line 391 of file icm20948.h.
Referenced by measure(), and ~ICM20948().
      
  | 
  private | 
Definition at line 422 of file icm20948.h.
Referenced by check_duplicate().
      
  | 
  private | 
Definition at line 416 of file icm20948.h.
Referenced by ICM20948_mag::_measure(), and measure().
      
  | 
  private | 
Definition at line 379 of file icm20948.h.
Referenced by init(), measure(), print_info(), reset(), and reset_mpu().
      
  | 
  private | 
Definition at line 412 of file icm20948.h.
Referenced by check_registers(), probe(), reset_mpu(), and write_checked_reg().
      
  | 
  private | 
Definition at line 376 of file icm20948.h.
Referenced by ICM20948(), measure(), print_info(), and set_accel_range().
      
  | 
  private | 
Definition at line 377 of file icm20948.h.
Referenced by ICM20948(), measure(), print_info(), and reset_mpu().
      
  | 
  private | 
Definition at line 397 of file icm20948.h.
Referenced by check_registers(), and measure().
      
  | 
  private | 
Definition at line 398 of file icm20948.h.
Referenced by check_registers(), init(), measure(), and reset().
      
  | 
  private | 
Definition at line 390 of file icm20948.h.
Referenced by check_duplicate(), check_null_data(), measure(), print_info(), and ~ICM20948().
      
  | 
  private | 
Definition at line 388 of file icm20948.h.
Referenced by _set_sample_rate(), init(), and reset_mpu().
      
  | 
  private | 
Definition at line 380 of file icm20948.h.
Referenced by select_register_bank().
      
  | 
  protected | 
Definition at line 366 of file icm20948.h.
Referenced by _set_dlpf_filter(), _set_sample_rate(), probe(), reset(), reset_mpu(), and set_accel_range().
      
  | 
  staticprivate | 
Definition at line 404 of file icm20948.h.
Referenced by probe().