mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-23 22:58:10 +08:00
new InvenSense ICM20649 IMU driver
This commit is contained in:
Daniel Agar
GitHub
@@ -75,7 +75,7 @@
|
||||
#define DRV_GYR_DEVTYPE_L3GD20 0x22
|
||||
#define DRV_GYR_DEVTYPE_GYROSIM 0x23
|
||||
#define DRV_IMU_DEVTYPE_MPU9250 0x24
|
||||
|
||||
#define DRV_IMU_DEVTYPE_ICM20649 0x25
|
||||
#define DRV_IMU_DEVTYPE_ICM42688P 0x26
|
||||
#define DRV_IMU_DEVTYPE_ICM40609D 0x27
|
||||
|
||||
|
||||
@@ -33,6 +33,7 @@
|
||||
|
||||
add_subdirectory(icm20602)
|
||||
add_subdirectory(icm20608g)
|
||||
add_subdirectory(icm20649)
|
||||
add_subdirectory(icm20689)
|
||||
add_subdirectory(icm40609d)
|
||||
add_subdirectory(icm42688p)
|
||||
|
||||
@@ -0,0 +1,46 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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__invensense__icm20649
|
||||
MAIN icm20649
|
||||
COMPILE_FLAGS
|
||||
SRCS
|
||||
ICM20649.cpp
|
||||
ICM20649.hpp
|
||||
icm20649_main.cpp
|
||||
InvenSense_ICM20649_registers.hpp
|
||||
DEPENDS
|
||||
px4_work_queue
|
||||
drivers_accelerometer
|
||||
drivers_gyroscope
|
||||
)
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,196 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 ICM20649.hpp
|
||||
*
|
||||
* Driver for the Invensense ICM20649 connected via SPI.
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "InvenSense_ICM20649_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>
|
||||
|
||||
using namespace InvenSense_ICM20649;
|
||||
|
||||
class ICM20649 : public device::SPI, public I2CSPIDriver<ICM20649>
|
||||
{
|
||||
public:
|
||||
ICM20649(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);
|
||||
~ICM20649() 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};
|
||||
};
|
||||
|
||||
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); }
|
||||
|
||||
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;
|
||||
|
||||
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{1000}; // default 1000 us / 1000 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_IF_DIS, USER_CTRL_BIT::I2C_MST_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_4000_DPS, GYRO_CONFIG_1_BIT::GYRO_FCHOICE },
|
||||
{ Register::BANK_2::ACCEL_CONFIG, ACCEL_CONFIG_BIT::ACCEL_FS_SEL_30G, ACCEL_CONFIG_BIT::ACCEL_FCHOICE },
|
||||
};
|
||||
};
|
||||
@@ -0,0 +1,208 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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_ICM20649_registers.hpp
|
||||
*
|
||||
* Invensense ICM-20649 registers.
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
// 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);
|
||||
|
||||
namespace InvenSense_ICM20649
|
||||
{
|
||||
static constexpr uint32_t SPI_SPEED = 7 * 1000 * 1000; // 7 MHz SPI
|
||||
static constexpr uint8_t DIR_READ = 0x80;
|
||||
|
||||
static constexpr uint8_t WHOAMI = 0xE1;
|
||||
|
||||
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,
|
||||
|
||||
TEMP_OUT_H = 0x39,
|
||||
TEMP_OUT_L = 0x3A,
|
||||
|
||||
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,
|
||||
};
|
||||
|
||||
};
|
||||
|
||||
|
||||
//---------------- BANK0 Register bits
|
||||
// USER_CTRL
|
||||
enum USER_CTRL_BIT : uint8_t {
|
||||
FIFO_EN = Bit6,
|
||||
I2C_MST_EN = Bit5,
|
||||
I2C_IF_DIS = Bit4, // Reset I2C Slave module and put the serial interface in SPI mode only
|
||||
};
|
||||
|
||||
// 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_500_DPS = 0, // 0b00 = ±500 dps
|
||||
GYRO_FS_SEL_1000_DPS = Bit1, // 0b01 = ±1000 dps
|
||||
GYRO_FS_SEL_2000_DPS = Bit2, // 0b10 = ±2000 dps
|
||||
GYRO_FS_SEL_4000_DPS = Bit2 | Bit1, // 0b11 = ±4000 dps
|
||||
|
||||
GYRO_FCHOICE = Bit0, // 0 – Bypass gyro DLPF
|
||||
};
|
||||
|
||||
// ACCEL_CONFIG
|
||||
enum ACCEL_CONFIG_BIT : uint8_t {
|
||||
// ACCEL_FS_SEL[1:0]
|
||||
ACCEL_FS_SEL_4G = 0, // 0b00: ±4g
|
||||
ACCEL_FS_SEL_8G = Bit1, // 0b01: ±8g
|
||||
ACCEL_FS_SEL_16G = Bit2, // 0b10: ±16g
|
||||
ACCEL_FS_SEL_30G = Bit2 | Bit1, // 0b11: ±30g
|
||||
|
||||
ACCEL_FCHOICE = Bit0, // 0: Bypass accel DLPF
|
||||
};
|
||||
|
||||
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_ICM20649
|
||||
@@ -0,0 +1,106 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 "ICM20649.hpp"
|
||||
|
||||
#include <px4_platform_common/getopt.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
|
||||
void ICM20649::print_usage()
|
||||
{
|
||||
PRINT_MODULE_USAGE_NAME("icm20649", "driver");
|
||||
PRINT_MODULE_USAGE_SUBCATEGORY("imu");
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
|
||||
PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
}
|
||||
|
||||
I2CSPIDriverBase *ICM20649::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
|
||||
int runtime_instance)
|
||||
{
|
||||
ICM20649 *instance = new ICM20649(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation,
|
||||
cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO());
|
||||
|
||||
if (!instance) {
|
||||
PX4_ERR("alloc failed");
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
if (OK != instance->init()) {
|
||||
delete instance;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
return instance;
|
||||
}
|
||||
|
||||
extern "C" int icm20649_main(int argc, char *argv[])
|
||||
{
|
||||
int ch;
|
||||
using ThisDriver = ICM20649;
|
||||
BusCLIArguments cli{false, true};
|
||||
cli.default_spi_frequency = SPI_SPEED;
|
||||
|
||||
while ((ch = cli.getopt(argc, argv, "R:")) != EOF) {
|
||||
switch (ch) {
|
||||
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_ICM20649);
|
||||
|
||||
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;
|
||||
}
|
||||
Reference in New Issue
Block a user