Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

new InvenSense icm20948 driver on SPI with AK09916 mag #14567

Merged
merged 1 commit into from
May 22, 2020
Merged
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion boards/mro/ctrl-zero-f7/default.cmake
Original file line number Diff line number Diff line change
@@ -31,7 +31,7 @@ px4_add_board(
#imu # all available imu drivers
imu/bmi088
imu/invensense/icm20602
imu/icm20948
imu/invensense/icm20948
irlock
lights/blinkm
lights/rgbled
4 changes: 2 additions & 2 deletions boards/mro/ctrl-zero-f7/init/rc.board_sensors
Original file line number Diff line number Diff line change
@@ -12,8 +12,8 @@ icm20602 -s -R 4 start
bmi088 -A -R 10 -s start
bmi088 -G -R 10 -s start

# Internal ICM-20948
icm20948 -s -R 10 start
# Internal ICM-20948 (with magnetometer)
icm20948 -s -R 2 -M start

# Interal DPS310 (barometer)
dps310 -s start
2 changes: 2 additions & 0 deletions boards/mro/ctrl-zero-f7/src/board_config.h
Original file line number Diff line number Diff line change
@@ -118,6 +118,7 @@
#define BOARD_NUMBER_BRICKS 1

#define GPIO_VDD_3V3_SPEKTRUM_POWER_EN /* PE4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4)
#define GPIO_VDD_3V3_SENSORS_EN /* PE3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN3)


/* Define True logic Power Control in arch agnostic form */
@@ -200,6 +201,7 @@
GPIO_CAN1_SILENT_S0, \
GPIO_nPOWER_IN_A, \
GPIO_VDD_3V3_SPEKTRUM_POWER_EN, \
GPIO_VDD_3V3_SENSORS_EN, \
GPIO_TONE_ALARM_IDLE, \
GPIO_SAFETY_SWITCH_IN, \
}
5 changes: 4 additions & 1 deletion boards/mro/ctrl-zero-f7/src/init.c
Original file line number Diff line number Diff line change
@@ -163,7 +163,7 @@ stm32_boardinitialize(void)
px4_gpio_init(gpio, arraySize(gpio));

/* configure SPI interfaces */
stm32_spiinitialize();
px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0);

/* configure USB interfaces */
stm32_usbinitialize();
@@ -198,11 +198,14 @@ stm32_boardinitialize(void)
__EXPORT int board_app_initialize(uintptr_t arg)
{
/* Power on Interfaces */
px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1);
board_control_spi_sensors_power(true, 0xffff);
VDD_3V3_SPEKTRUM_POWER_EN(true);

px4_platform_init();

stm32_spiinitialize();

/* configure the DMA allocator */
if (board_dma_alloc_init() < 0) {
syslog(LOG_ERR, "[boot] DMA alloc FAILED\n");
4 changes: 2 additions & 2 deletions boards/mro/ctrl-zero-f7/src/spi.cpp
Original file line number Diff line number Diff line change
@@ -39,15 +39,15 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
initSPIBus(SPI::Bus::SPI1, {
initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}),
initSPIDevice(DRV_IMU_DEVTYPE_ICM20948, SPI::CS{GPIO::PortE, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin12}),
}, {GPIO::PortE, GPIO::Pin3}),
}),
initSPIBus(SPI::Bus::SPI2, {
initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10}),
initSPIDevice(DRV_BARO_DEVTYPE_DPS310, SPI::CS{GPIO::PortD, GPIO::Pin7}),
}),
initSPIBus(SPI::Bus::SPI5, {
initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin10}, SPI::DRDY{GPIO::PortF, GPIO::Pin3}),
initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin6}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}),
}, {GPIO::PortE, GPIO::Pin3}),
}),
};

static constexpr bool unused = validateSPIConfig(px4_spi_buses);
3 changes: 2 additions & 1 deletion src/drivers/drv_sensor.h
Original file line number Diff line number Diff line change
@@ -61,7 +61,7 @@
#define DRV_MAG_DEVTYPE_RM3100 0x07
#define DRV_MAG_DEVTYPE_QMC5883 0x08
#define DRV_MAG_DEVTYPE_AK09916 0x09
#define DRV_IMU_DEVTYPE_ICM20948 0x0A

#define DRV_MAG_DEVTYPE_IST8308 0x0B
#define DRV_MAG_DEVTYPE_LIS2MDL 0x0C

@@ -79,6 +79,7 @@
#define DRV_IMU_DEVTYPE_ICM20649 0x25
#define DRV_IMU_DEVTYPE_ICM42688P 0x26
#define DRV_IMU_DEVTYPE_ICM40609D 0x27
#define DRV_IMU_DEVTYPE_ICM20948 0x28
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Note, I wouldn't typically do this, but the IMU portion of the old icm20948 never worked properly in the first place.


#define DRV_RNG_DEVTYPE_MB12XX 0x31
#define DRV_RNG_DEVTYPE_LL40LS 0x32
105 changes: 105 additions & 0 deletions src/drivers/imu/invensense/icm20948/AKM_AK09916_registers.hpp
Original file line number Diff line number Diff line change
@@ -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
52 changes: 52 additions & 0 deletions src/drivers/imu/invensense/icm20948/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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
)
755 changes: 755 additions & 0 deletions src/drivers/imu/invensense/icm20948/ICM20948.cpp

Large diffs are not rendered by default.

225 changes: 225 additions & 0 deletions src/drivers/imu/invensense/icm20948/ICM20948.hpp
Original file line number Diff line number Diff line change
@@ -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 &reg_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 &timestamp_sample, uint16_t samples);
void FIFOReset();

bool ProcessAccel(const hrt_abstime &timestamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples);
void ProcessGyro(const hrt_abstime &timestamp_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 },
};
};
298 changes: 298 additions & 0 deletions src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp
Original file line number Diff line number Diff line change
@@ -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 &reg : _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 &reg_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
133 changes: 133 additions & 0 deletions src/drivers/imu/invensense/icm20948/ICM20948_AK09916.hpp
Original file line number Diff line number Diff line change
@@ -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 &reg_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
272 changes: 272 additions & 0 deletions src/drivers/imu/invensense/icm20948/InvenSense_ICM20948_registers.hpp
Original file line number Diff line number Diff line change
@@ -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
112 changes: 112 additions & 0 deletions src/drivers/imu/invensense/icm20948/icm20948_main.cpp
Original file line number Diff line number Diff line change
@@ -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;
}