| // Copyright 2023 The Pigweed Authors |
| // |
| // Licensed under the Apache License, Version 2.0 (the "License"); you may not |
| // use this file except in compliance with the License. You may obtain a copy of |
| // the License at |
| // |
| // https://www.apache.org/licenses/LICENSE-2.0 |
| // |
| // Unless required by applicable law or agreed to in writing, software |
| // distributed under the License is distributed on an "AS IS" BASIS, WITHOUT |
| // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the |
| // License for the specific language governing permissions and limitations under |
| // the License. |
| |
| #include "icm42670p/device.h" |
| |
| #include <chrono> |
| #include <cstddef> |
| #include <cstdint> |
| |
| #define PW_LOG_MODULE_NAME "icm42670p" |
| #define PW_LOG_LEVEL PW_LOG_LEVEL_DEBUG |
| |
| #include "pw_bytes/bit.h" |
| #include "pw_i2c/address.h" |
| #include "pw_i2c/register_device.h" |
| #include "pw_log/log.h" |
| #include "pw_result/result.h" |
| #include "pw_status/status.h" |
| |
| using ::pw::Status; |
| using namespace std::chrono_literals; |
| |
| namespace kudzu::icm42670p { |
| |
| enum Icm42670pRegister : uint8_t { |
| kWhoAmI = 0x75, |
| kPwrMgmt0 = 0x1f, |
| kGyroConfig0 = 0x20, |
| kAccelConfig0 = 0x21, |
| kAccelDataX1 = 0x0b, |
| kGyroDataX1 = 0x11, |
| }; |
| |
| constexpr int16_t GYRO_UI_FS_SEL = 1000; |
| constexpr float ACCEL_UI_FS_SEL = 8.0f; |
| |
| namespace { |
| |
| constexpr pw::i2c::Address kAddress = pw::i2c::Address::SevenBit<0x68>(); |
| |
| void ImuReadReg(pw::i2c::RegisterDevice& device, |
| uint8_t addr, |
| const char* name) { |
| auto data = |
| device.ReadRegister8(addr, pw::chrono::SystemClock::for_at_least(10ms)); |
| |
| if (data.ok()) { |
| PW_LOG_INFO("%s: %02x", name, *data); |
| } else { |
| PW_LOG_INFO("failed to read %s", name); |
| } |
| } |
| |
| void ImuReadReg16(pw::i2c::RegisterDevice& device, |
| uint8_t addr, |
| const char* name) { |
| auto data = |
| device.ReadRegister16(addr, pw::chrono::SystemClock::for_at_least(10ms)); |
| |
| if (data.ok()) { |
| PW_LOG_INFO("%s: %04x", name, *data); |
| } else { |
| PW_LOG_INFO("failed to read %s", name); |
| } |
| } |
| |
| pw::Result<kudzu::imu::ImuSample> ReadData(pw::i2c::RegisterDevice& device) { |
| // TODO: asadmemon - Check the status register before reading the values. |
| |
| uint8_t data[12]; |
| device.ReadRegisters8(Icm42670pRegister::kAccelDataX1, |
| pw::span(data, 12), |
| pw::chrono::SystemClock::for_at_least(10ms)); |
| |
| uint16_t accel_x = |
| pw::bytes::ReadInOrder<uint16_t>(pw::endian::big, &data[0]); |
| uint16_t accel_y = |
| pw::bytes::ReadInOrder<uint16_t>(pw::endian::big, &data[2]); |
| uint16_t accel_z = |
| pw::bytes::ReadInOrder<uint16_t>(pw::endian::big, &data[4]); |
| |
| uint16_t gyro_x = pw::bytes::ReadInOrder<uint16_t>(pw::endian::big, &data[6]); |
| uint16_t gyro_y = pw::bytes::ReadInOrder<uint16_t>(pw::endian::big, &data[8]); |
| uint16_t gyro_z = |
| pw::bytes::ReadInOrder<uint16_t>(pw::endian::big, &data[10]); |
| |
| float f_accel_x = |
| static_cast<float>((int16_t)accel_x) * ACCEL_UI_FS_SEL / INT16_MAX; |
| float f_accel_y = |
| static_cast<float>((int16_t)accel_y) * ACCEL_UI_FS_SEL / INT16_MAX; |
| float f_accel_z = |
| static_cast<float>((int16_t)accel_z) * ACCEL_UI_FS_SEL / INT16_MAX; |
| |
| int16_t f_gyro_x = |
| static_cast<int16_t>((int16_t)gyro_x) * GYRO_UI_FS_SEL / INT16_MAX; |
| int16_t f_gyro_y = |
| static_cast<int16_t>((int16_t)gyro_y) * GYRO_UI_FS_SEL / INT16_MAX; |
| int16_t f_gyro_z = |
| static_cast<int16_t>((int16_t)gyro_z) * GYRO_UI_FS_SEL / INT16_MAX; |
| |
| kudzu::imu::ImuSample sample = { |
| {f_accel_x, f_accel_y, f_accel_z}, |
| {f_gyro_x, f_gyro_y, f_gyro_z}, |
| }; |
| return sample; |
| } |
| |
| } // namespace |
| |
| Device::Device(pw::i2c::Initiator& initiator) |
| : initiator_(initiator), |
| device_(initiator, |
| kAddress, |
| pw::endian::little, |
| pw::i2c::RegisterAddressSize::k1Byte) {} |
| |
| pw::Status Device::Enable() { |
| device_.WriteRegister8( |
| 0x1f, 0x0f, pw::chrono::SystemClock::for_at_least(10ms)); |
| |
| /* |
| +----------------+-----------------------------------+ |
| | ACCEL_CONFIG0 | | |
| +----------------+-----------------------------------+ |
| | Name | ACCEL_CONFIG0 | |
| | Address | 33 (21h) | |
| | Serial IF | R/W | |
| | Reset value | 0x06 | |
| +----------------+-----------------------------------+ |
| | BIT | NAME | FUNCTION | |
| +----------------+-----------------------------------+ |
| | 7 | - | Reserved | |
| | 6:5 | ACCEL_UI_FS_SEL | 00: ±16g | |
| | | | 01: ±8g | |
| | | | 10: ±4g | |
| | | | 11: ±2g | |
| | 4 | - | Reserved | |
| | 3:0 | ACCEL_ODR | 0101: 1.6k Hz (LN mode) | |
| | | | 0110: 800 Hz (LN mode) | |
| | | | 0111: 400 Hz (LP or LN) | |
| | | | 1000: 200 Hz (LP or LN) | |
| | | | 1001: 100 Hz (LP or LN) | |
| | | | 1010: 50 Hz (LP or LN) | |
| | | | 1011: 25 Hz (LP or LN) | |
| | | | 1100: 12.5 Hz (LP or LN) | |
| | | | 1101: 6.25 Hz (LP mode) | |
| | | | 1110: 3.125 Hz (LP mode) | |
| | | | 1111: 1.5625 Hz (LP mode) | |
| +----------------+-----------------------------------+ |
| */ |
| uint8_t new_accel_config = 0b00101000; |
| device_.WriteRegister8(Icm42670pRegister::kAccelConfig0, |
| new_accel_config, |
| pw::chrono::SystemClock::for_at_least(10ms)); |
| |
| /* |
| +--------------+------------------------+ |
| | GYRO_CONFIG0 | | |
| +--------------+------------------------+ |
| | Name | GYRO_CONFIG0 | |
| | Address | 32 (20h) | |
| | Serial IF | R/W | |
| | Reset value | 0x06 | |
| +--------------+------------------------+ |
| | BIT | NAME | FUNCTION | |
| +--------------+------------------------+ |
| | 7 | - | Reserved | |
| | 6:5 | GYRO_UI_FS_SEL | 00: ±2000 dps | |
| | | | 01: ±1000 dps | |
| | | | 10: ±500 dps | |
| | | | 11: ±250 dps | |
| | 4 | - | Reserved | |
| | 3:0 | GYRO_ODR | 0101: 1.6k Hz | |
| | | | 0110: 800 Hz | |
| | | | 0111: 400 Hz | |
| | | | 1000: 200 Hz | |
| | | | 1001: 100 Hz | |
| | | | 1010: 50 Hz | |
| | | | 1011: 25 Hz | |
| | | | 1100: 12.5 Hz | |
| +--------------+------------------------+ |
| */ |
| uint8_t new_gyro_config = 0b00101000; |
| device_.WriteRegister8(Icm42670pRegister::kGyroConfig0, |
| new_gyro_config, |
| pw::chrono::SystemClock::for_at_least(10ms)); |
| |
| return pw::OkStatus(); |
| } |
| |
| Status Device::Probe() { |
| pw::Status probe_result(initiator_.ProbeDeviceFor( |
| kAddress, pw::chrono::SystemClock::for_at_least(50ms))); |
| |
| if (probe_result != pw::OkStatus()) { |
| PW_LOG_DEBUG("ICM-42670-P Probe Failed"); |
| } else { |
| PW_LOG_DEBUG("ICM-42670-P Probe Ok"); |
| } |
| return probe_result; |
| } |
| |
| void Device::LogControllerInfo() { |
| device_.WriteRegister8( |
| 0x1f, 0x0f, pw::chrono::SystemClock::for_at_least(10ms)); |
| |
| ImuReadReg(device_, Icm42670pRegister::kWhoAmI, "WHO_AM_I"); |
| ImuReadReg(device_, Icm42670pRegister::kPwrMgmt0, "PWR_MGMT0"); |
| ImuReadReg(device_, Icm42670pRegister::kGyroConfig0, "GYRO_CONFIG0"); |
| ImuReadReg(device_, Icm42670pRegister::kAccelConfig0, "ACCEL_CONFIG0"); |
| } |
| |
| pw::Result<kudzu::imu::ImuSample> Device::ReadValues() { |
| return ReadData(device_); |
| } |
| |
| } // namespace kudzu::icm42670p |
