Code: Select all
#include"icm42688.h"
#include <stdio.h>
#include <stdint.h>
#include <math.h>
#include "esp_timer.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_log.h"
#include "driver/i2c.h"
static const char *TAG = "icm-42688_p";
#define I2C_MASTER_SCL_IO 22 /*!< GPIO number used for I2C master clock */
#define I2C_MASTER_SDA_IO 21 /*!< GPIO number used for I2C master data */
#define I2C_MASTER_NUM 0 /*!< I2C master i2c port number, the number of i2c peripheral interfaces available will depend on the chip */
#define I2C_MASTER_FREQ_HZ 400000 /*!< I2C master clock frequency */
#define I2C_MASTER_TX_BUF_DISABLE 0 /*!< I2C master doesn't need buffer */
#define I2C_MASTER_RX_BUF_DISABLE 0 /*!< I2C master doesn't need buffer */
#define I2C_MASTER_TIMEOUT_MS 1000
#define icm42688p_address 0x68 /*!< Slave address of the icm42688p sensor */
#define icm42688p_WHO_AM_I_REG_ADDR 0x75 /*!< Register addresses of the "who am I" register */
#define icm42688p_PWR_MGMT_REG_ADDR 0x6E /*!< Register addresses of the power managment register */
int ret = 0;
uint8_t _bank = 0; ///< current user bank
/*! Earth's gravity in m/s^2 */
#define GRAVITY_EARTH (9.80665f)
/**
* @brief Memory locations to store icm42688p accel and gyro Sensors data
*/
uint8_t accel_x_LSB[2] = {0};
uint8_t accel_x_MSB[2] = {0};
uint8_t accel_y_LSB[2] = {0};
uint8_t accel_y_MSB[2] = {0};
uint8_t accel_z_LSB[2] = {0};
uint8_t accel_z_MSB[2] = {0};
uint8_t gyro_x_LSB[2] = {0};
uint8_t gyro_x_MSB[2] = {0};
uint8_t gyro_y_LSB[2] = {0};
uint8_t gyro_y_MSB[2] = {0};
uint8_t gyro_z_LSB[2] = {0};
uint8_t gyro_z_MSB[2] = {0};
/**
* @brief Convertion of icm42688p accel and gyro Sensors data into signed integer
*/
float accel_x_int16 = 0;
float accel_y_int16 = 0;
float accel_z_int16 = 0;
float gyro_x_int16 = 0;
float gyro_y_int16 = 0;
float gyro_z_int16 = 0;
/**
* @brief icm42688p IMU Register Addresses
*/
typedef enum
{
ACC_X_LSB = 0x20,
ACC_X_MSB = 0x1F,
ACC_Y_LSB = 0x22,
ACC_Y_MSB = 0x21,
ACC_Z_LSB = 0x24,
ACC_Z_MSB = 0x23,
GYRO_X_LSB = 0x26,
GYRO_X_MSB = 0x25,
GYRO_Y_LSB = 0x28,
GYRO_Y_MSB = 0x27,
GYRO_Z_LSB = 0x2A,
GYRO_Z_MSB = 0x29,
UB0_REG_DEVICE_CONFIG = 0x11,
REG_BANK_SEL = 0x76,
}icm42688p_register_address;
/**
* @brief Read a sequence of bytes from a BM1088 accel sensor registers
*/
esp_err_t icm42688p_register_read(uint8_t reg_addr, uint8_t *data, size_t len){
for (size_t i = 0; i < len; i++) {
ret = i2c_master_write_read_device(i2c_port_t(I2C_MASTER_NUM), icm42688p_address, ®_addr, 1,
data, len, I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS );
}
return ret;
}
esp_err_t icm42688p_register_write_byte(uint8_t reg_addr, uint8_t data){
uint8_t write_buf[2] = {reg_addr, data};
ret = i2c_master_write_to_device(i2c_port_t(I2C_MASTER_NUM), icm42688p_address, write_buf, sizeof(write_buf),
I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS);
return ret;
}
esp_err_t i2c_master_init(void){
int i2c_master_port = I2C_MASTER_NUM;
i2c_config_t conf;
conf.mode = I2C_MODE_MASTER;
conf.sda_io_num = I2C_MASTER_SDA_IO;
conf.scl_io_num = I2C_MASTER_SCL_IO;
conf.sda_pullup_en = GPIO_PULLUP_ENABLE;
conf.scl_pullup_en = GPIO_PULLUP_ENABLE;
conf.master.clk_speed = I2C_MASTER_FREQ_HZ;
conf.clk_flags = 0;
i2c_param_config(i2c_port_t(i2c_master_port), &conf);
return i2c_driver_install(i2c_port_t(i2c_master_port), conf.mode, I2C_MASTER_RX_BUF_DISABLE, I2C_MASTER_TX_BUF_DISABLE, 0);
}
void ICM_INIT(){
uint8_t data[1];
ESP_ERROR_CHECK(i2c_master_init());
ESP_LOGI(TAG, "I2C initialized successfully");
reset();
/* Read the icm42688p WHO_AM_I register, on power up the register should have the value 0x47 */
esp_err_t err = icm42688p_register_read(icm42688p_WHO_AM_I_REG_ADDR, data, 1);
ESP_LOGI(TAG, "WHO_AM_I = %X", data[0]);
if (err != ESP_OK) {
ESP_LOGE(TAG, "Error reading WHO_AM_I register: %d", err);
}
/* Accel mode to low noise and gyro to standby mode to on both sensors */
uint8_t read_sensor_mode;
ESP_ERROR_CHECK(icm42688p_register_read(icm42688p_PWR_MGMT_REG_ADDR, &read_sensor_mode, 1));
read_sensor_mode |= 0x03 << 0;
ESP_ERROR_CHECK(icm42688p_register_write_byte(icm42688p_PWR_MGMT_REG_ADDR, read_sensor_mode));
vTaskDelay(50 / portTICK_PERIOD_MS);
}
int setBank(uint8_t bank){
// if we are already on this bank, bail
if (_bank == bank) return 1;
_bank = bank;
return icm42688p_register_write_byte(REG_BANK_SEL, bank);
}
void reset(){
setBank(0);
icm42688p_register_write_byte(UB0_REG_DEVICE_CONFIG, 0x01);
// wait for ICM42688 to come back up
vTaskDelay(1);
}
/*!
* @brief This function converts lsb to meter per second squared for 16 bit accelerometer at
* range 2G, 4G, 8G or 16G.
*/
float lsb_to_mps2(int16_t val, int8_t g_range, uint8_t bit_width)
{
double power = 2;
float half_scale = (float)((pow((double)power, (double)bit_width) / 2.0f));
return (GRAVITY_EARTH * val * g_range) / half_scale;
}
/*!
* @brief This function converts lsb to degree per second for 16 bit gyro at
* range 125, 250, 500, 1000 or 2000dps.
*/
float lsb_to_dps(int16_t val, float dps, uint8_t bit_width)
{
double power = 2;
float half_scale = (float)((pow((double)power, (double)bit_width) / 2.0f));
return (dps / (half_scale)) * (val);
}
/*!
* @brief This function reads raw accel_x LSB data and converts to degree per second
*/
double accel_read_rawX(){
uint8_t lsb, msb;
uint16_t msblsb;
ESP_ERROR_CHECK(icm42688p_register_read(ACC_X_LSB, accel_x_LSB, 2));
ESP_ERROR_CHECK(icm42688p_register_read(ACC_X_MSB, accel_x_MSB, 2));
lsb = accel_x_LSB[0];
msb = accel_x_MSB[0];
msblsb = (msb << 8) | lsb;
float x_original_int = ((int16_t) msblsb); /* Data in X axis */
float x = lsb_to_mps2(x_original_int, 24, 16);
return x;
}
/*!
* @brief This function reads raw accel_y LSB data and converts to degree per second
*/
double accel_read_rawY(){
uint8_t lsb, msb;
uint16_t msblsb;
ESP_ERROR_CHECK(icm42688p_register_read(ACC_Y_LSB, accel_y_LSB, 2));
ESP_ERROR_CHECK(icm42688p_register_read(ACC_Y_MSB, accel_y_MSB, 2));
lsb = accel_y_LSB[0];
msb = accel_y_MSB[0];
msblsb = (msb << 8) | lsb;
float y_original_int = ((int16_t) msblsb); /* Data in Y axis */
float y = lsb_to_mps2(y_original_int, 24, 16);
return y;
}
/*!
* @brief This function reads raw accel_z LSB data and converts to degree per second
*/
double accel_read_rawZ(){
uint8_t lsb, msb;
uint16_t msblsb;
ESP_ERROR_CHECK(icm42688p_register_read(ACC_Z_LSB, accel_z_LSB, 2));
ESP_ERROR_CHECK(icm42688p_register_read(ACC_Z_MSB, accel_z_MSB, 2));
lsb = accel_z_LSB[0];
msb = accel_z_MSB[0];
msblsb = (msb << 8) | lsb;
float z_original_int = ((int16_t) msblsb); /* Data in Z axis */
float z = lsb_to_mps2(z_original_int, 24, 16);
return z;
}
/*!
* @brief This function reads raw gyro_x LSB data and converts to degree per second
*/
double gyro_read_rawX(){
uint8_t lsb, msb;
uint16_t msblsb;
ESP_ERROR_CHECK(icm42688p_register_read(GYRO_X_LSB, gyro_x_LSB, 2));
ESP_ERROR_CHECK(icm42688p_register_read(GYRO_X_MSB, gyro_x_MSB, 2));
lsb = gyro_x_LSB[0];
msb = gyro_x_MSB[0];
msblsb = (msb << 8) | lsb;
float x_original_int = ((int16_t) msblsb); /* Data in X axis */
float x = lsb_to_dps(x_original_int, (float)250, 16);
return x;
}
/*!
* @brief This function reads raw gyro_y LSB data and converts to degree per second
*/
double gyro_read_rawY(){
uint8_t lsb, msb;
uint16_t msblsb;
ESP_ERROR_CHECK(icm42688p_register_read(GYRO_Y_LSB, gyro_y_LSB, 2));
ESP_ERROR_CHECK(icm42688p_register_read(GYRO_Y_MSB, gyro_y_MSB, 2));
lsb = gyro_y_LSB[0];
msb = gyro_y_MSB[0];
msblsb = (msb << 8) | lsb;
float y_original_int = ((int16_t) msblsb); /* Data in Y axis */
float y = lsb_to_dps(y_original_int, (float)250, 16);
return y;
}
/*!
* @brief This function reads raw gyro_z LSB data and converts to degree per second
*/
double gyro_read_rawZ(){
uint8_t lsb, msb;
uint16_t msblsb;
ESP_ERROR_CHECK(icm42688p_register_read(GYRO_Z_LSB, gyro_z_LSB, 2));
ESP_ERROR_CHECK(icm42688p_register_read(GYRO_Z_MSB, gyro_z_MSB, 2));
lsb = gyro_z_LSB[0];
msb = gyro_z_MSB[0];
msblsb = (msb << 8) | lsb;
float z_original_int = ((int16_t) msblsb); /* Data in Z axis */
float z = lsb_to_dps(z_original_int, (float)250, 16);
return z;
}
I (340) app_start: Starting scheduler on CPU0
I (344) app_start: Starting scheduler on CPU1
I (344) main_task: Started on CPU0
I (354) main_task: Calling app_main()
I (2384) icm-42688_p: I2C initialized successfully
I (2394) icm-42688_p: WHO_AM_I = FF
E (2394) icm-42688_p: Error reading WHO_AM_I register: -1
ESP_ERROR_CHECK failed: esp_err_t 0xffffffff (ESP_FAIL) at 0x400d57d7
0x400d57d7: ICM_INIT() at /Users/nyameaama/Documents/MARS-Flight-System-Firmware/test/server/server-test/build/../components/HALX/icm42688.cpp:137 (discriminator 1)
file: "../components/HALX/icm42688.cpp" line 137
func: void ICM_INIT()
expression: icm42688p_register_read(icm42688p_PWR_MGMT_REG_ADDR, &read_sensor_mode, 1)
abort() was called at PC 0x4008717b on core 0
0x4008717b: _esp_error_check_failed at /Users/nyameaama/esp/esp-idf/components/esp_system/esp_err.c:50
Backtrace: 0x40081922:0x3ffb4c30 0x40087185:0x3ffb4c50 0x4008c792:0x3ffb4c70 0x4008717b:0x3ffb4ce0 0x400d57d7:0x3ffb4d10 0x400d5666:0x3ffb4d40 0x400e95fc:0x3ffb4d70 0x40089879:0x3ffb4da0
0x40081922: panic_abort at /Users/nyameaama/esp/esp-idf/components/esp_system/panic.c:452
0x40087185: esp_system_abort at /Users/nyameaama/esp/esp-idf/components/esp_system/port/esp_system_chip.c:84
0x4008c792: abort at /Users/nyameaama/esp/esp-idf/components/newlib/abort.c:38
0x4008717b: _esp_error_check_failed at /Users/nyameaama/esp/esp-idf/components/esp_system/esp_err.c:50
0x400d57d7: ICM_INIT() at /Users/nyameaama/Documents/MARS-Flight-System-Firmware/test/server/server-test/build/../components/HALX/icm42688.cpp:137 (discriminator 1)
0x400d5666: app_main at /Users/nyameaama/Documents/MARS-Flight-System-Firmware/test/server/server-test/build/../main/server-test.cpp:54 (discriminator 2)
0x400e95fc: main_task at /Users/nyameaama/esp/esp-idf/components/freertos/app_startup.c:208 (discriminator 13)
0x40089879: vPortTaskWrapper at /Users/nyameaama/esp/esp-idf/components/freertos/FreeRTOS-Kernel/portable/xtensa/port.c:162
ELF file SHA256: bb1b17f19b665d1c
Rebooting...
Could anyone point me to any issues I have in the driver setup???