Loading files
// Copyright 2026 Filip Dymczyk and Konrad Grucel
#include "imu_config.h"
#include <zephyr/device.h>
#include <zephyr/devicetree.h>
#include <zephyr/drivers/i2c.h>
#include "logger.h"
#define MPU6050_I2C_ADDR 0x68
#define DLPF_REG_ADDR 0x1A
#define DLPF_44_HZ_REG_VAL 0x02
#define IMU_MEASUREMENT_INTERVAL_REG_ADDR 0x19
#define IMU_MEASUREMENT_INTERVAL_REG_VAL 0x01
#define PWR_MGMT_1 0x68
#define HDC_2080_NODE DT_INST(0, invensense_mpu6050)
static Logger<IS_ENABLED(CONFIG_IMU_LOG)> imu_logger("IMU");
static const i2c_dt_spec dev_i2c = I2C_DT_SPEC_GET(HDC_2080_NODE);
static void
set_sensor_settings(uint8_t reg, uint8_t _configuration)
{
uint8_t configuration[2] = {reg, _configuration};
int ret = i2c_write_dt(&dev_i2c, configuration, sizeof(configuration));
if(ret != 0)
{
imu_logger.platform_log(
LOG_LEVEL::ERR, "Failed to write I2C device %x at Reg. %x", dev_i2c.addr, (double)configuration[0]);
}
else
{
imu_logger.platform_log(LOG_LEVEL::INF, "I2C reg write successful.");
}
}
static int
get_sensor_settings(uint8_t reg, uint8_t* value)
{
int ret = 0;
uint8_t reg_addr = reg;
ret = i2c_write_dt(&dev_i2c, ®_addr, sizeof(reg_addr));
if(ret != 0)
{
imu_logger.platform_log(LOG_LEVEL::ERR, "Failed to write register address %x", reg);
return ret;
}
// Read register value:
ret = i2c_read_dt(&dev_i2c, value, sizeof(*value));
if(ret != 0)
{
imu_logger.platform_log(LOG_LEVEL::ERR, "Failed to read value from register %x", reg);
return ret;
}
imu_logger.platform_log(LOG_LEVEL::INF, "I2C reg read successful: Reg %x, Value %x", reg, *value);
return ret;
}
void
mpu_reset(uint8_t conf)
{
uint8_t tmp = 0u;
int const err = get_sensor_settings(PWR_MGMT_1, &tmp);
if(err != 0)
{
imu_logger.platform_log(LOG_LEVEL::ERR, "Failed to get MPU settings, err: %d", err);
return;
}
tmp &= ~(1 << 7);
tmp |= ((conf & 0x1) << 7);
set_sensor_settings(PWR_MGMT_1, tmp);
}
void
set_dlpf()
{
uint8_t tmp = 0u;
int const err = get_sensor_settings(DLPF_REG_ADDR, &tmp);
if(err != 0)
{
imu_logger.platform_log(LOG_LEVEL::ERR, "Failed to get MPU settings, err: %d", err);
return;
}
tmp |= DLPF_44_HZ_REG_VAL & 0x7;
set_sensor_settings(DLPF_REG_ADDR, tmp);
}
void
set_measurement_interval()
{
set_sensor_settings(IMU_MEASUREMENT_INTERVAL_REG_ADDR, IMU_MEASUREMENT_INTERVAL_REG_VAL);
}533027bbeThis file is pinned to the selected release. Download the original to open it in your design tool.