philip/MDH-UAV-Control-System
Archived
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #12 from MDHSweden/Gyro

Browse Source 
Working SPI gyroacc, Approved by Lennart Eriksson
This commit is contained in:
Lennart Eriksson 2016-10-03 08:50:23 +02:00 committed by GitHub
commit 94a00d607d
2 changed files with 736 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

183
UAV-ControlSystem/inc/drivers/accel_gyro.h Normal file
View File

@ -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 <stdbool.h>
#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_ */

553
UAV-ControlSystem/src/drivers/accel_gyro.c Normal file
View File

@ -0,0 +1,553 @@
/*
* gyro.c
*
* Created on: 16 sep. 2016
* Author: rsd12002
*/
#include <drivers/accel_gyro.h>
/***********************************************************************
* 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;
}