diff --git a/main/imu/I2C_Driver.c b/main/imu/I2C_Driver.c deleted file mode 100644 index f765743..0000000 --- a/main/imu/I2C_Driver.c +++ /dev/null @@ -1,54 +0,0 @@ -#include "I2C_Driver.h" - - -#define I2C_TRANS_BUF_MINIMUM_SIZE (sizeof(i2c_cmd_desc_t) + \ - sizeof(i2c_cmd_link_t) * 8) /* It is required to have allocate one i2c_cmd_desc_t per command: - * start + write (device address) + write buffer + - * start + write (device address) + read buffer + read buffer for NACK + - * stop */ -static const char *I2C_TAG = "I2C"; -/** - * @brief i2c master initialization - */ -static esp_err_t i2c_master_init(void) -{ - int i2c_master_port = I2C_MASTER_NUM; - - i2c_config_t conf = { - .mode = I2C_MODE_MASTER, - .sda_io_num = I2C_Touch_SDA_IO, - .scl_io_num = I2C_Touch_SCL_IO, - .sda_pullup_en = GPIO_PULLUP_ENABLE, - .scl_pullup_en = GPIO_PULLUP_ENABLE, - .master.clk_speed = I2C_MASTER_FREQ_HZ, - }; - - i2c_param_config(i2c_master_port, &conf); - - return i2c_driver_install(i2c_master_port, conf.mode, I2C_MASTER_RX_BUF_DISABLE, I2C_MASTER_TX_BUF_DISABLE, 0); -} -void I2C_Init(void) -{ - /********************* I2C *********************/ - ESP_ERROR_CHECK(i2c_master_init()); - ESP_LOGI(I2C_TAG, "I2C initialized successfully"); -} - - -// Reg addr is 8 bit -esp_err_t I2C_Write(uint8_t Driver_addr, uint8_t Reg_addr, const uint8_t *Reg_data, uint32_t Length) -{ - uint8_t buf[Length+1]; - - buf[0] = Reg_addr; - // Copy Reg_data to buf starting at buf[1] - memcpy(&buf[1], Reg_data, Length); - return i2c_master_write_to_device(I2C_MASTER_NUM, Driver_addr, buf, Length+1, I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS); -} - - - -esp_err_t I2C_Read(uint8_t Driver_addr, uint8_t Reg_addr, uint8_t *Reg_data, uint32_t Length) -{ - return i2c_master_write_read_device(I2C_MASTER_NUM, Driver_addr, &Reg_addr, 1, Reg_data, Length, I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS); -} diff --git a/main/imu/I2C_Driver.h b/main/imu/I2C_Driver.h deleted file mode 100644 index 34ad182..0000000 --- a/main/imu/I2C_Driver.h +++ /dev/null @@ -1,23 +0,0 @@ -#pragma once - -#include -#include // For memcpy -#include "esp_log.h" -#include "driver/gpio.h" -#include "driver/i2c.h" - - -/********************* I2C *********************/ -#define I2C_Touch_SCL_IO 47 /*!< GPIO number used for I2C master clock */ -#define I2C_Touch_SDA_IO 48 /*!< 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 - - -void I2C_Init(void); -// Reg addr is 8 bit -esp_err_t I2C_Write(uint8_t Driver_addr, uint8_t Reg_addr, const uint8_t *Reg_data, uint32_t Length); -esp_err_t I2C_Read(uint8_t Driver_addr, uint8_t Reg_addr, uint8_t *Reg_data, uint32_t Length); \ No newline at end of file diff --git a/main/imu/QMI8658.c b/main/imu/QMI8658.c deleted file mode 100644 index 7b47107..0000000 --- a/main/imu/QMI8658.c +++ /dev/null @@ -1,303 +0,0 @@ -#include "QMI8658.h" - -IMUdata Accel; -IMUdata Gyro; - -uint8_t Device_addr ; // default for SD0/SA0 low, 0x6A if high -acc_scale_t acc_scale = ACC_RANGE_4G; -gyro_scale_t gyro_scale = GYR_RANGE_64DPS; -acc_odr_t acc_odr = acc_odr_norm_8000; -gyro_odr_t gyro_odr = gyro_odr_norm_8000; -sensor_state_t sensor_state = sensor_default; -lpf_t acc_lpf; - -float accelScales, gyroScales; -float accelScales = 0; -uint8_t readings[12]; -uint32_t reading_timestamp_us; // timestamp in arduino micros() time -/** - * Inialize Wire and send default configs - * @param addr I2C address of sensor, typically 0x6A or 0x6B - */ -void QMI8658_Init(void) -{ - uint8_t buf[1]; - Device_addr = QMI8658_L_SLAVE_ADDRESS; - I2C_Read(Device_addr, QMI8658_REVISION_ID, buf, 1); - printf("QMI8658 Device ID: %x\r\n",buf[0]); // Get chip id - setState(sensor_running); - - setAccScale(acc_scale); - setAccODR(acc_odr); - setAccLPF(LPF_MODE_0); - switch (acc_scale) { - // Possible accelerometer scales (and their register bit settings) are: - // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11). - // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that - // 2-bit value: - case ACC_RANGE_2G: accelScales = 2.0 / 32768.0; break; - case ACC_RANGE_4G: accelScales = 4.0 / 32768.0; break; - case ACC_RANGE_8G: accelScales = 8.0 / 32768.0; break; - case ACC_RANGE_16G: accelScales = 16.0 / 32768.0; break; - } - - setGyroScale(gyro_scale); - setGyroODR(gyro_odr); - setGyroLPF(LPF_MODE_3); - switch (gyro_scale) { - // Possible gyro scales (and their register bit settings) are: - // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11). - // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that - // 2-bit value: - case GYR_RANGE_16DPS: gyroScales = 16.0 / 32768.0; break; - case GYR_RANGE_32DPS: gyroScales = 32.0 / 32768.0; break; - case GYR_RANGE_64DPS: gyroScales = 64.0 / 32768.0; break; - case GYR_RANGE_128DPS: gyroScales = 128.0 / 32768.0; break; - case GYR_RANGE_256DPS: gyroScales = 256.0 / 32768.0; break; - case GYR_RANGE_512DPS: gyroScales = 512.0 / 32768.0; break; - case GYR_RANGE_1024DPS: gyroScales = 1024.0 / 32768.0; break; - } -} -void QMI8658_Loop(void) -{ - getAccelerometer(); -} - -/** - * Transmit one uint8_t of data to QMI8658. - * @param addr address of data to be written - * @param data the data to be written - */ -void QMI8658_transmit(uint8_t addr, uint8_t data) -{ - I2C_Write(Device_addr, addr, &data, 1); -} - -/** - * Receive one uint8_t of data from QMI8658. - * @param addr address of data to be read - * @return the uint8_t of data that was read - */ -uint8_t QMI8658_receive(uint8_t addr) -{ - uint8_t retval; - I2C_Read(Device_addr, addr, &retval, 1); - return retval; -} - -/** - * Writes data to CTRL9 (command register) and waits for ACK. - * @param command the command to be executed - */ -void QMI8658_CTRL9_Write(uint8_t command) -{ - // transmit command - QMI8658_transmit(QMI8658_CTRL9, command); - - // wait for command to be done - while (((QMI8658_receive(QMI8658_STATUSINT)) & 0x80) == 0x00); -} - -/** - * Set output data rate (ODR) of accelerometer. - * @param odr acc_odr_t variable representing new data rate - */ -void setAccODR(acc_odr_t odr) -{ - if (sensor_state != sensor_default) // If the device is not in the default state - { - uint8_t ctrl2 = QMI8658_receive(QMI8658_CTRL2); - ctrl2 &= ~QMI8658_AODR_MASK; // clear previous setting - ctrl2 |= odr; // OR in new setting - QMI8658_transmit(QMI8658_CTRL2, ctrl2); - } - acc_odr = odr; -} - -/** - * Set output data rate (ODR) of gyro. - * @param odr gyro_odr_t variable representing new data rate - */ -void setGyroODR(gyro_odr_t odr) -{ - if (sensor_state != sensor_default) - { - uint8_t ctrl3 = QMI8658_receive(QMI8658_CTRL3); - ctrl3 &= ~QMI8658_GODR_MASK; // clear previous setting - ctrl3 |= odr; // OR in new setting - QMI8658_transmit(QMI8658_CTRL3, ctrl3); - } - gyro_odr = odr; -} - -/** - * Set scale of accelerometer output. - * @param scale acc_scale_t variable representing new scale - */ -void setAccScale(acc_scale_t scale) -{ - if (sensor_state != sensor_default) - { - uint8_t ctrl2 = QMI8658_receive(QMI8658_CTRL2); - ctrl2 &= ~QMI8658_ASCALE_MASK; // clear previous setting - ctrl2 |= scale << QMI8658_ASCALE_OFFSET; // OR in new setting - QMI8658_transmit(QMI8658_CTRL2, ctrl2); - } - acc_scale = scale; -} - -/** - * Set scale of gyro output. - * @param scale gyro_scale_t variable representing new scale - */ -void setGyroScale(gyro_scale_t scale) -{ - if (sensor_state != sensor_default) - { - uint8_t ctrl3 = QMI8658_receive(QMI8658_CTRL3); - ctrl3 &= ~QMI8658_GSCALE_MASK; // clear previous setting - ctrl3 |= scale << QMI8658_GSCALE_OFFSET; // OR in new setting - QMI8658_transmit(QMI8658_CTRL3, ctrl3); - } - gyro_scale = scale; -} - -/** - * Set new low-pass filter value for accelerometer - * @param lp lpf_t variable representing new low-pass filter value - */ -void setAccLPF(lpf_t lpf) -{ - if (sensor_state != sensor_default) - { - uint8_t ctrl5 = QMI8658_receive(QMI8658_CTRL5); - ctrl5 &= !QMI8658_ALPF_MASK; - ctrl5 |= lpf << QMI8658_ALPF_OFFSET; - ctrl5 |= 0x01; // turn on acc low pass filter - QMI8658_transmit(QMI8658_CTRL5, ctrl5); - } - acc_lpf = lpf; -} - -/** - * Set new low-pass filter value for gyro - * @param lp lpf_t variable representing new low-pass filter value - */ -void setGyroLPF(lpf_t lpf) -{ - if (sensor_state != sensor_default) - { - uint8_t ctrl5 = QMI8658_receive(QMI8658_CTRL5); - ctrl5 &= !QMI8658_GLPF_MASK; - ctrl5 |= lpf << QMI8658_GLPF_OFFSET; - ctrl5 |= 0x10; // turn on gyro low pass filter - QMI8658_transmit(QMI8658_CTRL5, ctrl5); - } -} - -/** - * Set new state of QMI8658. - * @param state new state to transition to - */ -void setState(sensor_state_t state) -{ - uint8_t ctrl1; - switch (state) - { - case sensor_running: - ctrl1 = QMI8658_receive(QMI8658_CTRL1); - // enable 2MHz oscillator - ctrl1 &= 0xFE; - // enable auto address increment for fast block reads - ctrl1 |= 0x40; - QMI8658_transmit(QMI8658_CTRL1, ctrl1); - - // enable high speed internal clock, - // acc and gyro in full mode, and - // disable syncSample mode - QMI8658_transmit(QMI8658_CTRL7, 0x43); - - // disable AttitudeEngine Motion On Demand - QMI8658_transmit(QMI8658_CTRL6, 0x00); - break; - case sensor_power_down: - // disable high speed internal clock, - // acc and gyro powered down - QMI8658_transmit(QMI8658_CTRL7, 0x00); - - ctrl1 = QMI8658_receive(QMI8658_CTRL1); - // disable 2MHz oscillator - ctrl1|= 0x01; - QMI8658_transmit(QMI8658_CTRL1, ctrl1); - break; - case sensor_locking: - ctrl1 = QMI8658_receive(QMI8658_CTRL1); - // enable 2MHz oscillator - ctrl1 &= 0xFE; - // enable auto address increment for fast block reads - ctrl1 |= 0x40; - QMI8658_transmit(QMI8658_CTRL1, ctrl1); - - // enable high speed internal clock, - // acc and gyro in full mode, and - // enable syncSample mode - QMI8658_transmit(QMI8658_CTRL7, 0x83); - - // disable AttitudeEngine Motion On Demand - QMI8658_transmit(QMI8658_CTRL6, 0x00); - - // disable internal AHB clock gating: - QMI8658_transmit(QMI8658_CAL1_L, 0x01); - QMI8658_CTRL9_Write(0x12); - // re-enable clock gating - QMI8658_transmit(QMI8658_CAL1_L, 0x00); - QMI8658_CTRL9_Write(0x12); - break; - default: - break; - } - sensor_state = state; -} - - -void getAccelerometer(void) -{ - - uint8_t buf[6]; - I2C_Read(Device_addr, QMI8658_AX_L, buf, 6); - Accel.x = (float)((int16_t)((buf[1]<<8) | (buf[0]))); - Accel.y = (float)((int16_t)((buf[3]<<8) | (buf[2]))); - Accel.z = (float)((int16_t)((buf[5]<<8) | (buf[4]))); - Accel.x = Accel.x * accelScales; - Accel.y = Accel.y * accelScales; - Accel.z = Accel.z * accelScales; - -} -void getGyroscope(void) -{ - uint8_t buf[6]; - I2C_Read(Device_addr, QMI8658_GX_L, buf, 6); - Gyro.x = (float)((int16_t)((buf[1]<<8) | (buf[0]))); - Gyro.y = (float)((int16_t)((buf[3]<<8) | (buf[2]))); - Gyro.z = (float)((int16_t)((buf[5]<<8) | (buf[4]))); - Gyro.x = Gyro.x * gyroScales; - Gyro.y = Gyro.y * gyroScales; - Gyro.z = Gyro.z * gyroScales; -} - - - - - - - - - - - - - - - - - diff --git a/main/imu/QMI8658.h b/main/imu/QMI8658.h deleted file mode 100644 index fd739ef..0000000 --- a/main/imu/QMI8658.h +++ /dev/null @@ -1,161 +0,0 @@ -#pragma once - -#include "I2C_Driver.h" - -//device address -#define QMI8658_L_SLAVE_ADDRESS (0x6B) -#define QMI8658_H_SLAVE_ADDRESS (0x6A) - -#define QMI8658_WHO_AM_I 0x00 // devide identifier -#define QMI8658_REVISION_ID 0x01 -#define QMI8658_CTRL1 0x02 // SPI interface and sensor enable -#define QMI8658_CTRL2 0x03 // Accelerometer settings -#define QMI8658_CTRL3 0x04 // Gyro settings -#define QMI8658_CTRL4 0x05 // reserved (we don't use this) -#define QMI8658_CTRL5 0x06 // Low-pass filter settings -#define QMI8658_CTRL6 0x07 // AttitudeEngine settings (we don't use these) -#define QMI8658_CTRL7 0x08 // Sensor enable -#define QMI8658_CTRL8 0x09 // Motion detection control (not in current lib version) -#define QMI8658_CTRL9 0x0A // Host commands (not in current lib version) - -#define QMI8658_CAL1_L 0x0B // calibration 1 register, lower bits -#define QMI8658_CAL1_H 0x0C // calibration 1 register, higher bits -#define QMI8658_CAL2_L 0x0D // calibration 2 register, lower bits -#define QMI8658_CAL2_H 0x0E // calibration 2 register, higher bits -#define QMI8658_CAL3_L 0x0F // calibration 3 register, lower bits -#define QMI8658_CAL3_H 0x10 // calibration 3 register, higher bits -#define QMI8658_CAL4_L 0x11 // calibration 4 register, lower bits -#define QMI8658_CAL4_H 0x12 // calibration 4 register, higher bits - -#define QMI8658_TEMP_L 0x33 // lower bits of temperature data -#define QMI8658_TEMP_H 0x34 // upper bits of temperature data - -#define QMI8658_STATUSINT 0x2D // status + interrupt register - -#define QMI8658_AX_L 0x35 // lower bits of x-axis acceleration -#define QMI8658_AX_H 0x36 // upper bits of x-axis acceleration -#define QMI8658_AY_L 0x37 // lower bits of y-axis acceleration -#define QMI8658_AY_H 0x38 // upper bits of y-axis acceleration -#define QMI8658_AZ_L 0x39 -#define QMI8658_AZ_H 0x3A -#define QMI8658_GX_L 0x3B // lower bits of x-axis angular velocity -#define QMI8658_GX_H 0x3C // upper bits of x-axis angular velocity -#define QMI8658_GY_L 0x3D -#define QMI8658_GY_H 0x3E -#define QMI8658_GZ_L 0x3F -#define QMI8658_GZ_H 0x40 - -#define QMI8658_AODR_MASK 0x0F // bits in acc data rate are 1, rest are 0 (CTRL2) -#define QMI8658_GODR_MASK 0x0F // bits in gyro data rate are 1, rest are 0 (CTRL3) -#define QMI8658_ASCALE_MASK 0x70 // bits in acc scale are 1, rest are 0 -#define QMI8658_GSCALE_MASK 0x70 // bits in gyro scale are 1, rest are 0 -#define QMI8658_ALPF_MASK 0x06 // bits in acc low pass filter setting -#define QMI8658_GLPF_MASK 0x60 // bits in gyro low pass filter setting -#define QMI8658_ASCALE_OFFSET 4 // offset to acc scale bits -#define QMI8658_GSCALE_OFFSET 4 // offset to gyro scale bits -#define QMI8658_ALPF_OFFSET 1 // offset to acc low pass filter bits -#define QMI8658_GLPF_OFFSET 5 // offset to gyro low pass filter bits - -#define QMI8658_COMM_TIMEOUT 50 // communication timeout, in ms - - -// delay between refreshes of sensor data in us -// applies to individual sensor readings while in locking mode -// has no effect in running mode -#define QMI8658_REFRESH_DELAY 2000 - -// control clock gating (necessary to use data locking) -#define QMI8658_CTRL_CMD_AHB_CLOCK_GATING 0x12 - - -typedef enum { - acc_odr_norm_8000 = 0x0, - acc_odr_norm_4000, - acc_odr_norm_2000, - acc_odr_norm_1000, - acc_odr_norm_500, - acc_odr_norm_250, - acc_odr_norm_120, - acc_odr_norm_60, - acc_odr_norm_30, - acc_odr_lp_128 = 0xC, - acc_odr_lp_21, - acc_odr_lp_11, - acc_odr_lp_3, -} acc_odr_t; - -typedef enum { - gyro_odr_norm_8000 = 0x0, - gyro_odr_norm_4000, - gyro_odr_norm_2000, - gyro_odr_norm_1000, - gyro_odr_norm_500, - gyro_odr_norm_250, - gyro_odr_norm_120, - gyro_odr_norm_60, - gyro_odr_norm_30 -} gyro_odr_t; - -typedef enum { - ACC_RANGE_2G = 0x0, - ACC_RANGE_4G, - ACC_RANGE_8G, - ACC_RANGE_16G -} acc_scale_t; - -typedef enum { - GYR_RANGE_16DPS = 0x0, - GYR_RANGE_32DPS, - GYR_RANGE_64DPS, - GYR_RANGE_128DPS, - GYR_RANGE_256DPS, - GYR_RANGE_512DPS, - GYR_RANGE_1024DPS -} gyro_scale_t; - -typedef enum { - LPF_MODE_0 = 0x0, //2.66% of ODR - LPF_MODE_1 = 0x2, //3.63% of ODR - LPF_MODE_2 = 0x4, //5.39% of ODR - LPF_MODE_3 = 0x6 //13.37% of ODR -} lpf_t; - -typedef enum { - sensor_default, - sensor_power_down, - sensor_running, - sensor_locking -} sensor_state_t; - -typedef struct __IMUdata { - float x; - float y; - float z; -} IMUdata; - -extern IMUdata Accel; -extern IMUdata Gyro; - -void QMI8658_Init(void); -void QMI8658_Loop(void); -void QMI8658_transmit(uint8_t addr, uint8_t data); -uint8_t QMI8658_receive(uint8_t addr); -void QMI8658_CTRL9_Write(uint8_t command); -void QMI8658_sensor_update(); -void QMI8658_update_if_needed(); -void setAccODR(acc_odr_t odr); -void setGyroODR(gyro_odr_t odr); -void setAccScale(acc_scale_t scale); -void setGyroScale(gyro_scale_t scale); -void setAccLPF(lpf_t lpf); -void setGyroLPF(lpf_t lpf); -void setState(sensor_state_t state); -void getRawReadings(int16_t* buf); -float getAccX(); -float getAccY(); -float getAccZ(); -float getGyroX(); -float getGyroY(); -float getGyroZ(); -void getAccelerometer(void); -void getGyroscope(void); \ No newline at end of file diff --git a/main/imu/imu_manager.c b/main/imu/imu_manager.c deleted file mode 100644 index 758bbdf..0000000 --- a/main/imu/imu_manager.c +++ /dev/null @@ -1,55 +0,0 @@ -#include "imu/imu_manager.h" - -#include -#include "esp_log.h" -#include "esp_timer.h" -#include "freertos/FreeRTOS.h" -#include "freertos/task.h" -#include "imu/I2C_Driver.h" -#include "imu/QMI8658.h" - -static const char *TAG = "imu"; - -static imu_shake_cb_t s_shake_cb = NULL; -static int64_t s_last_shake_us = 0; - -static void imu_task(void *arg) -{ - (void)arg; - const float threshold_g = 1.6f; - const int64_t min_interval_us = 800000; - - while (1) { - QMI8658_Loop(); - float ax = Accel.x; - float ay = Accel.y; - float az = Accel.z; - float mag = sqrtf(ax * ax + ay * ay + az * az); - float delta = fabsf(mag - 1.0f); - - if (delta > threshold_g) { - int64_t now = esp_timer_get_time(); - if (now - s_last_shake_us > min_interval_us) { - s_last_shake_us = now; - ESP_LOGI(TAG, "Shake detected (delta=%.2f)", delta); - if (s_shake_cb) { - s_shake_cb(); - } - } - } - - vTaskDelay(pdMS_TO_TICKS(50)); - } -} - -void imu_manager_init(void) -{ - I2C_Init(); - QMI8658_Init(); - xTaskCreatePinnedToCore(imu_task, "imu_task", 4096, NULL, 4, NULL, 0); -} - -void imu_manager_set_shake_callback(imu_shake_cb_t cb) -{ - s_shake_cb = cb; -} diff --git a/main/imu/imu_manager.h b/main/imu/imu_manager.h deleted file mode 100644 index 08f1c34..0000000 --- a/main/imu/imu_manager.h +++ /dev/null @@ -1,8 +0,0 @@ -#pragma once - -#include - -typedef void (*imu_shake_cb_t)(void); - -void imu_manager_init(void); -void imu_manager_set_shake_callback(imu_shake_cb_t cb);