From cd6087f3d482dae52924caf8f43da419401face1 Mon Sep 17 00:00:00 2001 From: Robert Skoglund Date: Mon, 3 Oct 2016 08:41:16 +0200 Subject: [PATCH] Working SPI gyroacc --- UAV-ControlSystem/inc/drivers/accel_gyro.h | 183 +++++++ UAV-ControlSystem/src/drivers/accel_gyro.c | 553 +++++++++++++++++++++ 2 files changed, 736 insertions(+) create mode 100644 UAV-ControlSystem/inc/drivers/accel_gyro.h create mode 100644 UAV-ControlSystem/src/drivers/accel_gyro.c diff --git a/UAV-ControlSystem/inc/drivers/accel_gyro.h b/UAV-ControlSystem/inc/drivers/accel_gyro.h new file mode 100644 index 0000000..43ecb2c --- /dev/null +++ b/UAV-ControlSystem/inc/drivers/accel_gyro.h @@ -0,0 +1,183 @@ +/* + * accelgyro.h + * + * Created on: 16 sep. 2016 + * Author: rsd12002 + */ + +/********************************************************************** + * NAME: accelgyro.h * + * AUTHOR: rsd12002 * + * PURPOSE: Set up and read from the gyroscope and accelerometer * + * INFORMATION: * + * How to use this driver is explained in page 810 of HAL driver * + * Enable the SPI clock * + * Enable the clock for the SPI GPIOs * + * Configure the MISO, MOSI, SCK pins as alternate function push-pull * + * Program the Mode, Direction, Data size, Baudrate Prescaler, * + * NSS management, Clock polarity and phase, FirstBit and * + * CRC configuration * + * * + * GLOBAL VARIABLES: * + * Variable Type Description * + * -------- ---- ----------- * + * * + **********************************************************************/ + +#ifndef DRIVERS_ACCEL_GYRO_H_ +#define DRIVERS_ACCEL_GYRO_H_ + +#include +#include "stm32f4xx.h" +#include "stm32f4xx_revo.h" + +#define MPU6000_CONFIG 0x1A + +// Bits +#define MPU_CLK_SEL_PLLGYROZ 0x03 +#define BITS_FS_250DPS 0x00 +#define BITS_FS_500DPS 0x08 +#define BITS_FS_1000DPS 0x10 +#define BITS_FS_2000DPS 0x18 +#define BITS_FS_2G 0x00 +#define BITS_FS_4G 0x08 +#define BITS_FS_8G 0x10 +#define BITS_FS_16G 0x18 +#define BITS_FS_MASK 0x18 +#define BITS_DLPF_CFG_256HZ 0x00 +#define BITS_DLPF_CFG_188HZ 0x01 +#define BITS_DLPF_CFG_98HZ 0x02 +#define BITS_DLPF_CFG_42HZ 0x03 +#define BITS_DLPF_CFG_20HZ 0x04 +#define BITS_DLPF_CFG_10HZ 0x05 +#define BITS_DLPF_CFG_5HZ 0x06 +#define BIT_I2C_IF_DIS 0x10 +#define BIT_H_RESET 0x80 +#define BIT_FIFO_RESET 0x04 +#define BIT_GYRO 3 +#define BIT_ACC 2 + +#define MPU6000_WHO_AM_I_CONST 0x68 + +// RA = Register Address +#define MPU_RA_PRODUCT_ID 0x0C +#define MPU_RA_SMPLRT_DIV 0x19 +#define MPU_RA_CONFIG 0x1A +#define MPU_RA_GYRO_CONFIG 0x1B +#define MPU_RA_ACCEL_CONFIG 0x1C +#define MPU_RA_FIFO_EN 0x23 +#define MPU_RA_ACCEL_XOUT_H 0x3B +#define MPU_RA_ACCEL_XOUT_L 0x3C +#define MPU_RA_ACCEL_YOUT_H 0x3D +#define MPU_RA_ACCEL_YOUT_L 0x3E +#define MPU_RA_ACCEL_ZOUT_H 0x3F +#define MPU_RA_ACCEL_ZOUT_L 0x40 +#define MPU_RA_GYRO_XOUT_H 0x43 +#define MPU_RA_GYRO_XOUT_L 0x44 +#define MPU_RA_GYRO_YOUT_H 0x45 +#define MPU_RA_GYRO_YOUT_L 0x46 +#define MPU_RA_GYRO_ZOUT_H 0x47 +#define MPU_RA_GYRO_ZOUT_L 0x48 +#define MPU_RA_SIGNAL_PATH_RESET 0x68 +#define MPU_RA_USER_CTRL 0x6A +#define MPU_RA_PWR_MGMT_1 0x6B +#define MPU_RA_PWR_MGMT_2 0x6C +#define MPU_RA_FIFO_COUNTH 0x72 +#define MPU_RA_FIFO_COUNTL 0x73 +#define MPU_RA_FIFO_R_W 0x74 +#define MPU_RA_WHO_AM_I 0x75 + +// Product ID Description for MPU6000 +#define MPU6000ES_REV_C4 0x14 +#define MPU6000ES_REV_C5 0x15 +#define MPU6000ES_REV_D6 0x16 +#define MPU6000ES_REV_D7 0x17 +#define MPU6000ES_REV_D8 0x18 +#define MPU6000_REV_C4 0x54 +#define MPU6000_REV_C5 0x55 +#define MPU6000_REV_D6 0x56 +#define MPU6000_REV_D7 0x57 +#define MPU6000_REV_D8 0x58 +#define MPU6000_REV_D9 0x59 +#define MPU6000_REV_D10 0x5A + +#define YAW_ROT_0 + +typedef struct gyro_t { + int16_t gyroX, gyroY, gyroZ; /* Gyroscope data converted into °/s */ + int16_t offsetX, offsetY, offsetZ; /* Gyroscope offset values */ + float scale; /* Scale factor */ +} gyro_t; + +typedef struct accel_t { + float accelXconv, accelYconv, accelZconv; /* Converted accelerometer data into G (9.82 m/s^2) */ + int16_t accelXraw, accelYraw, accelZraw; /* Raw accelerometer data */ + int16_t offsetX, offsetY, offsetZ; /* Accelerometer offset raw values */ + uint16_t accel1G; /* Sensitivity factor */ +} accel_t; + +/*********************************************************************** + * BRIEF: SPI1_Init initializes the SPI1 instance with predefined values* + * INFORMATION: * + * Mode = SPI_MODE_MASTER; * + * Direction = SPI_DIRECTION_2LINES; * + * DataSize = SPI_DATASIZE_8BIT; * + * CLKPolarity = SPI_POLARITY_LOW; * + * CLKPhase = SPI_PHASE_1EDGE; * + * NSS = SPI_NSS_HARD_OUTPUT; * + * BaudRatePrescaler = SPI_BAUDRATEPRESCALER_128; * + * FirstBit = SPI_FIRSTBIT_MSB; * + * TIMode = SPI_TIMODE_DISABLE; * + * CRCCalculation = SPI_CRCCALCULATION_DISABLED; * + ***********************************************************************/ +HAL_StatusTypeDef spi1_init(SPI_HandleTypeDef* hspi); + +/*********************************************************************** + * BRIEF: mpu6000_Init initializes the gyroscope and accelerometer * + * INFORMATION: * + * Utilizing the GyroZ clock * + * I2C bus disabled * + * Sample rate division = 0 * + * 256Hz DLPF * + * Full scale range of the gyroscope = ± 2000°/s * + ***********************************************************************/ +HAL_StatusTypeDef mpu6000_init(SPI_HandleTypeDef *hspi, gyro_t* gyro, accel_t* accel); + +/*********************************************************************** + * BRIEF: mpu6000_ReadGyro reads the three axis of the gyroscope and * + * stores the data, in °/s format, in the gyro struct * + * INFORMATION: * + ***********************************************************************/ +HAL_StatusTypeDef mpu6000_read_gyro(SPI_HandleTypeDef *hspi, gyro_t* gyro); + +/*********************************************************************** + * BRIEF: mpu6000_ReadGyro reads the three axis of the accelerometer * + * INFORMATION: * + * The data is both saved in raw format and in converted into the * + * number of Gs (9.82 m/s^2) the accelerometer is sensing * + ***********************************************************************/ +HAL_StatusTypeDef mpu6000_read_accel(SPI_HandleTypeDef *hspi, accel_t* accel); + +/*********************************************************************** + * BRIEF: mpu6000_ReadFIFO read the X, Y, and Z gyro axis from the * + * FIFO queue * + * INFORMATION: * + * Reads every complete set of gyro data (6 bytes) from the queue and * + * stores it it data_out * + * returns: * + * -1 if SPI transmission error occurs * + * -2 if FIFO queue overflow * + * -3 if FIFO queue doesn't contain any complete set of gyro data * + * else the number of bytes read from the FIFO queue * + ***********************************************************************/ +uint16_t mpu6000_read_fifo(SPI_HandleTypeDef* hspi, gyro_t* gyro, int16_t* data_out); + +/*********************************************************************** + * BRIEF: mpu6000_WhoAmI requests the product ID of the mpu6000 to * + * confirm device * + * INFORMATION: * + * returns true if correct device if found * + ***********************************************************************/ +bool mpu6000_who_am_i(SPI_HandleTypeDef *hspi); + +#endif /* DRIVERS_ACCEL_GYRO_H_ */ diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c new file mode 100644 index 0000000..be17729 --- /dev/null +++ b/UAV-ControlSystem/src/drivers/accel_gyro.c @@ -0,0 +1,553 @@ +/* + * gyro.c + * + * Created on: 16 sep. 2016 + * Author: rsd12002 + */ + +#include + +/*********************************************************************** + * BRIEF: SPI1_Init initializes the SPI1 instance with predefined values* + * INFORMATION: * + * Mode = SPI_MODE_MASTER; * + * Direction = SPI_DIRECTION_2LINES; * + * DataSize = SPI_DATASIZE_8BIT; * + * CLKPolarity = SPI_POLARITY_LOW; * + * CLKPhase = SPI_PHASE_1EDGE; * + * NSS = SPI_NSS_HARD_OUTPUT; * + * BaudRatePrescaler = SPI_BAUDRATEPRESCALER_128; * + * FirstBit = SPI_FIRSTBIT_MSB; * + * TIMode = SPI_TIMODE_DISABLE; * + * CRCCalculation = SPI_CRCCALCULATION_DISABLED; * + ***********************************************************************/ +HAL_StatusTypeDef spi1_init(SPI_HandleTypeDef* hspi) +{ + HAL_StatusTypeDef status; + + hspi->Instance = MPU6000_SPI_INSTANCE; + hspi->Init.Mode = SPI_MODE_MASTER; + hspi->Init.Direction = SPI_DIRECTION_2LINES; + hspi->Init.DataSize = SPI_DATASIZE_8BIT; + hspi->Init.CLKPolarity = SPI_POLARITY_LOW; + hspi->Init.CLKPhase = SPI_PHASE_1EDGE; + hspi->Init.NSS = SPI_NSS_HARD_OUTPUT; + /* mpu6000 SCLK Clock Frequency max 1 MHz */ + hspi->Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_128; + hspi->Init.FirstBit = SPI_FIRSTBIT_MSB; + hspi->Init.TIMode = SPI_TIMODE_DISABLE; + hspi->Init.CRCCalculation = SPI_CRCCALCULATION_DISABLED; + + HAL_SPI_MspInit(hspi); + status = HAL_SPI_Init(hspi); + return status; +} + +/*********************************************************************** + * BRIEF: HAL_SPI_MspInit initializes the SPI1 clock and GPIO pins used * + * INFORMATION: * + * Is called automatically * + ***********************************************************************/ +void HAL_SPI_MspInit(SPI_HandleTypeDef *hspi) +{ + if(__SPI1_IS_CLK_DISABLED()) + __SPI1_CLK_ENABLE(); + + /**SPI1 GPIO Configuration + PA7 ------> SPI1_MOSI + PA6 ------> SPI1_MISO + PA5 ------> SPI1_SCK + PA4 ------> SPI1_NSS + */ + + GPIO_InitTypeDef gpioInit; + + gpioInit.Pin = GPIO_PIN_7 | GPIO_PIN_6 | GPIO_PIN_5; + gpioInit.Mode = GPIO_MODE_AF_PP; + gpioInit.Pull = GPIO_NOPULL; + gpioInit.Speed = GPIO_SPEED_LOW; + gpioInit.Alternate = GPIO_AF5_SPI1; + HAL_GPIO_Init(GPIOA, &gpioInit); + + gpioInit.Pin = GPIO_PIN_4; + gpioInit.Mode = GPIO_MODE_OUTPUT_PP; + gpioInit.Pull = GPIO_PULLUP; + gpioInit.Speed = GPIO_SPEED_LOW; + HAL_GPIO_Init(GPIOA, &gpioInit); +} + +/*********************************************************************** + * BRIEF: mpu6000_Transmit transmits the specified register and data * + * to the mpu6000 * + * INFORMATION: * + * data[0] = register * + * data[1] = command * + ***********************************************************************/ +HAL_StatusTypeDef mpu6000_transmit(SPI_HandleTypeDef *hspi, uint8_t* data) +{ + HAL_StatusTypeDef err; /* SPI transmission status variable */ + + HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_RESET); + err = HAL_SPI_Transmit(hspi, data, 2, 10); + HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_SET); + + HAL_Delay(1); + return err; +} + +/*********************************************************************** + * BRIEF: mpu6000_TransmitReceive is used to request data from the * + * mpu6000 which it stores in data * + * INFORMATION: * + ***********************************************************************/ +HAL_StatusTypeDef mpu6000_transmit_receive(SPI_HandleTypeDef *hspi, uint8_t reg, uint8_t* data, uint8_t length) +{ + HAL_StatusTypeDef err; /* SPI transmission status variable */ + + uint8_t tmpData[length+1]; /* Temporary data variable */ + tmpData[0] = reg | 0x80; /* Flips the request for data bit */ + + HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_RESET); + err = HAL_SPI_TransmitReceive(hspi, tmpData, tmpData, length+1, 10); + HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_SET); + if(err != HAL_OK) + return err; + + HAL_Delay(1); + + if(length == 1) + { + *data = tmpData[1]; + } + else + { + for(int i = 1; i < length; i++) + { + data[i-1] = tmpData[i]; + } + } + return err; +} + +/*********************************************************************** + * BRIEF: mpu6000_read_offset reads and returns the offset of the * + * gyroscope and accelerometer * + * INFORMATION: * + * Is automatically called when mpu6000_init is called * + * The flight controller needs to be stationary when this function is * + * called * + * When the UAV is finished this data could be saved so that the * + * offset doesn't need to be read every time * + ***********************************************************************/ +HAL_StatusTypeDef mpu6000_read_offset(SPI_HandleTypeDef *hspi, gyro_t* gyro, accel_t* accel) +{ + uint8_t dataG[6]; /* Temporary data variable used to receive gyroscope data */ + uint8_t dataA[6]; /* Temporary data variable used to receive accelerometer data */ + HAL_StatusTypeDef err; /* SPI transmission status variable */ + + err = mpu6000_transmit_receive(hspi, MPU_RA_ACCEL_XOUT_H, dataA, 6); + if(err != HAL_OK) + return err; + + err = mpu6000_transmit_receive(hspi, MPU_RA_GYRO_XOUT_H, dataG, 6); + if(err != HAL_OK) + return err; + +#ifdef YAW_ROT_0 + gyro->offsetX = -(((int16_t)dataG[0] << 8) | dataG[1]); + gyro->offsetY = -(((int16_t)dataG[2] << 8) | dataG[3]); + gyro->offsetZ = (((int16_t)dataG[4] << 8) | dataG[5]); + + accel->offsetX = -((int16_t)dataA[0] << 8) | dataA[1]; + accel->offsetY = -((int16_t)dataA[2] << 8) | dataA[3]; + accel->offsetZ = accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]); +#elif defined(YAW_ROT_90) + gyro->offsetX = -(((int16_t)dataG[2] << 8) | dataG[3]); + gyro->offsetY = (((int16_t)dataG[0] << 8) | dataG[1]); + gyro->offsetZ = (((int16_t)dataG[4] << 8) | dataG[5]); + + accel->offsetX = -((int16_t)dataA[2] << 8) | dataA[3]; + accel->offsetY = ((int16_t)dataA[0] << 8) | dataA[1]; + accel->offsetZ = accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]); +#elif defined(YAW_ROT_180) + gyro->offsetX = (((int16_t)dataG[0] << 8) | dataG[1]); + gyro->offsetY = (((int16_t)dataG[2] << 8) | dataG[3]); + gyro->offsetZ = (((int16_t)dataG[4] << 8) | dataG[5]); + + accel->offsetX = ((int16_t)dataA[0] << 8) | dataA[1]; + accel->offsetY = ((int16_t)dataA[2] << 8) | dataA[3]; + accel->offsetZ = accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]); +#elif defined(YAW_ROT_270) + gyro->offsetX = (((int16_t)dataG[2] << 8) | dataG[3]); + gyro->offsetY = -(((int16_t)dataG[0] << 8) | dataG[1]); + gyro->offsetZ = (((int16_t)dataG[4] << 8) | dataG[5]); + + accel->offsetX = ((int16_t)dataA[2] << 8) | dataA[3]; + accel->offsetY = -((int16_t)dataA[0] << 8) | dataA[1]; + accel->offsetZ = accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]); +#endif + + return err; +} + +/*********************************************************************** + * BRIEF: mpu6000_Init initializes the gyroscope and accelerometer * + * INFORMATION: * + * Utilizing the GyroZ clock * + * I2C bus disabled * + * Sample rate division = 0 * + * 256Hz Digital Low Pass Filter (DLPF) * + * Full scale range of the gyroscope = ± 2000°/s * + ***********************************************************************/ +HAL_StatusTypeDef mpu6000_init(SPI_HandleTypeDef *hspi, gyro_t* gyro, accel_t* accel) +{ + HAL_StatusTypeDef err; /* SPI transmission status variable */ + uint8_t reg[2]; /* Register address and bit selection */ + + // Reset device + reg[0] = MPU_RA_PWR_MGMT_1; + reg[1] = BIT_H_RESET; + err = mpu6000_transmit(hspi, reg); + if(err != HAL_OK) + return err; + + // Reset Signal Path + HAL_Delay(10); + reg[0] = MPU_RA_SIGNAL_PATH_RESET; + reg[1] = BIT_GYRO | BIT_ACC; + err = mpu6000_transmit(hspi, reg); + HAL_Delay(150); + if(err != HAL_OK) + return err; + + // Wake up device and select GyroZ clock (better performance) + reg[0] = MPU_RA_PWR_MGMT_1; + reg[1] = 0b00001000 | MPU_CLK_SEL_PLLGYROZ; + err = mpu6000_transmit(hspi, reg); + if(err != HAL_OK) + return err; + + // Disable I2C bus + reg[0] = MPU_RA_USER_CTRL; + reg[1] = 0x50; + err = mpu6000_transmit(hspi, reg); + if(err != HAL_OK) + return err; + + // No standby mode + reg[0] = MPU_RA_PWR_MGMT_2; + reg[1] = 0x00; + err = mpu6000_transmit(hspi, reg); + if(err != HAL_OK) + return err; + + // Sample rate + reg[0] = MPU_RA_SMPLRT_DIV; + reg[1] = 0x00; + err = mpu6000_transmit(hspi, reg); + if(err != HAL_OK) + return err; + + // Digital Low Pass Filter (DLPF) + reg[0] = MPU_RA_CONFIG; + reg[1] = BITS_DLPF_CFG_256HZ; + err = mpu6000_transmit(hspi, reg); + if(err != HAL_OK) + return err; + + // FIFO + reg[0] = MPU_RA_FIFO_EN; + // Temperature GyroX GyroY GyroZ Accel Slave Slave Slave + reg[1] = 0 << 7 | 1 << 6 | 1 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0; + err = mpu6000_transmit(hspi, reg); + if(err != HAL_OK) + return err; + + // Gyroscope 2000DPS + reg[0] = MPU_RA_GYRO_CONFIG; + reg[1] = BITS_FS_2000DPS; + err = mpu6000_transmit(hspi, reg); + gyro->scale = (1.0f / 16.4f); + if(err != HAL_OK) + return err; + + // Accelerometer 16±G + reg[0] = MPU_RA_ACCEL_CONFIG; + reg[1] = BITS_FS_16G; + err = mpu6000_transmit(hspi, reg); + accel->accel1G = 2048; // (32768/16)/G + if(err != HAL_OK) + return err; + + mpu6000_read_offset(hspi, gyro, accel); + + return HAL_OK; +} + +/*********************************************************************** + * BRIEF: mpu6000_ReadGyro reads the three axis of the gyroscope and * + * stores the data, in °/s format, in the gyro struct * + * INFORMATION: * + ***********************************************************************/ +HAL_StatusTypeDef mpu6000_read_gyro(SPI_HandleTypeDef *hspi, gyro_t* gyro) +{ + uint8_t data[6]; /* Temporary data variable used to receive gyroscope data */ + HAL_StatusTypeDef err; /* SPI transmission status variable */ + + err = mpu6000_transmit_receive(hspi, MPU_RA_GYRO_XOUT_H, data, 6); + if(err != HAL_OK) + return err; + +#ifdef YAW_ROT_0 + gyro->gyroX = -(((int16_t)data[0] << 8) | data[1]); + gyro->gyroX = (gyro->gyroX - gyro->offsetX) * gyro->scale; + gyro->gyroY = -(((int16_t)data[2] << 8) | data[3]); + gyro->gyroY = (gyro->gyroY - gyro->offsetY) * gyro->scale; + gyro->gyroZ = (((int16_t)data[4] << 8) | data[5]); + gyro->gyroZ = (gyro->gyroZ - gyro->offsetZ) * gyro->scale; +#elif defined(YAW_ROT_90) + gyro->gyroX = -(((int16_t)data[2] << 8) | data[3]); + gyro->gyroX = (gyro->gyroX - gyro->offsetX) * gyro->scale; + gyro->gyroY = (((int16_t)data[0] << 8) | data[1]); + gyro->gyroY = (gyro->gyroY - gyro->offsetY) * gyro->scale; + gyro->gyroZ = (((int16_t)data[4] << 8) | data[5]); + gyro->gyroZ = (gyro->gyroZ - gyro->offsetZ) * gyro->scale; +#elif defined(YAW_ROT_180) + gyro->gyroX = (((int16_t)data[0] << 8) | data[1]); + gyro->gyroX = (gyro->gyroX - gyro->offsetX) * gyro->scale; + gyro->gyroY = (((int16_t)data[2] << 8) | data[3]); + gyro->gyroY = (gyro->gyroY - gyro->offsetY) * gyro->scale; + gyro->gyroZ = (((int16_t)data[4] << 8) | data[5]); + gyro->gyroZ = (gyro->gyroZ - gyro->offsetZ) * gyro->scale; +#elif defined(YAW_ROT_270) + gyro->gyroX = (((int16_t)data[2] << 8) | data[3]); + gyro->gyroX = (gyro->gyroX - gyro->offsetX) * gyro->scale; + gyro->gyroY = -(((int16_t)data[0] << 8) | data[1]); + gyro->gyroY = (gyro->gyroY - gyro->offsetY) * gyro->scale; + gyro->gyroZ = (((int16_t)data[4] << 8) | data[5]); + gyro->gyroZ = (gyro->gyroZ - gyro->offsetZ) * gyro->scale; +#else + No Yaw Direction Defined +#endif + + return err; +} + +/*********************************************************************** + * BRIEF: mpu6000_ReadGyro reads the three axis of the accelerometer * + * INFORMATION: * + * The data is both saved in raw format and in converted into the * + * number of G (9.82 m/s^2) the accelerometer is sensing * + ***********************************************************************/ +HAL_StatusTypeDef mpu6000_read_accel(SPI_HandleTypeDef *hspi, accel_t* accel) +{ + uint8_t data[6]; /* Temporary data variable used to receive accelerometer data */ + HAL_StatusTypeDef err; /* SPI transmission status variable */ + + err = mpu6000_transmit_receive(hspi, MPU_RA_ACCEL_XOUT_H, data, 6); + +#ifdef YAW_ROT_0 + accel->accelXraw = -((int16_t)data[0] << 8) | data[1]; + accel->accelXraw = accel->accelXraw - accel->offsetX; + accel->accelYraw = -((int16_t)data[2] << 8) | data[3]; + accel->accelYraw = accel->accelYraw - accel->offsetY; + accel->accelZraw = ((int16_t)data[4] << 8) | data[5]; + accel->accelZraw = accel->accelZraw + accel->offsetZ; + + accel->accelXconv = ((float)accel->accelXraw / accel->accel1G); + accel->accelYconv = ((float)accel->accelYraw / accel->accel1G); + accel->accelZconv = ((float)accel->accelZraw / accel->accel1G); +#elif defined(YAW_ROT_90) + accel->accelXraw = -((int16_t)data[2] << 8) | data[3]; + accel->accelXraw = accel->accelXraw - accel->offsetX; + accel->accelYraw = ((int16_t)data[0] << 8) | data[1]; + accel->accelYraw = accel->accelYraw - accel->offsetY; + accel->accelZraw = ((int16_t)data[4] << 8) | data[5]; + accel->accelZraw = accel->accelZraw + accel->offsetZ; + + accel->accelXconv = (float)(accel->accelYraw / accel->accel1G); + accel->accelYconv = (float)(accel->accelXraw / accel->accel1G); + accel->accelZconv = (float)(accel->accelZraw / accel->accel1G); +#elif defined(YAW_ROT_180) + accel->accelXraw = ((int16_t)data[0] << 8) | data[1]; + accel->accelXraw = accel->accelXraw - accel->offsetX; + accel->accelYraw = ((int16_t)data[2] << 8) | data[3]; + accel->accelYraw = accel->accelYraw - accel->offsetY; + accel->accelZraw = ((int16_t)data[4] << 8) | data[5]; + accel->accelZraw = accel->accelZraw + accel->offsetZ; + + accel->accelXconv = ((float)accel->accelXraw / accel->accel1G); + accel->accelYconv = ((float)accel->accelYraw / accel->accel1G); + accel->accelZconv = ((float)accel->accelZraw / accel->accel1G); +#elif defined(YAW_ROT_270) + accel->accelXraw = ((int16_t)data[2] << 8) | data[3]; + accel->accelXraw = accel->accelXraw - accel->offsetX; + accel->accelYraw = -((int16_t)data[0] << 8) | data[1]; + accel->accelYraw = accel->accelYraw - accel->offsetY; + accel->accelZraw = ((int16_t)data[4] << 8) | data[5]; + accel->accelZraw = accel->accelZraw + accel->offsetZ; + + accel->accelXconv = ((float)accel->accelYraw / accel->accel1G); + accel->accelYconv = ((float)accel->accelXraw / accel->accel1G); + accel->accelZconv = ((float)accel->accelZraw / accel->accel1G); +#endif + + return err; +} + +/*********************************************************************** + * BRIEF: mpu6000_ReadFIFO read the X, Y, and Z gyroscope axis from the * + * FIFO queue * + * INFORMATION: * + * Reads every complete set of gyro data (6 bytes) from the queue and * + * stores it it data_out * + * returns: * + * -1 if SPI transmission error occurs * + * -2 if FIFO queue overflow * + * -3 if FIFO queue doesn't contain any complete set of gyro data * + * else the number of bytes read from the FIFO queue * + ***********************************************************************/ +uint16_t mpu6000_read_fifo(SPI_HandleTypeDef* hspi, gyro_t* gyro, int16_t* data_out) +{ + uint16_t fifoCount = 0; /* Number of bytes in the FIFO queue */ + uint8_t countH = 0; /* Bits 8-16 of the number of bytes in the FIFO queue */ + uint8_t countL = 0; /* Bits 0-7 of the number of bytes in the FIFO queue */ + uint16_t bytesRead = 0; /* Number of bytes actually read from the FIFO queue */ + HAL_StatusTypeDef err = 0; /* SPI transmission status variable */ + uint8_t reg[2]; /* Register address and bit selection */ + + err = mpu6000_transmit_receive(hspi, MPU_RA_FIFO_COUNTH, &countH, 1); + if(err != HAL_OK) + return -1; + + err = mpu6000_transmit_receive(hspi, MPU_RA_FIFO_COUNTL, &countL, 1); + if(err != HAL_OK) + return -1; + + + fifoCount = (uint16_t)((countH << 8) | countL); + if (fifoCount == 1024) + { + reg[0] = MPU_RA_USER_CTRL; + reg[1] = BIT_FIFO_RESET; + mpu6000_transmit(hspi, reg); + return -2; + } + + if (fifoCount < 6) + return -3; + + /* Make sure that only complete sets of gyro data are read */ + bytesRead = (fifoCount/6)*6; + + uint8_t fifobuffer[bytesRead+1]; + + fifobuffer[0] = MPU_RA_FIFO_R_W | 0x80; + HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_RESET); + err = HAL_SPI_TransmitReceive(hspi, fifobuffer, fifobuffer, bytesRead+1, 10); + HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_SET); + + if(err != HAL_OK) + return -1; + + uint8_t xL, xH, yL, yH, zL, zH; + for(int j = 1; 6+((j-1)*6) < bytesRead+1; j++) + { + xH = fifobuffer[1+((j-1)*6)]; + xL = fifobuffer[2+((j-1)*6)]; + yH = fifobuffer[3+((j-1)*6)]; + yL = fifobuffer[4+((j-1)*6)]; + zH = fifobuffer[5+((j-1)*6)]; + zL = fifobuffer[6+((j-1)*6)]; + +#ifdef YAW_ROT_0 + + + data_out[0+((j-1)*3)] = -(((int16_t)xH << 8) | xL); // X + data_out[0+((j-1)*3)] = (data_out[0+((j-1)*3)] - gyro->offsetX) * gyro->scale; + + data_out[1+((j-1)*3)] = -(((int16_t)yH << 8) | yL); // Y + data_out[1+((j-1)*3)] = (data_out[1+((j-1)*3)] - gyro->offsetY) * gyro->scale; + + data_out[2+((j-1)*3)] = (((int16_t)zH << 8) | zL); // Z + data_out[2+((j-1)*3)] = (data_out[2+((j-1)*3)] - gyro->offsetZ) * gyro->scale; + +#elif defined(YAW_ROT_90) + data_out[0+((j-1)*3)] = -(((int16_t)yH << 8) | yL); // X + data_out[0+((j-1)*3)] = (data_out[0+((j-1)*3)] - gyro->offsetX) * gyro->scale; + + data_out[1+((j-1)*3)] = (((int16_t)xH << 8) | xL); // Y + data_out[1+((j-1)*3)] = (data_out[1+((j-1)*3)] - gyro->offsetY) * gyro->scale; + + data_out[2+((j-1)*3)] = (((int16_t)zH << 8) | zL); // Z + data_out[2+((j-1)*3)] = (data_out[2+((j-1)*3)] - gyro->offsetZ) * gyro->scale; +#elif defined(YAW_ROT_180) + data_out[0+((j-1)*3)] = (((int16_t)xH << 8) | xL); // X + data_out[0+((j-1)*3)] = (data_out[0+((j-1)*3)] - gyro->offsetX) * gyro->scale; + + data_out[1+((j-1)*3)] = (((int16_t)yH << 8) | yL); // Y + data_out[1+((j-1)*3)] = (data_out[1+((j-1)*3)] - gyro->offsetY) * gyro->scale; + + data_out[2+((j-1)*3)] = (((int16_t)zH << 8) | zL); // Z + data_out[2+((j-1)*3)] = (data_out[2+((j-1)*3)] - gyro->offsetZ) * gyro->scale; +#elif defined(YAW_ROT_270) + data_out[0+((j-1)*3)] = (((int16_t)yH << 8) | yL); // X + data_out[0+((j-1)*3)] = (data_out[0+((j-1)*3)] - gyro->offsetX) * gyro->scale; + + data_out[1+((j-1)*3)] = -(((int16_t)xH << 8) | xL); // Y + data_out[1+((j-1)*3)] = (data_out[1+((j-1)*3)] - gyro->offsetY) * gyro->scale; + + data_out[2+((j-1)*3)] = (((int16_t)zH << 8) | zL); // Z + data_out[2+((j-1)*3)] = (data_out[2+((j-1)*3)] - gyro->offsetZ) * gyro->scale; +#endif + } + return bytesRead; +} + +/*********************************************************************** + * BRIEF: mpu6000_WhoAmI requests the product ID of the mpu6000 to * + * confirm device * + * INFORMATION: * + * returns true if correct device and revision if found * + ***********************************************************************/ +bool mpu6000_who_am_i(SPI_HandleTypeDef *hspi) +{ + HAL_StatusTypeDef err; /* SPI transmission status variable */ + uint8_t data = 0; /* Received data is placed in this variable */ + + + err = mpu6000_transmit_receive(hspi, MPU_RA_WHO_AM_I, &data, 1); + if(err != HAL_OK) + return false; + + if (data != MPU6000_WHO_AM_I_CONST) + { + return false; + } + + + /* look for a product ID we recognize */ + err = mpu6000_transmit_receive(hspi, MPU_RA_PRODUCT_ID, &data, 1); + if(err != HAL_OK) + return false; + + // verify product revision + switch (data) + { + case MPU6000ES_REV_C4: + case MPU6000ES_REV_C5: + case MPU6000_REV_C4: + case MPU6000_REV_C5: + case MPU6000ES_REV_D6: + case MPU6000ES_REV_D7: + case MPU6000ES_REV_D8: + case MPU6000_REV_D6: + case MPU6000_REV_D7: + case MPU6000_REV_D8: + case MPU6000_REV_D9: + case MPU6000_REV_D10: + return true; + } + + return false; +}