Daniel Agar
5 years ago
committed by
GitHub
14 changed files with 1965 additions and 7 deletions
@ -0,0 +1,105 @@
@@ -0,0 +1,105 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2019 PX4 Development Team. All rights reserved. |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/**
|
||||
* @file AKM_AK09916_registers.hpp |
||||
* |
||||
* Asahi Kasei Microdevices (AKM) AK09916 registers. |
||||
* |
||||
*/ |
||||
|
||||
#pragma once |
||||
|
||||
#include <cstdint> |
||||
|
||||
namespace AKM_AK09916 |
||||
{ |
||||
|
||||
// TODO: move to a central header
|
||||
static constexpr uint8_t Bit0 = (1 << 0); |
||||
static constexpr uint8_t Bit1 = (1 << 1); |
||||
static constexpr uint8_t Bit2 = (1 << 2); |
||||
static constexpr uint8_t Bit3 = (1 << 3); |
||||
static constexpr uint8_t Bit4 = (1 << 4); |
||||
static constexpr uint8_t Bit5 = (1 << 5); |
||||
static constexpr uint8_t Bit6 = (1 << 6); |
||||
static constexpr uint8_t Bit7 = (1 << 7); |
||||
|
||||
static constexpr uint32_t I2C_SPEED = 400 * 1000; // 400 kHz I2C serial interface
|
||||
static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0b0001100; |
||||
|
||||
static constexpr uint8_t WHOAMI = 0x09; |
||||
|
||||
enum class Register : uint8_t { |
||||
WIA = 0x01, // Device ID
|
||||
|
||||
ST1 = 0x10, // Status 1
|
||||
HXL = 0x11, |
||||
HXH = 0x12, |
||||
HYL = 0x13, |
||||
HYH = 0x14, |
||||
HZL = 0x15, |
||||
HZH = 0x16, |
||||
|
||||
ST2 = 0x18, // Status 2
|
||||
|
||||
CNTL2 = 0x31, // Control 2
|
||||
CNTL3 = 0x32, // Control 3
|
||||
}; |
||||
|
||||
// ST1
|
||||
enum ST1_BIT : uint8_t { |
||||
DOR = Bit1, // Data overrun
|
||||
DRDY = Bit0, // Data is ready
|
||||
}; |
||||
|
||||
// ST2
|
||||
enum ST2_BIT : uint8_t { |
||||
BITM = Bit4, // Output bit setting (mirror)
|
||||
HOFL = Bit3, // Magnetic sensor overflow
|
||||
}; |
||||
|
||||
// CNTL2
|
||||
enum CNTL2_BIT : uint8_t { |
||||
MODE1 = Bit1, // Continuous measurement mode 1 (10Hz)
|
||||
MODE2 = Bit2, // Continuous measurement mode 2 (20Hz)
|
||||
MODE3 = Bit2 | Bit1, // Continuous measurement mode 3 (50Hz)
|
||||
MODE4 = Bit3, // Continuous measurement mode 4 (100Hz)
|
||||
}; |
||||
|
||||
// CNTL3
|
||||
enum CNTL3_BIT : uint8_t { |
||||
SRST = Bit0, |
||||
}; |
||||
|
||||
} // namespace AKM_AK09916
|
||||
@ -0,0 +1,52 @@
@@ -0,0 +1,52 @@
|
||||
############################################################################ |
||||
# |
||||
# Copyright (c) 2020 PX4 Development Team. All rights reserved. |
||||
# |
||||
# Redistribution and use in source and binary forms, with or without |
||||
# modification, are permitted provided that the following conditions |
||||
# are met: |
||||
# |
||||
# 1. Redistributions of source code must retain the above copyright |
||||
# notice, this list of conditions and the following disclaimer. |
||||
# 2. Redistributions in binary form must reproduce the above copyright |
||||
# notice, this list of conditions and the following disclaimer in |
||||
# the documentation and/or other materials provided with the |
||||
# distribution. |
||||
# 3. Neither the name PX4 nor the names of its contributors may be |
||||
# used to endorse or promote products derived from this software |
||||
# without specific prior written permission. |
||||
# |
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
# POSSIBILITY OF SUCH DAMAGE. |
||||
# |
||||
############################################################################ |
||||
px4_add_module( |
||||
MODULE drivers__imu__icm20948 |
||||
MAIN icm20948 |
||||
COMPILE_FLAGS |
||||
-DDEBUG_BUILD |
||||
-O0 |
||||
SRCS |
||||
AKM_AK09916_registers.hpp |
||||
ICM20948.cpp |
||||
ICM20948.hpp |
||||
ICM20948_AK09916.cpp |
||||
ICM20948_AK09916.hpp |
||||
icm20948_main.cpp |
||||
InvenSense_ICM20948_registers.hpp |
||||
DEPENDS |
||||
px4_work_queue |
||||
drivers_accelerometer |
||||
drivers_gyroscope |
||||
drivers_magnetometer |
||||
) |
||||
@ -0,0 +1,755 @@
@@ -0,0 +1,755 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2020 PX4 Development Team. All rights reserved. |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
#include "ICM20948.hpp" |
||||
|
||||
#include "AKM_AK09916_registers.hpp" |
||||
|
||||
using namespace time_literals; |
||||
|
||||
static constexpr int16_t combine(uint8_t msb, uint8_t lsb) |
||||
{ |
||||
return (msb << 8u) | lsb; |
||||
} |
||||
|
||||
ICM20948::ICM20948(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, |
||||
spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio, bool enable_magnetometer) : |
||||
SPI(DRV_IMU_DEVTYPE_ICM20948, MODULE_NAME, bus, device, spi_mode, bus_frequency), |
||||
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), |
||||
_drdy_gpio(drdy_gpio), |
||||
_px4_accel(get_device_id(), ORB_PRIO_DEFAULT, rotation), |
||||
_px4_gyro(get_device_id(), ORB_PRIO_DEFAULT, rotation) |
||||
{ |
||||
ConfigureSampleRate(_px4_gyro.get_max_rate_hz()); |
||||
|
||||
if (enable_magnetometer) { |
||||
_slave_ak09916_magnetometer = new AKM_AK09916::ICM20948_AK09916(*this, rotation); |
||||
|
||||
if (_slave_ak09916_magnetometer) { |
||||
for (auto &r : _register_bank3_cfg) { |
||||
if (r.reg == Register::BANK_3::I2C_SLV4_CTRL) { |
||||
r.set_bits = I2C_SLV4_CTRL_BIT::I2C_MST_DLY; |
||||
|
||||
} else if (r.reg == Register::BANK_3::I2C_MST_CTRL) { |
||||
r.set_bits = I2C_MST_CTRL_BIT::I2C_MST_P_NSR | I2C_MST_CTRL_BIT::I2C_MST_CLK_400_kHz; |
||||
|
||||
} else if (r.reg == Register::BANK_3::I2C_MST_DELAY_CTRL) { |
||||
r.set_bits = I2C_MST_DELAY_CTRL_BIT::I2C_SLVX_DLY_EN; |
||||
} |
||||
} |
||||
} |
||||
} |
||||
} |
||||
|
||||
ICM20948::~ICM20948() |
||||
{ |
||||
perf_free(_transfer_perf); |
||||
perf_free(_bad_register_perf); |
||||
perf_free(_bad_transfer_perf); |
||||
perf_free(_fifo_empty_perf); |
||||
perf_free(_fifo_overflow_perf); |
||||
perf_free(_fifo_reset_perf); |
||||
perf_free(_drdy_interval_perf); |
||||
|
||||
delete _slave_ak09916_magnetometer; |
||||
} |
||||
|
||||
int ICM20948::init() |
||||
{ |
||||
int ret = SPI::init(); |
||||
|
||||
if (ret != PX4_OK) { |
||||
DEVICE_DEBUG("SPI::init failed (%i)", ret); |
||||
return ret; |
||||
} |
||||
|
||||
return Reset() ? 0 : -1; |
||||
} |
||||
|
||||
bool ICM20948::Reset() |
||||
{ |
||||
_state = STATE::RESET; |
||||
ScheduleClear(); |
||||
ScheduleNow(); |
||||
return true; |
||||
} |
||||
|
||||
void ICM20948::exit_and_cleanup() |
||||
{ |
||||
DataReadyInterruptDisable(); |
||||
I2CSPIDriverBase::exit_and_cleanup(); |
||||
} |
||||
|
||||
void ICM20948::print_status() |
||||
{ |
||||
I2CSPIDriverBase::print_status(); |
||||
PX4_INFO("FIFO empty interval: %d us (%.3f Hz)", _fifo_empty_interval_us, |
||||
static_cast<double>(1000000 / _fifo_empty_interval_us)); |
||||
|
||||
perf_print_counter(_transfer_perf); |
||||
perf_print_counter(_bad_register_perf); |
||||
perf_print_counter(_bad_transfer_perf); |
||||
perf_print_counter(_fifo_empty_perf); |
||||
perf_print_counter(_fifo_overflow_perf); |
||||
perf_print_counter(_fifo_reset_perf); |
||||
perf_print_counter(_drdy_interval_perf); |
||||
|
||||
_px4_accel.print_status(); |
||||
_px4_gyro.print_status(); |
||||
|
||||
if (_slave_ak09916_magnetometer) { |
||||
_slave_ak09916_magnetometer->PrintInfo(); |
||||
} |
||||
} |
||||
|
||||
int ICM20948::probe() |
||||
{ |
||||
const uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I); |
||||
|
||||
if (whoami != WHOAMI) { |
||||
DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); |
||||
return PX4_ERROR; |
||||
} |
||||
|
||||
return PX4_OK; |
||||
} |
||||
|
||||
void ICM20948::RunImpl() |
||||
{ |
||||
switch (_state) { |
||||
case STATE::RESET: |
||||
// PWR_MGMT_1: Device Reset
|
||||
RegisterWrite(Register::BANK_0::PWR_MGMT_1, PWR_MGMT_1_BIT::DEVICE_RESET); |
||||
_reset_timestamp = hrt_absolute_time(); |
||||
_state = STATE::WAIT_FOR_RESET; |
||||
ScheduleDelayed(10_ms); |
||||
break; |
||||
|
||||
case STATE::WAIT_FOR_RESET: |
||||
|
||||
// The reset value is 0x00 for all registers other than the registers below
|
||||
if ((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI) |
||||
&& (RegisterRead(Register::BANK_0::PWR_MGMT_1) == 0x41)) { |
||||
|
||||
// if reset succeeded then configure
|
||||
_state = STATE::CONFIGURE; |
||||
ScheduleDelayed(10_ms); |
||||
|
||||
} else { |
||||
// RESET not complete
|
||||
if (hrt_elapsed_time(&_reset_timestamp) > 100_ms) { |
||||
PX4_DEBUG("Reset failed, retrying"); |
||||
_state = STATE::RESET; |
||||
ScheduleDelayed(100_ms); |
||||
|
||||
} else { |
||||
PX4_DEBUG("Reset not complete, check again in 10 ms"); |
||||
ScheduleDelayed(10_ms); |
||||
} |
||||
} |
||||
|
||||
break; |
||||
|
||||
case STATE::CONFIGURE: |
||||
if (Configure()) { |
||||
|
||||
// start AK09916 magnetometer (I2C aux)
|
||||
if (_slave_ak09916_magnetometer) { |
||||
_slave_ak09916_magnetometer->Reset(); |
||||
} |
||||
|
||||
// if configure succeeded then start reading from FIFO
|
||||
_state = STATE::FIFO_READ; |
||||
|
||||
if (DataReadyInterruptConfigure()) { |
||||
_data_ready_interrupt_enabled = true; |
||||
|
||||
// backup schedule as a watchdog timeout
|
||||
ScheduleDelayed(10_ms); |
||||
|
||||
} else { |
||||
_data_ready_interrupt_enabled = false; |
||||
ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us); |
||||
} |
||||
|
||||
FIFOReset(); |
||||
|
||||
} else { |
||||
PX4_DEBUG("Configure failed, retrying"); |
||||
// try again in 10 ms
|
||||
ScheduleDelayed(10_ms); |
||||
} |
||||
|
||||
break; |
||||
|
||||
case STATE::FIFO_READ: { |
||||
hrt_abstime timestamp_sample = 0; |
||||
uint8_t samples = 0; |
||||
|
||||
if (_data_ready_interrupt_enabled) { |
||||
// re-schedule as watchdog timeout
|
||||
ScheduleDelayed(10_ms); |
||||
|
||||
// timestamp set in data ready interrupt
|
||||
if (!_force_fifo_count_check) { |
||||
samples = _fifo_read_samples.load(); |
||||
|
||||
} else { |
||||
const uint16_t fifo_count = FIFOReadCount(); |
||||
samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) * SAMPLES_PER_TRANSFER; // round down to nearest
|
||||
} |
||||
|
||||
timestamp_sample = _fifo_watermark_interrupt_timestamp; |
||||
} |
||||
|
||||
bool failure = false; |
||||
|
||||
// manually check FIFO count if no samples from DRDY or timestamp looks bogus
|
||||
if (!_data_ready_interrupt_enabled || (samples == 0) |
||||
|| (hrt_elapsed_time(×tamp_sample) > (_fifo_empty_interval_us / 2))) { |
||||
|
||||
// use the time now roughly corresponding with the last sample we'll pull from the FIFO
|
||||
timestamp_sample = hrt_absolute_time(); |
||||
const uint16_t fifo_count = FIFOReadCount(); |
||||
samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) * SAMPLES_PER_TRANSFER; // round down to nearest
|
||||
} |
||||
|
||||
if (samples > FIFO_MAX_SAMPLES) { |
||||
// not technically an overflow, but more samples than we expected or can publish
|
||||
perf_count(_fifo_overflow_perf); |
||||
failure = true; |
||||
FIFOReset(); |
||||
|
||||
} else if (samples >= SAMPLES_PER_TRANSFER) { |
||||
// require at least SAMPLES_PER_TRANSFER (we want at least 1 new accel sample per transfer)
|
||||
if (!FIFORead(timestamp_sample, samples)) { |
||||
failure = true; |
||||
_px4_accel.increase_error_count(); |
||||
_px4_gyro.increase_error_count(); |
||||
} |
||||
|
||||
} else if (samples == 0) { |
||||
failure = true; |
||||
perf_count(_fifo_empty_perf); |
||||
} |
||||
|
||||
if (failure || hrt_elapsed_time(&_last_config_check_timestamp) > 10_ms) { |
||||
// check BANK_0 & BANK_2 registers incrementally
|
||||
if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0], true) |
||||
&& RegisterCheck(_register_bank2_cfg[_checked_register_bank2], true) |
||||
&& RegisterCheck(_register_bank3_cfg[_checked_register_bank3], true) |
||||
) { |
||||
_last_config_check_timestamp = timestamp_sample; |
||||
_checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg; |
||||
_checked_register_bank2 = (_checked_register_bank2 + 1) % size_register_bank2_cfg; |
||||
_checked_register_bank3 = (_checked_register_bank3 + 1) % size_register_bank3_cfg; |
||||
|
||||
} else { |
||||
// register check failed, force reconfigure
|
||||
PX4_DEBUG("Health check failed, reconfiguring"); |
||||
_state = STATE::CONFIGURE; |
||||
ScheduleNow(); |
||||
} |
||||
|
||||
} else { |
||||
// periodically update temperature (1 Hz)
|
||||
if (hrt_elapsed_time(&_temperature_update_timestamp) > 1_s) { |
||||
UpdateTemperature(); |
||||
_temperature_update_timestamp = timestamp_sample; |
||||
} |
||||
} |
||||
} |
||||
|
||||
break; |
||||
} |
||||
} |
||||
|
||||
void ICM20948::ConfigureAccel() |
||||
{ |
||||
const uint8_t ACCEL_FS_SEL = RegisterRead(Register::BANK_2::ACCEL_CONFIG) & (Bit2 | Bit1); // 2:1 ACCEL_FS_SEL[1:0]
|
||||
|
||||
switch (ACCEL_FS_SEL) { |
||||
case ACCEL_FS_SEL_2G: |
||||
_px4_accel.set_scale(CONSTANTS_ONE_G / 16384.f); |
||||
_px4_accel.set_range(2.f * CONSTANTS_ONE_G); |
||||
break; |
||||
|
||||
case ACCEL_FS_SEL_4G: |
||||
_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f); |
||||
_px4_accel.set_range(4.f * CONSTANTS_ONE_G); |
||||
break; |
||||
|
||||
case ACCEL_FS_SEL_8G: |
||||
_px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f); |
||||
_px4_accel.set_range(8.f * CONSTANTS_ONE_G); |
||||
break; |
||||
|
||||
case ACCEL_FS_SEL_16G: |
||||
_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f); |
||||
_px4_accel.set_range(16.f * CONSTANTS_ONE_G); |
||||
break; |
||||
} |
||||
} |
||||
|
||||
void ICM20948::ConfigureGyro() |
||||
{ |
||||
const uint8_t GYRO_FS_SEL = RegisterRead(Register::BANK_2::GYRO_CONFIG_1) & (Bit2 | Bit1); // 2:1 GYRO_FS_SEL[1:0]
|
||||
|
||||
switch (GYRO_FS_SEL) { |
||||
case GYRO_FS_SEL_250_DPS: |
||||
_px4_gyro.set_scale(math::radians(1.f / 131.f)); |
||||
_px4_gyro.set_range(math::radians(250.f)); |
||||
break; |
||||
|
||||
case GYRO_FS_SEL_500_DPS: |
||||
_px4_gyro.set_scale(math::radians(1.f / 65.5f)); |
||||
_px4_gyro.set_range(math::radians(500.f)); |
||||
break; |
||||
|
||||
case GYRO_FS_SEL_1000_DPS: |
||||
_px4_gyro.set_scale(math::radians(1.f / 32.8f)); |
||||
_px4_gyro.set_range(math::radians(1000.f)); |
||||
break; |
||||
|
||||
case GYRO_FS_SEL_2000_DPS: |
||||
_px4_gyro.set_scale(math::radians(1.f / 16.4f)); |
||||
_px4_gyro.set_range(math::radians(2000.f)); |
||||
break; |
||||
} |
||||
} |
||||
|
||||
void ICM20948::ConfigureSampleRate(int sample_rate) |
||||
{ |
||||
if (sample_rate == 0) { |
||||
sample_rate = 800; // default to ~800 Hz
|
||||
} |
||||
|
||||
// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER
|
||||
const float min_interval = SAMPLES_PER_TRANSFER * FIFO_SAMPLE_DT; |
||||
_fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval); |
||||
|
||||
_fifo_gyro_samples = roundf(math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES)); |
||||
|
||||
// recompute FIFO empty interval (us) with actual gyro sample limit
|
||||
_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE); |
||||
|
||||
_fifo_accel_samples = roundf(math::min(_fifo_empty_interval_us / (1e6f / ACCEL_RATE), (float)FIFO_MAX_SAMPLES)); |
||||
|
||||
_px4_accel.set_update_rate(1e6f / _fifo_empty_interval_us); |
||||
_px4_gyro.set_update_rate(1e6f / _fifo_empty_interval_us); |
||||
} |
||||
|
||||
void ICM20948::SelectRegisterBank(enum REG_BANK_SEL_BIT bank) |
||||
{ |
||||
if (bank != _last_register_bank) { |
||||
// select BANK_0
|
||||
uint8_t cmd_bank_sel[2] {}; |
||||
cmd_bank_sel[0] = static_cast<uint8_t>(Register::BANK_0::REG_BANK_SEL); |
||||
cmd_bank_sel[1] = bank; |
||||
transfer(cmd_bank_sel, cmd_bank_sel, sizeof(cmd_bank_sel)); |
||||
|
||||
_last_register_bank = bank; |
||||
} |
||||
} |
||||
|
||||
bool ICM20948::Configure() |
||||
{ |
||||
bool success = true; |
||||
|
||||
for (const auto ® : _register_bank0_cfg) { |
||||
if (!RegisterCheck(reg)) { |
||||
success = false; |
||||
} |
||||
} |
||||
|
||||
for (const auto ® : _register_bank2_cfg) { |
||||
if (!RegisterCheck(reg)) { |
||||
success = false; |
||||
} |
||||
} |
||||
|
||||
for (const auto ® : _register_bank3_cfg) { |
||||
if (!RegisterCheck(reg)) { |
||||
success = false; |
||||
} |
||||
} |
||||
|
||||
ConfigureAccel(); |
||||
ConfigureGyro(); |
||||
|
||||
return success; |
||||
} |
||||
|
||||
int ICM20948::DataReadyInterruptCallback(int irq, void *context, void *arg) |
||||
{ |
||||
static_cast<ICM20948 *>(arg)->DataReady(); |
||||
return 0; |
||||
} |
||||
|
||||
void ICM20948::DataReady() |
||||
{ |
||||
perf_count(_drdy_interval_perf); |
||||
|
||||
if (_data_ready_count.fetch_add(1) >= (_fifo_gyro_samples - 1)) { |
||||
_data_ready_count.store(0); |
||||
_fifo_watermark_interrupt_timestamp = hrt_absolute_time(); |
||||
_fifo_read_samples.store(_fifo_gyro_samples); |
||||
ScheduleNow(); |
||||
} |
||||
} |
||||
|
||||
bool ICM20948::DataReadyInterruptConfigure() |
||||
{ |
||||
if (_drdy_gpio == 0) { |
||||
return false; |
||||
} |
||||
|
||||
// Setup data ready on falling edge
|
||||
return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0; |
||||
} |
||||
|
||||
bool ICM20948::DataReadyInterruptDisable() |
||||
{ |
||||
if (_drdy_gpio == 0) { |
||||
return false; |
||||
} |
||||
|
||||
return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0; |
||||
} |
||||
|
||||
template <typename T> |
||||
bool ICM20948::RegisterCheck(const T ®_cfg, bool notify) |
||||
{ |
||||
bool success = true; |
||||
|
||||
const uint8_t reg_value = RegisterRead(reg_cfg.reg); |
||||
|
||||
if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) { |
||||
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits); |
||||
success = false; |
||||
} |
||||
|
||||
if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) { |
||||
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits); |
||||
success = false; |
||||
} |
||||
|
||||
if (!success) { |
||||
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); |
||||
|
||||
if (notify) { |
||||
perf_count(_bad_register_perf); |
||||
_px4_accel.increase_error_count(); |
||||
_px4_gyro.increase_error_count(); |
||||
} |
||||
} |
||||
|
||||
return success; |
||||
} |
||||
|
||||
template <typename T> |
||||
uint8_t ICM20948::RegisterRead(T reg) |
||||
{ |
||||
SelectRegisterBank(reg); |
||||
|
||||
uint8_t cmd[2] {}; |
||||
cmd[0] = static_cast<uint8_t>(reg) | DIR_READ; |
||||
transfer(cmd, cmd, sizeof(cmd)); |
||||
return cmd[1]; |
||||
} |
||||
|
||||
template <typename T> |
||||
void ICM20948::RegisterWrite(T reg, uint8_t value) |
||||
{ |
||||
SelectRegisterBank(reg); |
||||
|
||||
uint8_t cmd[2] { (uint8_t)reg, value }; |
||||
transfer(cmd, cmd, sizeof(cmd)); |
||||
} |
||||
|
||||
template <typename T> |
||||
void ICM20948::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits) |
||||
{ |
||||
const uint8_t orig_val = RegisterRead(reg); |
||||
uint8_t val = orig_val; |
||||
|
||||
if (setbits) { |
||||
val |= setbits; |
||||
} |
||||
|
||||
if (clearbits) { |
||||
val &= ~clearbits; |
||||
} |
||||
|
||||
RegisterWrite(reg, val); |
||||
} |
||||
|
||||
uint16_t ICM20948::FIFOReadCount() |
||||
{ |
||||
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); |
||||
|
||||
// read FIFO count
|
||||
uint8_t fifo_count_buf[3] {}; |
||||
fifo_count_buf[0] = static_cast<uint8_t>(Register::BANK_0::FIFO_COUNTH) | DIR_READ; |
||||
|
||||
if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) { |
||||
perf_count(_bad_transfer_perf); |
||||
return 0; |
||||
} |
||||
|
||||
return combine(fifo_count_buf[1], fifo_count_buf[2]); |
||||
} |
||||
|
||||
bool ICM20948::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples) |
||||
{ |
||||
perf_begin(_transfer_perf); |
||||
|
||||
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); |
||||
|
||||
FIFOTransferBuffer buffer{}; |
||||
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 3, FIFO::SIZE); |
||||
|
||||
if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) { |
||||
perf_end(_transfer_perf); |
||||
perf_count(_bad_transfer_perf); |
||||
return false; |
||||
} |
||||
|
||||
perf_end(_transfer_perf); |
||||
|
||||
const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL); |
||||
const uint16_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA); |
||||
|
||||
if (fifo_count_samples == 0) { |
||||
perf_count(_fifo_empty_perf); |
||||
return false; |
||||
} |
||||
|
||||
if (fifo_count_bytes >= FIFO::SIZE) { |
||||
perf_count(_fifo_overflow_perf); |
||||
FIFOReset(); |
||||
return false; |
||||
} |
||||
|
||||
const uint16_t valid_samples = math::min(samples, fifo_count_samples); |
||||
|
||||
if (fifo_count_samples < samples) { |
||||
// force check if there is somehow fewer samples actually in the FIFO (potentially a serious error)
|
||||
_force_fifo_count_check = true; |
||||
|
||||
} else if (fifo_count_samples >= samples + 2) { |
||||
// if we're more than a couple samples behind force FIFO_COUNT check
|
||||
_force_fifo_count_check = true; |
||||
|
||||
} else { |
||||
// skip earlier FIFO_COUNT and trust DRDY count if we're in sync
|
||||
_force_fifo_count_check = false; |
||||
} |
||||
|
||||
if (valid_samples > 0) { |
||||
ProcessGyro(timestamp_sample, buffer, valid_samples); |
||||
|
||||
if (ProcessAccel(timestamp_sample, buffer, valid_samples)) { |
||||
return true; |
||||
} |
||||
} |
||||
|
||||
// force FIFO count check if there was any other error
|
||||
_force_fifo_count_check = true; |
||||
|
||||
return false; |
||||
} |
||||
|
||||
void ICM20948::FIFOReset() |
||||
{ |
||||
perf_count(_fifo_reset_perf); |
||||
|
||||
// FIFO_RST: reset FIFO
|
||||
RegisterSetBits(Register::BANK_0::FIFO_RST, FIFO_RST_BIT::FIFO_RESET); |
||||
RegisterClearBits(Register::BANK_0::FIFO_RST, FIFO_RST_BIT::FIFO_RESET); |
||||
|
||||
// reset while FIFO is disabled
|
||||
_data_ready_count.store(0); |
||||
_fifo_watermark_interrupt_timestamp = 0; |
||||
_fifo_read_samples.store(0); |
||||
} |
||||
|
||||
static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1) |
||||
{ |
||||
return (memcmp(&f0.ACCEL_XOUT_H, &f1.ACCEL_XOUT_H, 6) == 0); |
||||
} |
||||
|
||||
bool ICM20948::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, |
||||
const uint8_t samples) |
||||
{ |
||||
PX4Accelerometer::FIFOSample accel; |
||||
accel.timestamp_sample = timestamp_sample; |
||||
accel.dt = _fifo_empty_interval_us / _fifo_accel_samples; |
||||
|
||||
bool bad_data = false; |
||||
|
||||
// accel data is doubled in FIFO, but might be shifted
|
||||
int accel_first_sample = 1; |
||||
|
||||
if (samples >= 4) { |
||||
if (fifo_accel_equal(buffer.f[0], buffer.f[1]) && fifo_accel_equal(buffer.f[2], buffer.f[3])) { |
||||
// [A0, A1, A2, A3]
|
||||
// A0==A1, A2==A3
|
||||
accel_first_sample = 1; |
||||
|
||||
} else if (fifo_accel_equal(buffer.f[1], buffer.f[2])) { |
||||
// [A0, A1, A2, A3]
|
||||
// A0, A1==A2, A3
|
||||
accel_first_sample = 0; |
||||
|
||||
} else { |
||||
perf_count(_bad_transfer_perf); |
||||
bad_data = true; |
||||
} |
||||
} |
||||
|
||||
int accel_samples = 0; |
||||
|
||||
for (int i = accel_first_sample; i < samples; i = i + 2) { |
||||
const FIFO::DATA &fifo_sample = buffer.f[i]; |
||||
int16_t accel_x = combine(fifo_sample.ACCEL_XOUT_H, fifo_sample.ACCEL_XOUT_L); |
||||
int16_t accel_y = combine(fifo_sample.ACCEL_YOUT_H, fifo_sample.ACCEL_YOUT_L); |
||||
int16_t accel_z = combine(fifo_sample.ACCEL_ZOUT_H, fifo_sample.ACCEL_ZOUT_L); |
||||
|
||||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
accel.x[accel_samples] = accel_x; |
||||
accel.y[accel_samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; |
||||
accel.z[accel_samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; |
||||
accel_samples++; |
||||
} |
||||
|
||||
accel.samples = accel_samples; |
||||
|
||||
_px4_accel.updateFIFO(accel); |
||||
|
||||
return !bad_data; |
||||
} |
||||
|
||||
void ICM20948::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples) |
||||
{ |
||||
PX4Gyroscope::FIFOSample gyro; |
||||
gyro.timestamp_sample = timestamp_sample; |
||||
gyro.samples = samples; |
||||
gyro.dt = _fifo_empty_interval_us / _fifo_gyro_samples; |
||||
|
||||
for (int i = 0; i < samples; i++) { |
||||
const FIFO::DATA &fifo_sample = buffer.f[i]; |
||||
|
||||
const int16_t gyro_x = combine(fifo_sample.GYRO_XOUT_H, fifo_sample.GYRO_XOUT_L); |
||||
const int16_t gyro_y = combine(fifo_sample.GYRO_YOUT_H, fifo_sample.GYRO_YOUT_L); |
||||
const int16_t gyro_z = combine(fifo_sample.GYRO_ZOUT_H, fifo_sample.GYRO_ZOUT_L); |
||||
|
||||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
gyro.x[i] = gyro_x; |
||||
gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; |
||||
gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; |
||||
} |
||||
|
||||
_px4_gyro.updateFIFO(gyro); |
||||
} |
||||
|
||||
void ICM20948::UpdateTemperature() |
||||
{ |
||||
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); |
||||
|
||||
// read current temperature
|
||||
uint8_t temperature_buf[3] {}; |
||||
temperature_buf[0] = static_cast<uint8_t>(Register::BANK_0::TEMP_OUT_H) | DIR_READ; |
||||
|
||||
if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) { |
||||
perf_count(_bad_transfer_perf); |
||||
return; |
||||
} |
||||
|
||||
const int16_t TEMP_OUT = combine(temperature_buf[1], temperature_buf[2]); |
||||
const float TEMP_degC = (TEMP_OUT / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET; |
||||
|
||||
if (PX4_ISFINITE(TEMP_degC)) { |
||||
_px4_accel.set_temperature(TEMP_degC); |
||||
_px4_gyro.set_temperature(TEMP_degC); |
||||
|
||||
if (_slave_ak09916_magnetometer) { |
||||
_slave_ak09916_magnetometer->set_temperature(TEMP_degC); |
||||
} |
||||
} |
||||
} |
||||
|
||||
void ICM20948::I2CSlaveRegisterStartRead(uint8_t slave_i2c_addr, uint8_t reg) |
||||
{ |
||||
I2CSlaveExternalSensorDataEnable(slave_i2c_addr, reg, 1); |
||||
} |
||||
|
||||
void ICM20948::I2CSlaveRegisterWrite(uint8_t slave_i2c_addr, uint8_t reg, uint8_t val) |
||||
{ |
||||
RegisterWrite(Register::BANK_3::I2C_SLV0_ADDR, slave_i2c_addr); |
||||
RegisterWrite(Register::BANK_3::I2C_SLV0_REG, reg); |
||||
RegisterWrite(Register::BANK_3::I2C_SLV0_DO, val); |
||||
RegisterSetBits(Register::BANK_3::I2C_SLV0_CTRL, 1); |
||||
} |
||||
|
||||
void ICM20948::I2CSlaveExternalSensorDataEnable(uint8_t slave_i2c_addr, uint8_t reg, uint8_t size) |
||||
{ |
||||
//RegisterWrite(Register::I2C_SLV0_ADDR, 0); // disable slave
|
||||
RegisterWrite(Register::BANK_3::I2C_SLV0_ADDR, slave_i2c_addr | I2C_SLV0_ADDR_BIT::I2C_SLV0_RNW); |
||||
RegisterWrite(Register::BANK_3::I2C_SLV0_REG, reg); |
||||
RegisterWrite(Register::BANK_3::I2C_SLV0_CTRL, size | I2C_SLV0_CTRL_BIT::I2C_SLV0_EN); |
||||
} |
||||
|
||||
bool ICM20948::I2CSlaveExternalSensorDataRead(uint8_t *buffer, uint8_t length) |
||||
{ |
||||
bool ret = false; |
||||
|
||||
if (buffer != nullptr && length <= 24) { |
||||
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); |
||||
|
||||
// max EXT_SENS_DATA 24 bytes
|
||||
uint8_t transfer_buffer[24 + 1] {}; |
||||
transfer_buffer[0] = static_cast<uint8_t>(Register::BANK_0::EXT_SLV_SENS_DATA_00) | DIR_READ; |
||||
|
||||
if (transfer(transfer_buffer, transfer_buffer, length + 1) == PX4_OK) { |
||||
ret = true; |
||||
} |
||||
|
||||
// copy data after cmd back to return buffer
|
||||
memcpy(buffer, &transfer_buffer[1], length); |
||||
} |
||||
|
||||
return ret; |
||||
} |
||||
@ -0,0 +1,225 @@
@@ -0,0 +1,225 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2020 PX4 Development Team. All rights reserved. |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/**
|
||||
* @file ICM20948.hpp |
||||
* |
||||
* Driver for the Invensense ICM20948 connected via SPI. |
||||
* |
||||
*/ |
||||
|
||||
#pragma once |
||||
|
||||
#include "InvenSense_ICM20948_registers.hpp" |
||||
|
||||
#include <drivers/drv_hrt.h> |
||||
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp> |
||||
#include <lib/drivers/device/spi.h> |
||||
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp> |
||||
#include <lib/ecl/geo/geo.h> |
||||
#include <lib/perf/perf_counter.h> |
||||
#include <px4_platform_common/atomic.h> |
||||
#include <px4_platform_common/i2c_spi_buses.h> |
||||
|
||||
#include "ICM20948_AK09916.hpp" |
||||
|
||||
using namespace InvenSense_ICM20948; |
||||
|
||||
class ICM20948 : public device::SPI, public I2CSPIDriver<ICM20948> |
||||
{ |
||||
public: |
||||
ICM20948(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, |
||||
spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio, bool enable_magnetometer = false); |
||||
~ICM20948() override; |
||||
|
||||
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, |
||||
int runtime_instance); |
||||
static void print_usage(); |
||||
|
||||
void RunImpl(); |
||||
|
||||
int init() override; |
||||
void print_status() override; |
||||
|
||||
private: |
||||
void exit_and_cleanup() override; |
||||
|
||||
// Sensor Configuration
|
||||
static constexpr float FIFO_SAMPLE_DT{1e6f / 9000.f}; |
||||
static constexpr uint32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer
|
||||
static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 9000 Hz gyro
|
||||
static constexpr float ACCEL_RATE{GYRO_RATE / 2.f}; // 4500 Hz accel
|
||||
|
||||
static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))}; |
||||
|
||||
// Transfer data
|
||||
struct FIFOTransferBuffer { |
||||
uint8_t cmd{static_cast<uint8_t>(Register::BANK_0::FIFO_COUNTH) | DIR_READ}; |
||||
uint8_t FIFO_COUNTH{0}; |
||||
uint8_t FIFO_COUNTL{0}; |
||||
FIFO::DATA f[FIFO_MAX_SAMPLES] {}; |
||||
}; |
||||
// ensure no struct padding
|
||||
static_assert(sizeof(FIFOTransferBuffer) == (3 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA))); |
||||
|
||||
struct register_bank0_config_t { |
||||
Register::BANK_0 reg; |
||||
uint8_t set_bits{0}; |
||||
uint8_t clear_bits{0}; |
||||
}; |
||||
|
||||
struct register_bank2_config_t { |
||||
Register::BANK_2 reg; |
||||
uint8_t set_bits{0}; |
||||
uint8_t clear_bits{0}; |
||||
}; |
||||
|
||||
struct register_bank3_config_t { |
||||
Register::BANK_3 reg; |
||||
uint8_t set_bits{0}; |
||||
uint8_t clear_bits{0}; |
||||
}; |
||||
|
||||
int probe() override; |
||||
|
||||
bool Reset(); |
||||
|
||||
bool Configure(); |
||||
void ConfigureAccel(); |
||||
void ConfigureGyro(); |
||||
void ConfigureSampleRate(int sample_rate); |
||||
|
||||
void SelectRegisterBank(enum REG_BANK_SEL_BIT bank); |
||||
void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); } |
||||
void SelectRegisterBank(Register::BANK_2 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_2); } |
||||
void SelectRegisterBank(Register::BANK_3 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_3); } |
||||
|
||||
static int DataReadyInterruptCallback(int irq, void *context, void *arg); |
||||
void DataReady(); |
||||
bool DataReadyInterruptConfigure(); |
||||
bool DataReadyInterruptDisable(); |
||||
|
||||
template <typename T> bool RegisterCheck(const T ®_cfg, bool notify = false); |
||||
template <typename T> uint8_t RegisterRead(T reg); |
||||
template <typename T> void RegisterWrite(T reg, uint8_t value); |
||||
template <typename T> void RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits); |
||||
template <typename T> void RegisterSetBits(T reg, uint8_t setbits) { RegisterSetAndClearBits(reg, setbits, 0); } |
||||
template <typename T> void RegisterClearBits(T reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); } |
||||
|
||||
uint16_t FIFOReadCount(); |
||||
bool FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples); |
||||
void FIFOReset(); |
||||
|
||||
bool ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples); |
||||
void ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples); |
||||
void UpdateTemperature(); |
||||
|
||||
const spi_drdy_gpio_t _drdy_gpio; |
||||
|
||||
// I2C AUX interface (slave 1 - 4)
|
||||
friend class AKM_AK09916::ICM20948_AK09916; |
||||
|
||||
void I2CSlaveRegisterStartRead(uint8_t slave_i2c_addr, uint8_t reg); |
||||
void I2CSlaveRegisterWrite(uint8_t slave_i2c_addr, uint8_t reg, uint8_t val); |
||||
void I2CSlaveExternalSensorDataEnable(uint8_t slave_i2c_addr, uint8_t reg, uint8_t size); |
||||
bool I2CSlaveExternalSensorDataRead(uint8_t *buffer, uint8_t length); |
||||
|
||||
AKM_AK09916::ICM20948_AK09916 *_slave_ak09916_magnetometer{nullptr}; |
||||
|
||||
PX4Accelerometer _px4_accel; |
||||
PX4Gyroscope _px4_gyro; |
||||
|
||||
perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": transfer")}; |
||||
perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")}; |
||||
perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")}; |
||||
perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO empty")}; |
||||
perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO overflow")}; |
||||
perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO reset")}; |
||||
perf_counter_t _drdy_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": DRDY interval")}; |
||||
|
||||
hrt_abstime _reset_timestamp{0}; |
||||
hrt_abstime _last_config_check_timestamp{0}; |
||||
hrt_abstime _fifo_watermark_interrupt_timestamp{0}; |
||||
hrt_abstime _temperature_update_timestamp{0}; |
||||
|
||||
enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0}; |
||||
|
||||
px4::atomic<uint8_t> _data_ready_count{0}; |
||||
px4::atomic<uint8_t> _fifo_read_samples{0}; |
||||
bool _data_ready_interrupt_enabled{false}; |
||||
bool _force_fifo_count_check{true}; |
||||
|
||||
enum class STATE : uint8_t { |
||||
RESET, |
||||
WAIT_FOR_RESET, |
||||
CONFIGURE, |
||||
FIFO_READ, |
||||
}; |
||||
|
||||
STATE _state{STATE::RESET}; |
||||
|
||||
uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
|
||||
uint8_t _fifo_gyro_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; |
||||
uint8_t _fifo_accel_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))}; |
||||
|
||||
uint8_t _checked_register_bank0{0}; |
||||
static constexpr uint8_t size_register_bank0_cfg{6}; |
||||
register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] { |
||||
// Register | Set bits, Clear bits
|
||||
{ Register::BANK_0::USER_CTRL, USER_CTRL_BIT::FIFO_EN | USER_CTRL_BIT::I2C_MST_EN | USER_CTRL_BIT::I2C_IF_DIS, USER_CTRL_BIT::DMP_EN }, |
||||
{ Register::BANK_0::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0, PWR_MGMT_1_BIT::DEVICE_RESET | PWR_MGMT_1_BIT::SLEEP }, |
||||
{ Register::BANK_0::INT_PIN_CFG, INT_PIN_CFG_BIT::INT1_ACTL, 0 }, |
||||
{ Register::BANK_0::INT_ENABLE_1, INT_ENABLE_1_BIT::RAW_DATA_0_RDY_EN, 0 }, |
||||
{ Register::BANK_0::FIFO_EN_2, FIFO_EN_2_BIT::ACCEL_FIFO_EN | FIFO_EN_2_BIT::GYRO_Z_FIFO_EN | FIFO_EN_2_BIT::GYRO_Y_FIFO_EN | FIFO_EN_2_BIT::GYRO_X_FIFO_EN, FIFO_EN_2_BIT::TEMP_FIFO_EN }, |
||||
{ Register::BANK_0::FIFO_MODE, FIFO_MODE_BIT::Snapshot, 0 }, |
||||
// { Register::BANK_0::FIFO_CFG, FIFO_CFG_BIT::FIFO_CFG, 0 }, // TODO: enable data ready interrupt
|
||||
}; |
||||
|
||||
uint8_t _checked_register_bank2{0}; |
||||
static constexpr uint8_t size_register_bank2_cfg{2}; |
||||
register_bank2_config_t _register_bank2_cfg[size_register_bank2_cfg] { |
||||
// Register | Set bits, Clear bits
|
||||
{ Register::BANK_2::GYRO_CONFIG_1, GYRO_CONFIG_1_BIT::GYRO_FS_SEL_2000_DPS, GYRO_CONFIG_1_BIT::GYRO_FCHOICE }, |
||||
{ Register::BANK_2::ACCEL_CONFIG, ACCEL_CONFIG_BIT::ACCEL_FS_SEL_16G, ACCEL_CONFIG_BIT::ACCEL_FCHOICE }, |
||||
}; |
||||
|
||||
uint8_t _checked_register_bank3{0}; |
||||
static constexpr uint8_t size_register_bank3_cfg{4}; |
||||
register_bank3_config_t _register_bank3_cfg[size_register_bank3_cfg] { |
||||
// Register | Set bits, Clear bits
|
||||
{ Register::BANK_3::I2C_MST_ODR_CONFIG, 0, 0 }, |
||||
{ Register::BANK_3::I2C_MST_CTRL, 0, 0 }, |
||||
{ Register::BANK_3::I2C_MST_DELAY_CTRL, 0, 0 }, |
||||
{ Register::BANK_3::I2C_SLV4_CTRL, 0, 0 }, |
||||
}; |
||||
}; |
||||
@ -0,0 +1,298 @@
@@ -0,0 +1,298 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2020 PX4 Development Team. All rights reserved. |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
#include "ICM20948_AK09916.hpp" |
||||
|
||||
#include "ICM20948.hpp" |
||||
|
||||
using namespace time_literals; |
||||
|
||||
namespace AKM_AK09916 |
||||
{ |
||||
|
||||
static constexpr int16_t combine(uint8_t msb, uint8_t lsb) |
||||
{ |
||||
return (msb << 8u) | lsb; |
||||
} |
||||
|
||||
ICM20948_AK09916::ICM20948_AK09916(ICM20948 &icm20948, enum Rotation rotation) : |
||||
ScheduledWorkItem("icm20948_ak09916", px4::device_bus_to_wq(icm20948.get_device_id())), |
||||
_icm20948(icm20948), |
||||
_px4_mag(icm20948.get_device_id(), ORB_PRIO_DEFAULT, rotation) |
||||
{ |
||||
_px4_mag.set_device_type(DRV_MAG_DEVTYPE_AK09916); |
||||
_px4_mag.set_external(icm20948.external()); |
||||
} |
||||
|
||||
ICM20948_AK09916::~ICM20948_AK09916() |
||||
{ |
||||
ScheduleClear(); |
||||
|
||||
perf_free(_transfer_perf); |
||||
perf_free(_bad_register_perf); |
||||
perf_free(_bad_transfer_perf); |
||||
perf_free(_duplicate_data_perf); |
||||
perf_free(_data_not_ready); |
||||
} |
||||
|
||||
bool ICM20948_AK09916::Init() |
||||
{ |
||||
return Reset(); |
||||
} |
||||
|
||||
bool ICM20948_AK09916::Reset() |
||||
{ |
||||
_state = STATE::RESET; |
||||
ScheduleClear(); |
||||
ScheduleNow(); |
||||
return true; |
||||
} |
||||
|
||||
void ICM20948_AK09916::PrintInfo() |
||||
{ |
||||
perf_print_counter(_transfer_perf); |
||||
perf_print_counter(_bad_register_perf); |
||||
perf_print_counter(_bad_transfer_perf); |
||||
perf_print_counter(_duplicate_data_perf); |
||||
perf_print_counter(_data_not_ready); |
||||
|
||||
_px4_mag.print_status(); |
||||
} |
||||
|
||||
void ICM20948_AK09916::Run() |
||||
{ |
||||
switch (_state) { |
||||
case STATE::RESET: |
||||
// CNTL3 SRST: Soft reset
|
||||
RegisterWrite(Register::CNTL3, CNTL3_BIT::SRST); |
||||
_reset_timestamp = hrt_absolute_time(); |
||||
_state = STATE::READ_WHO_AM_I; |
||||
ScheduleDelayed(100_ms); |
||||
break; |
||||
|
||||
case STATE::READ_WHO_AM_I: |
||||
_icm20948.I2CSlaveRegisterStartRead(I2C_ADDRESS_DEFAULT, (uint8_t)Register::WIA); |
||||
_state = STATE::WAIT_FOR_RESET; |
||||
ScheduleDelayed(10_ms); |
||||
break; |
||||
|
||||
case STATE::WAIT_FOR_RESET: { |
||||
|
||||
uint8_t WIA = 0; |
||||
_icm20948.I2CSlaveExternalSensorDataRead(&WIA, 1); |
||||
|
||||
if (WIA == WHOAMI) { |
||||
// if reset succeeded then configure
|
||||
PX4_DEBUG("AK09916 reset successful, configuring"); |
||||
_state = STATE::CONFIGURE; |
||||
ScheduleDelayed(10_ms); |
||||
|
||||
} else { |
||||
// RESET not complete
|
||||
if (hrt_elapsed_time(&_reset_timestamp) > 100_ms) { |
||||
PX4_DEBUG("Reset failed, retrying"); |
||||
_state = STATE::RESET; |
||||
ScheduleDelayed(100_ms); |
||||
|
||||
} else { |
||||
PX4_DEBUG("Reset not complete, check again in 100 ms"); |
||||
ScheduleDelayed(100_ms); |
||||
} |
||||
} |
||||
} |
||||
|
||||
break; |
||||
|
||||
// TODO: read FUSE ROM (to get ASA corrections)
|
||||
|
||||
case STATE::CONFIGURE: |
||||
if (Configure()) { |
||||
// if configure succeeded then start reading
|
||||
PX4_DEBUG("AK09916 configure successful, reading"); |
||||
_icm20948.I2CSlaveExternalSensorDataEnable(I2C_ADDRESS_DEFAULT, (uint8_t)Register::ST1, sizeof(TransferBuffer)); |
||||
_state = STATE::READ; |
||||
ScheduleOnInterval(20_ms, 20_ms); // 50 Hz
|
||||
|
||||
} else { |
||||
PX4_DEBUG("Configure failed, retrying"); |
||||
// try again in 100 ms
|
||||
ScheduleDelayed(100_ms); |
||||
} |
||||
|
||||
break; |
||||
|
||||
case STATE::READ: { |
||||
perf_begin(_transfer_perf); |
||||
|
||||
TransferBuffer buffer{}; |
||||
const hrt_abstime timestamp_sample = hrt_absolute_time(); |
||||
bool success = _icm20948.I2CSlaveExternalSensorDataRead((uint8_t *)&buffer, sizeof(TransferBuffer)); |
||||
|
||||
perf_end(_transfer_perf); |
||||
|
||||
if (success && !(buffer.ST2 & ST2_BIT::HOFL) && (buffer.ST1 & ST1_BIT::DRDY)) { |
||||
// sensor's frame is +y forward (x), -x right, +z down
|
||||
int16_t x = combine(buffer.HYH, buffer.HYL); // +Y
|
||||
int16_t y = combine(buffer.HXH, buffer.HXL); // +X
|
||||
y = (y == INT16_MIN) ? INT16_MAX : -y; // flip y
|
||||
int16_t z = combine(buffer.HZH, buffer.HZL); |
||||
|
||||
const bool all_zero = (x == 0 && y == 0 && z == 0); |
||||
const bool new_data = (_last_measurement[0] != x || _last_measurement[1] != y || _last_measurement[2] != z); |
||||
|
||||
if (!new_data) { |
||||
perf_count(_duplicate_data_perf); |
||||
} |
||||
|
||||
if (!all_zero && new_data) { |
||||
_px4_mag.update(timestamp_sample, x, y, z); |
||||
|
||||
_last_measurement[0] = x; |
||||
_last_measurement[1] = y; |
||||
_last_measurement[2] = z; |
||||
|
||||
} else { |
||||
success = false; |
||||
} |
||||
|
||||
} else { |
||||
perf_count(_data_not_ready); |
||||
} |
||||
|
||||
if (!success) { |
||||
perf_count(_bad_transfer_perf); |
||||
} |
||||
} |
||||
|
||||
break; |
||||
} |
||||
} |
||||
|
||||
bool ICM20948_AK09916::Configure() |
||||
{ |
||||
bool success = true; |
||||
|
||||
for (const auto ® : _register_cfg) { |
||||
if (!RegisterCheck(reg)) { |
||||
success = false; |
||||
} |
||||
} |
||||
|
||||
// TODO: read ASA and set sensitivity
|
||||
|
||||
//const uint8_t ASAX = RegisterRead(Register::ASAX);
|
||||
//const uint8_t ASAY = RegisterRead(Register::ASAY);
|
||||
//const uint8_t ASAZ = RegisterRead(Register::ASAZ);
|
||||
|
||||
// float ak8963_ASA[3] {};
|
||||
|
||||
// for (int i = 0; i < 3; i++) {
|
||||
// if (0 != response[i] && 0xff != response[i]) {
|
||||
// ak8963_ASA[i] = ((float)(response[i] - 128) / 256.0f) + 1.0f;
|
||||
|
||||
// } else {
|
||||
// return false;
|
||||
// }
|
||||
// }
|
||||
|
||||
// _px4_mag.set_sensitivity(ak8963_ASA[0], ak8963_ASA[1], ak8963_ASA[2]);
|
||||
|
||||
|
||||
// in 16-bit sampling mode the mag resolution is 1.5 milli Gauss per bit */
|
||||
_px4_mag.set_scale(1.5e-3f); |
||||
|
||||
return success; |
||||
} |
||||
|
||||
bool ICM20948_AK09916::RegisterCheck(const register_config_t ®_cfg, bool notify) |
||||
{ |
||||
bool success = true; |
||||
|
||||
const uint8_t reg_value = RegisterRead(reg_cfg.reg); |
||||
|
||||
if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) { |
||||
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits); |
||||
success = false; |
||||
} |
||||
|
||||
if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) { |
||||
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits); |
||||
success = false; |
||||
} |
||||
|
||||
if (!success) { |
||||
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); |
||||
|
||||
if (notify) { |
||||
perf_count(_bad_register_perf); |
||||
_px4_mag.increase_error_count(); |
||||
} |
||||
} |
||||
|
||||
return success; |
||||
} |
||||
|
||||
uint8_t ICM20948_AK09916::RegisterRead(Register reg) |
||||
{ |
||||
// TODO: use slave 4 and check register
|
||||
_icm20948.I2CSlaveRegisterStartRead(I2C_ADDRESS_DEFAULT, static_cast<uint8_t>(reg)); |
||||
usleep(1000); |
||||
|
||||
uint8_t buffer{}; |
||||
_icm20948.I2CSlaveExternalSensorDataRead(&buffer, 1); |
||||
|
||||
return buffer; |
||||
} |
||||
|
||||
void ICM20948_AK09916::RegisterWrite(Register reg, uint8_t value) |
||||
{ |
||||
return _icm20948.I2CSlaveRegisterWrite(I2C_ADDRESS_DEFAULT, static_cast<uint8_t>(reg), value); |
||||
} |
||||
|
||||
void ICM20948_AK09916::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits) |
||||
{ |
||||
const uint8_t orig_val = RegisterRead(reg); |
||||
uint8_t val = orig_val; |
||||
|
||||
if (setbits) { |
||||
val |= setbits; |
||||
} |
||||
|
||||
if (clearbits) { |
||||
val &= ~clearbits; |
||||
} |
||||
|
||||
RegisterWrite(reg, val); |
||||
} |
||||
|
||||
} // namespace AKM_AK09916
|
||||
@ -0,0 +1,133 @@
@@ -0,0 +1,133 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2020 PX4 Development Team. All rights reserved. |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/**
|
||||
* @file ICM20948_AK09916.hpp |
||||
* |
||||
* Driver for the AKM AK09916 connected via I2C. |
||||
* |
||||
*/ |
||||
|
||||
#pragma once |
||||
|
||||
#include "AKM_AK09916_registers.hpp" |
||||
|
||||
#include <drivers/drv_hrt.h> |
||||
#include <lib/drivers/device/i2c.h> |
||||
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp> |
||||
#include <lib/perf/perf_counter.h> |
||||
#include <px4_platform_common/atomic.h> |
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp> |
||||
|
||||
class ICM20948; |
||||
|
||||
namespace AKM_AK09916 |
||||
{ |
||||
|
||||
class ICM20948_AK09916 : public px4::ScheduledWorkItem |
||||
{ |
||||
public: |
||||
ICM20948_AK09916(ICM20948 &icm20948, enum Rotation rotation = ROTATION_NONE); |
||||
~ICM20948_AK09916() override; |
||||
|
||||
bool Init(); |
||||
bool Reset(); |
||||
void PrintInfo(); |
||||
|
||||
void set_temperature(float temperature) { _px4_mag.set_temperature(temperature); } |
||||
|
||||
private: |
||||
|
||||
struct TransferBuffer { |
||||
uint8_t ST1; |
||||
uint8_t HXL; |
||||
uint8_t HXH; |
||||
uint8_t HYL; |
||||
uint8_t HYH; |
||||
uint8_t HZL; |
||||
uint8_t HZH; |
||||
uint8_t TMPS; |
||||
uint8_t ST2; |
||||
}; |
||||
|
||||
struct register_config_t { |
||||
AKM_AK09916::Register reg; |
||||
uint8_t set_bits{0}; |
||||
uint8_t clear_bits{0}; |
||||
}; |
||||
|
||||
int probe(); |
||||
|
||||
void Run() override; |
||||
|
||||
bool Configure(); |
||||
|
||||
bool RegisterCheck(const register_config_t ®_cfg, bool notify = false); |
||||
|
||||
uint8_t RegisterRead(AKM_AK09916::Register reg); |
||||
void RegisterWrite(AKM_AK09916::Register reg, uint8_t value); |
||||
void RegisterSetAndClearBits(AKM_AK09916::Register reg, uint8_t setbits, uint8_t clearbits); |
||||
|
||||
ICM20948 &_icm20948; |
||||
|
||||
PX4Magnetometer _px4_mag; |
||||
|
||||
perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME"_ak09916: transfer")}; |
||||
perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_ak09916: bad register")}; |
||||
perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_ak09916: bad transfer")}; |
||||
perf_counter_t _duplicate_data_perf{perf_alloc(PC_COUNT, MODULE_NAME"_ak09916: duplicate data")}; |
||||
perf_counter_t _data_not_ready{perf_alloc(PC_COUNT, MODULE_NAME"_ak09916: data not ready")}; |
||||
|
||||
hrt_abstime _reset_timestamp{0}; |
||||
hrt_abstime _last_config_check_timestamp{0}; |
||||
|
||||
int16_t _last_measurement[3] {}; |
||||
|
||||
uint8_t _checked_register{0}; |
||||
|
||||
enum class STATE : uint8_t { |
||||
RESET, |
||||
READ_WHO_AM_I, |
||||
WAIT_FOR_RESET, |
||||
CONFIGURE, |
||||
READ, |
||||
} _state{STATE::RESET};; |
||||
|
||||
static constexpr uint8_t size_register_cfg{1}; |
||||
register_config_t _register_cfg[size_register_cfg] { |
||||
// Register | Set bits, Clear bits
|
||||
{ AKM_AK09916::Register::CNTL2, AKM_AK09916::CNTL2_BIT::MODE3, (uint8_t)~AKM_AK09916::CNTL2_BIT::MODE3 }, |
||||
}; |
||||
}; |
||||
|
||||
} // namespace AKM_AK09916
|
||||
@ -0,0 +1,272 @@
@@ -0,0 +1,272 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2020 PX4 Development Team. All rights reserved. |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/**
|
||||
* @file InvenSense_ICM20948_registers.hpp |
||||
* |
||||
* Invensense ICM-20948 registers. |
||||
* |
||||
*/ |
||||
|
||||
#pragma once |
||||
|
||||
#include <cstdint> |
||||
|
||||
namespace InvenSense_ICM20948 |
||||
{ |
||||
// TODO: move to a central header
|
||||
static constexpr uint8_t Bit0 = (1 << 0); |
||||
static constexpr uint8_t Bit1 = (1 << 1); |
||||
static constexpr uint8_t Bit2 = (1 << 2); |
||||
static constexpr uint8_t Bit3 = (1 << 3); |
||||
static constexpr uint8_t Bit4 = (1 << 4); |
||||
static constexpr uint8_t Bit5 = (1 << 5); |
||||
static constexpr uint8_t Bit6 = (1 << 6); |
||||
static constexpr uint8_t Bit7 = (1 << 7); |
||||
|
||||
static constexpr uint32_t SPI_SPEED = 7 * 1000 * 1000; // 7 MHz SPI
|
||||
static constexpr uint8_t DIR_READ = 0x80; |
||||
|
||||
static constexpr uint8_t WHOAMI = 0xEA; |
||||
|
||||
static constexpr float TEMPERATURE_SENSITIVITY = 333.87f; // LSB/C
|
||||
static constexpr float TEMPERATURE_OFFSET = 21.f; // C
|
||||
|
||||
namespace Register |
||||
{ |
||||
|
||||
enum class BANK_0 : uint8_t { |
||||
WHO_AM_I = 0x00, |
||||
|
||||
USER_CTRL = 0x03, |
||||
|
||||
PWR_MGMT_1 = 0x06, |
||||
|
||||
INT_PIN_CFG = 0x0F, |
||||
|
||||
INT_ENABLE_1 = 0x11, |
||||
|
||||
I2C_MST_STATUS = 0x17, |
||||
|
||||
TEMP_OUT_H = 0x39, |
||||
TEMP_OUT_L = 0x3A, |
||||
EXT_SLV_SENS_DATA_00 = 0x3B, |
||||
// [EXT_SLV_SENS_DATA_01, EXT_SLV_SENS_DATA_22]
|
||||
EXT_SLV_SENS_DATA_23 = 0x52, |
||||
|
||||
FIFO_EN_2 = 0x67, |
||||
FIFO_RST = 0x68, |
||||
FIFO_MODE = 0x69, |
||||
FIFO_COUNTH = 0x70, |
||||
FIFO_COUNTL = 0x71, |
||||
FIFO_R_W = 0x72, |
||||
|
||||
FIFO_CFG = 0x76, |
||||
|
||||
REG_BANK_SEL = 0x7F, |
||||
}; |
||||
|
||||
enum class BANK_2 : uint8_t { |
||||
GYRO_CONFIG_1 = 0x01, |
||||
|
||||
ACCEL_CONFIG = 0x14, |
||||
|
||||
REG_BANK_SEL = 0x7F, |
||||
}; |
||||
|
||||
enum class BANK_3 : uint8_t { |
||||
I2C_MST_ODR_CONFIG = 0x00, |
||||
I2C_MST_CTRL = 0x01, |
||||
I2C_MST_DELAY_CTRL = 0x02, |
||||
|
||||
I2C_SLV0_ADDR = 0x03, |
||||
I2C_SLV0_REG = 0x04, |
||||
I2C_SLV0_CTRL = 0x05, |
||||
I2C_SLV0_DO = 0x06, |
||||
|
||||
I2C_SLV4_CTRL = 0x15, |
||||
|
||||
REG_BANK_SEL = 0x7F, |
||||
}; |
||||
|
||||
}; |
||||
|
||||
|
||||
//---------------- BANK0 Register bits
|
||||
// USER_CTRL
|
||||
enum USER_CTRL_BIT : uint8_t { |
||||
DMP_EN = Bit7, |
||||
FIFO_EN = Bit6, |
||||
I2C_MST_EN = Bit5, // Enable the I2C Master I/F module
|
||||
I2C_IF_DIS = Bit4, // Reset I2C Slave module and put the serial interface in SPI mode only
|
||||
DMP_RST = Bit3, // Reset DMP module. Reset is asynchronous. This bit auto clears after one clock cycle of the internal 20 MHz clock.
|
||||
SRAM_RST = Bit2, // Reset SRAM module. Reset is asynchronous. This bit auto clears after one clock cycle of the internal 20 MHz clock.
|
||||
I2C_MST_RST = Bit1, // Reset I2C Master module.
|
||||
}; |
||||
|
||||
// PWR_MGMT_1
|
||||
enum PWR_MGMT_1_BIT : uint8_t { |
||||
DEVICE_RESET = Bit7, |
||||
SLEEP = Bit6, |
||||
|
||||
CLKSEL_2 = Bit2, |
||||
CLKSEL_1 = Bit1, |
||||
CLKSEL_0 = Bit0, |
||||
}; |
||||
|
||||
// INT_PIN_CFG
|
||||
enum INT_PIN_CFG_BIT : uint8_t { |
||||
INT1_ACTL = Bit7, |
||||
}; |
||||
|
||||
// INT_ENABLE_1
|
||||
enum INT_ENABLE_1_BIT : uint8_t { |
||||
RAW_DATA_0_RDY_EN = Bit0, |
||||
}; |
||||
|
||||
// FIFO_EN_2
|
||||
enum FIFO_EN_2_BIT : uint8_t { |
||||
ACCEL_FIFO_EN = Bit4, |
||||
GYRO_Z_FIFO_EN = Bit3, |
||||
GYRO_Y_FIFO_EN = Bit2, |
||||
GYRO_X_FIFO_EN = Bit1, |
||||
TEMP_FIFO_EN = Bit0, |
||||
}; |
||||
|
||||
// FIFO_RST
|
||||
enum FIFO_RST_BIT : uint8_t { |
||||
FIFO_RESET = Bit4 | Bit3 | Bit2 | Bit1 | Bit0, |
||||
}; |
||||
|
||||
// FIFO_MODE
|
||||
enum FIFO_MODE_BIT : uint8_t { |
||||
Snapshot = Bit0, |
||||
}; |
||||
|
||||
// FIFO_CFG
|
||||
enum FIFO_CFG_BIT : uint8_t { |
||||
FIFO_CFG = Bit0, |
||||
}; |
||||
|
||||
// REG_BANK_SEL
|
||||
enum REG_BANK_SEL_BIT : uint8_t { |
||||
USER_BANK_0 = 0, // 0: Select USER BANK 0.
|
||||
USER_BANK_1 = Bit4, // 1: Select USER BANK 1.
|
||||
USER_BANK_2 = Bit5, // 2: Select USER BANK 2.
|
||||
USER_BANK_3 = Bit5 | Bit4, // 3: Select USER BANK 3.
|
||||
}; |
||||
|
||||
//---------------- BANK2 Register bits
|
||||
// GYRO_CONFIG_1
|
||||
enum GYRO_CONFIG_1_BIT : uint8_t { |
||||
// GYRO_FS_SEL[1:0]
|
||||
GYRO_FS_SEL_250_DPS = 0, // 0b00 = ±250 dps
|
||||
GYRO_FS_SEL_500_DPS = Bit1, // 0b01 = ±500 dps
|
||||
GYRO_FS_SEL_1000_DPS = Bit2, // 0b10 = ±1000 dps
|
||||
GYRO_FS_SEL_2000_DPS = Bit2 | Bit1, // 0b11 = ±2000 dps
|
||||
|
||||
GYRO_FCHOICE = Bit0, // 0 – Bypass gyro DLPF
|
||||
}; |
||||
|
||||
// ACCEL_CONFIG
|
||||
enum ACCEL_CONFIG_BIT : uint8_t { |
||||
// ACCEL_FS_SEL[1:0]
|
||||
ACCEL_FS_SEL_2G = 0, // 0b00: ±2g
|
||||
ACCEL_FS_SEL_4G = Bit1, // 0b01: ±4g
|
||||
ACCEL_FS_SEL_8G = Bit2, // 0b10: ±8g
|
||||
ACCEL_FS_SEL_16G = Bit2 | Bit1, // 0b11: ±16g
|
||||
|
||||
ACCEL_FCHOICE = Bit0, // 0: Bypass accel DLPF
|
||||
}; |
||||
|
||||
|
||||
//---------------- BANK3 Register bits
|
||||
|
||||
// I2C_MST_CTRL
|
||||
enum I2C_MST_CTRL_BIT : uint8_t { |
||||
I2C_MST_P_NSR = Bit4, // I2C Master’s transition from one slave read to the next slave read
|
||||
|
||||
// I2C_MST_CLK [3:0]
|
||||
I2C_MST_CLK_400_kHz = 7, // To achieve a targeted clock frequency of 400 kHz, MAX, it is recommended to set I2C_MST_CLK = 7 (345.6 kHz / 46.67% duty cycle)
|
||||
}; |
||||
|
||||
// I2C_MST_DELAY_CTRL
|
||||
enum I2C_MST_DELAY_CTRL_BIT : uint8_t { |
||||
I2C_SLVX_DLY_EN = Bit4 | Bit3 | Bit2 | Bit1 | Bit0, // limit all slave access (1+I2C_MST_DLY)
|
||||
}; |
||||
|
||||
// I2C_SLV0_ADDR
|
||||
enum I2C_SLV0_ADDR_BIT : uint8_t { |
||||
I2C_SLV0_RNW = Bit7, // 1 – Transfer is a read
|
||||
|
||||
// I2C_ID_0[6:0]
|
||||
I2C_ID_0 = Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1 | Bit0, // Physical address of I2C slave 0
|
||||
}; |
||||
|
||||
// I2C_SLV0_CTRL
|
||||
enum I2C_SLV0_CTRL_BIT : uint8_t { |
||||
I2C_SLV0_EN = Bit7, // Enable reading data from this slave
|
||||
I2C_SLV0_BYTE_SW = Bit6, // Swap bytes when reading both the low and high byte of a word
|
||||
I2C_SLV0_REG_DIS = Bit5, // transaction does not write a register value (only read data)
|
||||
|
||||
I2C_SLV0_LENG = Bit3 | Bit2 | Bit1 | Bit0, // Number of bytes to be read from I2C slave 0
|
||||
}; |
||||
|
||||
// I2C_SLV4_CTRL
|
||||
enum I2C_SLV4_CTRL_BIT : uint8_t { |
||||
// I2C_MST_DLY[4:0]
|
||||
I2C_MST_DLY = Bit4 | Bit3 | Bit2 | Bit1 | Bit0, |
||||
}; |
||||
|
||||
|
||||
namespace FIFO |
||||
{ |
||||
static constexpr size_t SIZE = 512; |
||||
|
||||
// FIFO_DATA layout when FIFO_EN has ACCEL_FIFO_EN and GYRO_{Z, Y, X}_FIFO_EN set
|
||||
struct DATA { |
||||
uint8_t ACCEL_XOUT_H; |
||||
uint8_t ACCEL_XOUT_L; |
||||
uint8_t ACCEL_YOUT_H; |
||||
uint8_t ACCEL_YOUT_L; |
||||
uint8_t ACCEL_ZOUT_H; |
||||
uint8_t ACCEL_ZOUT_L; |
||||
uint8_t GYRO_XOUT_H; |
||||
uint8_t GYRO_XOUT_L; |
||||
uint8_t GYRO_YOUT_H; |
||||
uint8_t GYRO_YOUT_L; |
||||
uint8_t GYRO_ZOUT_H; |
||||
uint8_t GYRO_ZOUT_L; |
||||
}; |
||||
} |
||||
} // namespace InvenSense_ICM20948
|
||||
@ -0,0 +1,112 @@
@@ -0,0 +1,112 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2020 PX4 Development Team. All rights reserved. |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
#include "ICM20948.hpp" |
||||
|
||||
#include <px4_platform_common/getopt.h> |
||||
#include <px4_platform_common/module.h> |
||||
|
||||
void ICM20948::print_usage() |
||||
{ |
||||
PRINT_MODULE_USAGE_NAME("icm20948", "driver"); |
||||
PRINT_MODULE_USAGE_SUBCATEGORY("imu"); |
||||
PRINT_MODULE_USAGE_COMMAND("start"); |
||||
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true); |
||||
PRINT_MODULE_USAGE_PARAM_FLAG('M', "Enable Magnetometer (AK8963)", true); |
||||
PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); |
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); |
||||
} |
||||
|
||||
I2CSPIDriverBase *ICM20948::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, |
||||
int runtime_instance) |
||||
{ |
||||
bool mag = (cli.custom1 == 1); |
||||
ICM20948 *instance = new ICM20948(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, |
||||
cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO(), mag); |
||||
|
||||
if (!instance) { |
||||
PX4_ERR("alloc failed"); |
||||
return nullptr; |
||||
} |
||||
|
||||
if (OK != instance->init()) { |
||||
delete instance; |
||||
return nullptr; |
||||
} |
||||
|
||||
return instance; |
||||
} |
||||
|
||||
extern "C" int icm20948_main(int argc, char *argv[]) |
||||
{ |
||||
int ch; |
||||
using ThisDriver = ICM20948; |
||||
BusCLIArguments cli{false, true}; |
||||
cli.default_spi_frequency = SPI_SPEED; |
||||
|
||||
while ((ch = cli.getopt(argc, argv, "MR:")) != EOF) { |
||||
switch (ch) { |
||||
case 'M': |
||||
cli.custom1 = 1; |
||||
break; |
||||
|
||||
case 'R': |
||||
cli.rotation = (enum Rotation)atoi(cli.optarg()); |
||||
break; |
||||
} |
||||
} |
||||
|
||||
const char *verb = cli.optarg(); |
||||
|
||||
if (!verb) { |
||||
ThisDriver::print_usage(); |
||||
return -1; |
||||
} |
||||
|
||||
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_ICM20948); |
||||
|
||||
if (!strcmp(verb, "start")) { |
||||
return ThisDriver::module_start(cli, iterator); |
||||
} |
||||
|
||||
if (!strcmp(verb, "stop")) { |
||||
return ThisDriver::module_stop(iterator); |
||||
} |
||||
|
||||
if (!strcmp(verb, "status")) { |
||||
return ThisDriver::module_status(iterator); |
||||
} |
||||
|
||||
ThisDriver::print_usage(); |
||||
return -1; |
||||
} |
||||
Loading…
Reference in new issue