From 53f474f449c579df6ddacf7d4903c30ffaffa83b Mon Sep 17 00:00:00 2001 From: Shengwen Cheng Date: Fri, 9 Feb 2024 00:20:47 +0800 Subject: [PATCH] Import MPU6500 driver --- drivers/devices/mpu6500.c | 163 ++++++++++++++++++++++++++++++++++++++ drivers/devices/mpu6500.h | 96 ++++++++++++++++++++++ drivers/drivers.mk | 12 +-- drivers/periph/spi.c | 93 ++++++++++++++++++++++ drivers/periph/spi.h | 12 +++ include/kernel/delay.h | 9 +++ kernel/time.c | 5 ++ 7 files changed, 385 insertions(+), 5 deletions(-) create mode 100644 drivers/devices/mpu6500.c create mode 100644 drivers/devices/mpu6500.h create mode 100644 drivers/periph/spi.c create mode 100644 drivers/periph/spi.h create mode 100644 include/kernel/delay.h diff --git a/drivers/devices/mpu6500.c b/drivers/devices/mpu6500.c new file mode 100644 index 00000000..2bc68c48 --- /dev/null +++ b/drivers/devices/mpu6500.c @@ -0,0 +1,163 @@ +#include +#include +#include + +#include "mpu6500.h" +#include "spi.h" + +#define s8_to_s16(upper_s8, lower_s8) ((int16_t) upper_s8 << 8 | lower_s8) + +struct mpu6500_device mpu6500 = { + .gyro_bias = {0, 0, 0}, + .accel_bias = {0, 0, 0}, + .accel_fs = MPU6500_GYRO_FS_8G, + .gyro_fs = MPU6500_GYRO_FS_1000_DPS, +}; + +int mpu6500_ioctl(struct file *filp, unsigned int cmd, unsigned long arg); +int mpu6500_open(struct inode *inode, struct file *file); + +static struct file_operations mpu6500_file_ops = { + .ioctl = mpu6500_ioctl, + .open = mpu6500_open, +}; + +int mpu6500_open(struct inode *inode, struct file *file) +{ + return 0; +} + +int mpu6500_ioctl(struct file *filp, unsigned int cmd, unsigned long arg) +{ + return 0; +} + +static uint8_t mpu6500_read_byte(uint8_t address) +{ + uint8_t read; + + spi1_chip_select(); + spi1_read_write(address | 0x80); + read = spi1_read_write(0xff); + spi1_chip_deselect(); + + return read; +} + +static void mpu6500_write_byte(uint8_t address, uint8_t data) +{ + spi1_chip_select(); + spi1_read_write(address); + spi1_read_write(data); + spi1_chip_deselect(); +} + +static uint8_t mpu6500_read_who_am_i() +{ + uint8_t id = mpu6500_read_byte(MPU6500_WHO_AM_I); + msleep(5); + return id; +} + +static void mpu6500_reset() +{ + mpu6500_write_byte(MPU6500_PWR_MGMT_1, 0x80); + msleep(100); +} + +void mpu6500_init(void) +{ + /* Probe the sensor */ + while (mpu6500_read_who_am_i() != 0x70) + ; + + mpu6500_reset(); + + switch (mpu6500.accel_fs) { + case MPU6500_GYRO_FS_2G: + mpu6500.accel_scale = 9.81 / 16384.0f; + mpu6500_write_byte(MPU6500_ACCEL_CONFIG, 0x00); + break; + case MPU6500_GYRO_FS_4G: + mpu6500.accel_scale = 9.81f / 8192.0f; + mpu6500_write_byte(MPU6500_ACCEL_CONFIG, 0x08); + break; + case MPU6500_GYRO_FS_8G: + mpu6500.accel_scale = 9.81f / 4096.0f; + mpu6500_write_byte(MPU6500_ACCEL_CONFIG, 0x10); + break; + case MPU6500_GYRO_FS_16G: + mpu6500.accel_scale = 9.81f / 2048.0f; + mpu6500_write_byte(MPU6500_ACCEL_CONFIG, 0x18); + break; + } + msleep(100); + + switch (mpu6500.gyro_fs) { + case MPU6500_GYRO_FS_250_DPS: + mpu6500.gyro_scale = 1.0f / 131.0f; + mpu6500_write_byte(MPU6500_GYRO_CONFIG, 0x00); + break; + case MPU6500_GYRO_FS_500_DPS: + mpu6500.gyro_scale = 1.0f / 65.5f; + mpu6500_write_byte(MPU6500_GYRO_CONFIG, 0x08); + break; + case MPU6500_GYRO_FS_1000_DPS: + mpu6500.gyro_scale = 1.0f / 32.8f; + mpu6500_write_byte(MPU6500_GYRO_CONFIG, 0x10); + break; + case MPU6500_GYRO_FS_2000_DPS: + mpu6500.gyro_scale = 1.0f / 16.4f; + mpu6500_write_byte(MPU6500_GYRO_CONFIG, 0x18); + break; + } + msleep(100); + + /* Gyroscope update rate = 1KHz, Low-pass filter bandwitdh = 20Hz */ + mpu6500_write_byte(MPU6500_CONFIG, GYRO_DLPF_BANDWIDTH_20Hz); + msleep(100); + + /* Acceleromter update rate = 1KHz, Low-pass filter bandwitdh = 20Hz */ + mpu6500_write_byte(MPU6500_ACCEL_CONFIG2, ACCEL_DLPF_BANDWIDTH_20Hz); + msleep(100); + + /* Enable data-ready interrupt */ + mpu6500_write_byte(MPU6500_INT_ENABLE, 0x01); + msleep(100); + + register_chrdev("imu0", &mpu6500_file_ops); + printk("imu0: mp6500"); +} + +void mpu6500_interrupt_handler(void) +{ + uint8_t buffer[14]; + + /* Read measurements */ + spi1_chip_select(); + spi1_read_write(MPU6500_ACCEL_XOUT_H | 0x80); + buffer[0] = spi1_read_write(0xff); + buffer[1] = spi1_read_write(0xff); + buffer[2] = spi1_read_write(0xff); + buffer[3] = spi1_read_write(0xff); + buffer[4] = spi1_read_write(0xff); + buffer[5] = spi1_read_write(0xff); + buffer[6] = spi1_read_write(0xff); + buffer[7] = spi1_read_write(0xff); + buffer[8] = spi1_read_write(0xff); + buffer[9] = spi1_read_write(0xff); + buffer[10] = spi1_read_write(0xff); + buffer[11] = spi1_read_write(0xff); + buffer[12] = spi1_read_write(0xff); + buffer[13] = spi1_read_write(0xff); + spi1_chip_deselect(); + + /* Composite measurements */ + mpu6500.accel_unscaled[0] = -s8_to_s16(buffer[0], buffer[1]); + mpu6500.accel_unscaled[1] = +s8_to_s16(buffer[2], buffer[3]); + mpu6500.accel_unscaled[2] = -s8_to_s16(buffer[4], buffer[5]); + mpu6500.temp_unscaled = s8_to_s16(buffer[6], buffer[7]); + mpu6500.gyro_unscaled[0] = -s8_to_s16(buffer[8], buffer[9]); + mpu6500.gyro_unscaled[1] = +s8_to_s16(buffer[10], buffer[11]); + mpu6500.gyro_unscaled[2] = -s8_to_s16(buffer[12], buffer[13]); +} diff --git a/drivers/devices/mpu6500.h b/drivers/devices/mpu6500.h new file mode 100644 index 00000000..deeeaed5 --- /dev/null +++ b/drivers/devices/mpu6500.h @@ -0,0 +1,96 @@ +#ifndef __MPU6500_H__ +#define __MPU6500_H__ + +#include + +#define MPU6500_SMPLRT_DIV 0x19 +#define MPU6500_CONFIG 0x1A +#define MPU6500_GYRO_CONFIG 0x1B +#define MPU6500_ACCEL_CONFIG 0x1C +#define MPU6500_ACCEL_CONFIG2 0x1D +#define MPU6500_MOT_THR 0x1F +#define MPU6500_FIFO_EN 0x23 +#define MPU6500_INT_PIN_CFG 0x37 +#define MPU6500_INT_ENABLE 0x38 +#define MPU6500_INT_STATUS 0x3A +#define MPU6500_ACCEL_XOUT_H 0x3B +#define MPU6500_ACCEL_XOUT_L 0x3C +#define MPU6500_ACCEL_YOUT_H 0x3D +#define MPU6500_ACCEL_YOUT_L 0x3E +#define MPU6500_ACCEL_ZOUT_H 0x3F +#define MPU6500_ACCEL_ZOUT_L 0x40 +#define MPU6500_TEMP_OUT_H 0x41 +#define MPU6500_TEMP_OUT_L 0x42 +#define MPU6500_GYRO_XOUT_H 0x43 +#define MPU6500_GYRO_XOUT_L 0x44 +#define MPU6500_GYRO_YOUT_H 0x45 +#define MPU6500_GYRO_YOUT_L 0x46 +#define MPU6500_GYRO_ZOUT_H 0x47 +#define MPU6500_GYRO_ZOUT_L 0x48 +#define MPU6500_USER_CTRL 0x6A +#define MPU6500_PWR_MGMT_1 0x6B +#define MPU6500_PWR_MGMT_2 0x6C +#define MPU6500_FIFO_COUNTH 0x72 +#define MPU6500_FIFO_COUNTL 0x73 +#define MPU6500_FIFO_R_W 0x74 +#define MPU6500_WHO_AM_I 0x75 + +#define MPU6500_I2C_SLV4_ADDR 0x31 +#define MPU6500_I2C_SLV4_REG 0x32 +#define MPU6500_I2C_SLV4_CTRL 0x34 +#define MPU6500_I2C_SLV4_DI 0x35 +#define MPU6500_I2C_SLV4_DO 0x40 + +#define MPU6500T_85degC 0.00294f + +#define GYRO_DLPF_BANDWIDTH_20Hz 0x04 + +#define ACCEL_DLPF_BANDWIDTH_20Hz 0x04 +#define ACCEL_DLPF_DISABLE 0x08 + +enum { + MPU6500_GYRO_FS_2G = 0, + MPU6500_GYRO_FS_4G = 1, + MPU6500_GYRO_FS_8G = 2, + MPU6500_GYRO_FS_16G = 3, +}; + +enum { + MPU6500_GYRO_FS_250_DPS = 0, + MPU6500_GYRO_FS_500_DPS = 1, + MPU6500_GYRO_FS_1000_DPS = 2, + MPU6500_GYRO_FS_2000_DPS = 3, +}; + +struct mpu6500_device { + /* Sensor range */ + int accel_fs; + int gyro_fs; + + /* Sensor resolution */ + float accel_scale; + float gyro_scale; + + /* Calibration data */ + int16_t gyro_bias[3]; + float accel_bias[3]; + float accel_rescale[3]; + + /* Sensor data */ + int16_t accel_unscaled[3]; + int16_t gyro_unscaled[3]; + int16_t temp_unscaled; + + float accel_raw[3]; + float gyro_raw[3]; + float temp_raw; + + float accel_lpf[3]; + float gyro_lpf[3]; + + /* Update rate */ + float last_read_time; + float update_freq; +}; + +#endif diff --git a/drivers/drivers.mk b/drivers/drivers.mk index a71f568c..6921c0b9 100644 --- a/drivers/drivers.mk +++ b/drivers/drivers.mk @@ -4,8 +4,10 @@ CFLAGS += -I $(PROJ_ROOT)/drivers/device CFLAGS += -I $(PROJ_ROOT)/drivers/periph CFLAGS += -I $(PROJ_ROOT)/drivers/periph/serial -SRC += $(PROJ_ROOT)/drivers/periph/serial/uart.c \ - $(PROJ_ROOT)/drivers/periph/serial/console.c \ - $(PROJ_ROOT)/drivers/periph/serial/mavlink.c \ - $(PROJ_ROOT)/drivers/periph/serial/debug_link.c \ - $(PROJ_ROOT)/drivers/periph/pwm.c +SRC += $(PROJ_ROOT)/drivers/periph/serial/uart.c +SRC += $(PROJ_ROOT)/drivers/periph/serial/console.c +SRC += $(PROJ_ROOT)/drivers/periph/serial/mavlink.c +SRC += $(PROJ_ROOT)/drivers/periph/serial/debug_link.c +SRC += $(PROJ_ROOT)/drivers/periph/pwm.c +SRC += $(PROJ_ROOT)/drivers/periph/spi.c +SRC += $(PROJ_ROOT)/drivers/devices/mpu6500.c diff --git a/drivers/periph/spi.c b/drivers/periph/spi.c new file mode 100644 index 00000000..ec852885 --- /dev/null +++ b/drivers/periph/spi.c @@ -0,0 +1,93 @@ +#include + +#include +#include + +#include "stm32f4xx.h" + +int spi_ioctl(struct file *filp, unsigned int cmd, unsigned long arg); +int spi_open(struct inode *inode, struct file *file); + +static struct file_operations spi_file_ops = { + .ioctl = spi_ioctl, + .open = spi_open, +}; + +int spi_open(struct inode *inode, struct file *file) +{ + return 0; +} + +int spi_ioctl(struct file *filp, unsigned int cmd, unsigned long arg) +{ + return 0; +} + +/* SPI1 + * CS: GPIO A4 + * SCK: GPIO A5 + * MISO: GPIO A6 + * MOSI: GPIO A7 + */ +void spi1_init(void) +{ + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); + RCC_APB2PeriphClockCmd(RCC_APB2Periph_SPI1, ENABLE); + + GPIO_PinAFConfig(GPIOA, GPIO_PinSource5, GPIO_AF_SPI1); + GPIO_PinAFConfig(GPIOA, GPIO_PinSource6, GPIO_AF_SPI1); + GPIO_PinAFConfig(GPIOA, GPIO_PinSource7, GPIO_AF_SPI1); + + GPIO_InitTypeDef GPIO_InitStruct = { + .GPIO_Pin = GPIO_Pin_5 | GPIO_Pin_6 | GPIO_Pin_7, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP, + }; + GPIO_Init(GPIOA, &GPIO_InitStruct); + + GPIO_InitStruct.GPIO_Pin = GPIO_Pin_4; + GPIO_InitStruct.GPIO_Mode = GPIO_Mode_OUT; + GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP; + GPIO_Init(GPIOA, &GPIO_InitStruct); + + SPI_InitTypeDef SPI_InitStruct = { + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_Mode = SPI_Mode_Master, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_NSS = SPI_NSS_Soft, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_4, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + }; + SPI_Init(SPI1, &SPI_InitStruct); + + SPI_Cmd(SPI1, ENABLE); + + register_chrdev("spi", &spi_file_ops); + printk("spi1: full duplex mode"); +} + +uint8_t spi1_read_write(uint8_t data) +{ + while (SPI_I2S_GetFlagStatus(SPI1, SPI_FLAG_TXE) == RESET) + ; + SPI_I2S_SendData(SPI1, data); + + while (SPI_I2S_GetFlagStatus(SPI1, SPI_FLAG_RXNE) == RESET) + ; + return SPI_I2S_ReceiveData(SPI1); +} + +void spi1_chip_select(void) +{ + GPIO_ResetBits(GPIOA, GPIO_Pin_4); +} + +void spi1_chip_deselect(void) +{ + GPIO_SetBits(GPIOA, GPIO_Pin_4); +} diff --git a/drivers/periph/spi.h b/drivers/periph/spi.h new file mode 100644 index 00000000..4d84ff81 --- /dev/null +++ b/drivers/periph/spi.h @@ -0,0 +1,12 @@ +#ifndef __SPI_H__ +#define __SPI_H__ + +#include + +void spi1_init(void); + +uint8_t spi1_read_write(uint8_t data); +void spi1_chip_select(void); +void spi1_chip_deselect(void); + +#endif diff --git a/include/kernel/delay.h b/include/kernel/delay.h new file mode 100644 index 00000000..9cdeb741 --- /dev/null +++ b/include/kernel/delay.h @@ -0,0 +1,9 @@ +/** + * @file + */ +#ifndef __KERNEL_DELAY_H__ +#define __KERNEL_DELAY_H__ + +void msleep(unsigned int msecs); + +#endif diff --git a/kernel/time.c b/kernel/time.c index 67d21880..0feb70c8 100644 --- a/kernel/time.c +++ b/kernel/time.c @@ -114,3 +114,8 @@ time_t time(time_t *tloc) return tp.tv_sec; } + +void msleep(unsigned int msecs) +{ + // TODO +}