43 #include <px4_platform_common/px4_config.h> 44 #include <px4_platform_common/time.h> 62 #define ICM20948_TIMER_REDUCTION 200 65 #define ACCEL_RANGE_G 16 88 ScheduledWorkItem(MODULE_NAME,
px4::device_bus_to_wq(interface->get_device_id())),
89 _interface(interface),
92 _mag(this, mag_interface, rotation),
138 PX4_DEBUG(
"probe failed");
145 PX4_ERR(
"Exiting! Device failed to take initialization");
155 PX4_ERR(
"failed to setup ak09916 interface");
163 PX4_DEBUG(
"mag reset failed");
260 while (!all_ok && retries--) {
265 uint8_t bankcheck = 0;
279 return all_ok ?
OK : -EIO;
307 PX4_DEBUG(
"unexpected whoami 0x%02x",
_whoami);
322 if (desired_sample_rate_hz == 0) {
328 div = 1100 / desired_sample_rate_hz;
332 if (div > 200) { div = 200; }
334 if (div < 1) { div = 1; }
362 if (frequency_hz == 0) {
366 }
else if (frequency_hz <= 5) {
370 }
else if (frequency_hz <= 11) {
374 }
else if (frequency_hz <= 23) {
378 }
else if (frequency_hz <= 51) {
382 }
else if (frequency_hz <= 119) {
386 }
else if (frequency_hz <= 151) {
390 }
else if (frequency_hz <= 197) {
404 if (frequency_hz == 0) {
408 }
else if (frequency_hz <= 5) {
412 }
else if (frequency_hz <= 11) {
416 }
else if (frequency_hz <= 23) {
420 }
else if (frequency_hz <= 50) {
424 }
else if (frequency_hz <= 111) {
428 }
else if (frequency_hz <= 246) {
463 while (bank != buf && retries > 0) {
481 PX4_DEBUG(
"SELECT FAILED %d %d %d %d", retries,
_selected_bank, bank, buf);
520 return (uint16_t)(buf[0] << 8) | buf[1];
575 }
else if (max_g_in > 4) {
580 }
else if (max_g_in > 2) {
763 sizeof(icm_report))) {
828 int16_t accel_xt = report.accel_y;
829 int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x);
831 int16_t gyro_xt = report.gyro_y;
832 int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x);
835 report.accel_x = accel_xt;
836 report.accel_y = accel_yt;
837 report.gyro_x = gyro_xt;
838 report.gyro_y = gyro_yt;
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);
#define BIT_INT_ANYRD_2CLEAR
#define ICM_BITS_GYRO_DLPF_CFG_361HZ
#define ICMREG_20948_PWR_MGMT_2
#define BIT_INT_BYPASS_EN
virtual int init()
Initialise the driver and make it ready for use.
static const uint16_t _icm20948_checked_registers[ICM20948_NUM_CHECKED_REGISTERS]
#define ICM_BITS_GYRO_DLPF_CFG_5HZ
unsigned _num_checked_registers
measure the time elapsed performing an event
uint16_t read_reg16(unsigned reg)
perf_counter_t _sample_perf
int select_register_bank(uint8_t bank)
Select a register bank in ICM20948.
Definition of geo / math functions to perform geodesic calculations.
device::Device * _interface
void update(hrt_abstime timestamp, float x, float y, float z)
#define ICMREG_20948_GYRO_CONFIG_2
#define ICMREG_20948_ACCEL_SMPLRT_DIV_1
Report conversation within the mpu, including command byte and interrupt status.
#define ICM20948_LOW_SPEED_OP(r)
#define ICMREG_20948_USER_CTRL
#define ICMREG_20948_INT_ENABLE_3
#define ICM_BITS_DEC3_CFG_32
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
Modify a register in the mpu.
#define ICM_BITS_GYRO_DLPF_CFG_51HZ
A set of useful macros for enhanced runtime and compile time error detection and warning suppression...
#define ICMREG_20948_ACCEL_SMPLRT_DIV_2
#define ICM_BITS_GYRO_DLPF_CFG_151HZ
perf_counter_t _duplicates
count the number of times an event occurs
#define ICMREG_20948_WHOAMI
High-resolution timer with callouts and timekeeping.
void set_device_type(uint8_t devtype)
static constexpr float CONSTANTS_ONE_G
void set_error_count(uint64_t error_count)
#define ICM_BITS_ACCEL_DLPF_CFG_50HZ
#define ICM20948_HIGH_BUS_SPEED
void _set_dlpf_filter(uint16_t frequency_hz)
#define ICMREG_20948_INT_PIN_CFG
#define ICM20948_SET_SPEED(r, s)
device::Device * _interface
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.
uint8_t read_reg(unsigned reg, uint32_t speed=ICM20948_LOW_BUS_SPEED)
Read a register from the mpu.
void _measure(hrt_abstime timestamp_sample, ak09916_regs data)
perf_counter_t _interval_perf
void perf_count(perf_counter_t handle)
Count a performance event.
Report conversation within the mpu, including command byte and interrupt status.
#define ICM20948_DEFAULT_ONCHIP_FILTER_FREQ
void perf_free(perf_counter_t handle)
Free a counter.
void set_temperature(float temperature)
#define ICM_BITS_GYRO_FS_SEL_MASK
Definition of commonly used conversions.
#define ICMREG_20948_GYRO_SMPLRT_DIV
void measure()
Fetch measurements from the sensor and update the report buffers.
Rotation
Enum for board and external compass rotations.
#define ICM_BITS_GYRO_DLPF_CFG_119HZ
static struct mag_report _mag
mag report
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
#define ICMREG_20948_ACCEL_XOUT_H
#define ICMREG_20948_INT_ENABLE_1
virtual int probe()
whoami result
int reset_mpu()
Resets the main chip (excluding the magnetometer if any).
uint8_t _last_accel_data[6]
#define ICM_BITS_ACCEL_FS_SEL_MASK
void write_checked_reg(unsigned reg, uint8_t value)
Write a register in the mpu, updating _checked_values.
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.
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
ICM20948(device::Device *interface, device::Device *mag_interface, enum Rotation rotation)
virtual int write(unsigned address, void *data, unsigned count)
Write directly to the device.
#define ICM_BITS_ACCEL_DLPF_CFG_11HZ
#define ICM_BITS_ACCEL_DLPF_CFG_23HZ
#define ICMREG_20948_ACCEL_CONFIG_2
static constexpr int ICM20948_NUM_CHECKED_REGISTERS
void set_scale(float scale)
const uint16_t * _checked_registers
#define ICMREG_20948_INT_ENABLE
#define ICM_BITS_GYRO_DLPF_CFG_11HZ
void set_temperature(float temperature)
#define ICM_BITS_ACCEL_DLPF_CFG_473HZ
#define ICM_BITS_ACCEL_DLPF_CFG_246HZ
#define ICMREG_20948_PWR_MGMT_1
#define ICMREG_20948_INT_ENABLE_2
#define ICM_BITS_GYRO_FS_SEL_2000DPS
bool check_duplicate(uint8_t *accel_data)
#define ICM_BITS_ACCEL_DLPF_CFG_5HZ
int set_accel_range(unsigned max_g)
Set the mpu measurement range.
void modify_checked_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
Modify a checked register in the mpu.
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.
Fundamental base class for all physical drivers (I2C, SPI).
#define ICM_BITS_ACCEL_DLPF_CFG_111HZ
void _set_sample_rate(unsigned desired_sample_rate_hz)
PX4Accelerometer _px4_accel
#define ICMREG_20948_GYRO_CONFIG_1
#define ICM_BITS_GYRO_DLPF_CFG_197HZ
bool check_null_data(uint16_t *data, uint8_t size)
int16_t int16_t_from_bytes(uint8_t bytes[])
Converts a signed 16 bit integer from big endian to little endian.
void print_info()
Diagnostics - print some basic information about the driver.
uint8_t _checked_bad[ICM20948_NUM_CHECKED_REGISTERS]
void set_scale(float scale)
DeviceBusType get_device_bus_type() const
Return the bus type the device is connected to.
void set_device_type(uint8_t devtype)
#define ICM20948_GYRO_DEFAULT_RATE
perf_counter_t _good_transfers
#define ICMREG_20948_ACCEL_CONFIG
perf_counter_t _bad_transfers
#define ICM_BITS_GYRO_DLPF_CFG_23HZ
perf_counter_t _bad_registers
#define ICM20948_TIMER_REDUCTION
#define ICMREG_20948_BANK_SEL
#define DRV_DEVTYPE_ICM20948
void perf_begin(perf_counter_t handle)
Begin a performance event.
uint8_t _checked_values[ICM20948_NUM_CHECKED_REGISTERS]
__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.
Performance measuring tools.