mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-24 07:09:48 +08:00
drivers/imu/invensense: new ICM20948 driver on SPI with AK09916 mag
This commit is contained in:
Daniel Agar
GitHub
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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, \
|
||||
}
|
||||
|
||||
@@ -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");
|
||||
|
||||
@@ -39,7 +39,7 @@ 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}),
|
||||
@@ -47,7 +47,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
|
||||
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);
|
||||
|
||||
@@ -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
|
||||
|
||||
#define DRV_RNG_DEVTYPE_MB12XX 0x31
|
||||
#define DRV_RNG_DEVTYPE_LL40LS 0x32
|
||||
|
||||
@@ -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 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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
|
||||
)
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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;
|
||||
}
|
||||
Reference in New Issue
Block a user