Import MPU6500 driver

drivers/devices/mpu6500.c

@@ -0,0 +1,163 @@
+#include <fs/fs.h>
+#include <kernel/delay.h>
+#include <printk.h>
+
+#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]);
+}

drivers/devices/mpu6500.h

@@ -0,0 +1,96 @@
+#ifndef __MPU6500_H__
+#define __MPU6500_H__
+
+#include <stdint.h>
+
+#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

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

drivers/periph/spi.c

@@ -0,0 +1,93 @@
+#include <stdbool.h>
+
+#include <fs/fs.h>
+#include <printk.h>
+
+#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);
+}

drivers/periph/spi.h

@@ -0,0 +1,12 @@
+#ifndef __SPI_H__
+#define __SPI_H__
+
+#include <stdint.h>
+
+void spi1_init(void);
+
+uint8_t spi1_read_write(uint8_t data);
+void spi1_chip_select(void);
+void spi1_chip_deselect(void);
+
+#endif

include/kernel/delay.h

@@ -0,0 +1,9 @@
+/**
+ * @file
+ */
+#ifndef __KERNEL_DELAY_H__
+#define __KERNEL_DELAY_H__
+
+void msleep(unsigned int msecs);
+
+#endif