-
Notifications
You must be signed in to change notification settings - Fork 13.7k
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
+1,965
−7
Merged
Changes from all commits
Commits
File filter
Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
105 changes: 105 additions & 0 deletions
105
src/drivers/imu/invensense/icm20948/AKM_AK09916_registers.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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 | ||
) |
Large diffs are not rendered by default.
Oops, something went wrong.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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 ®_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 }, | ||
}; | ||
}; |
298 changes: 298 additions & 0 deletions
298
src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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 ® : _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 |
133 changes: 133 additions & 0 deletions
133
src/drivers/imu/invensense/icm20948/ICM20948_AK09916.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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 ®_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
272
src/drivers/imu/invensense/icm20948/InvenSense_ICM20948_registers.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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; | ||
} |
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
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.