455 lines
17 KiB
C
455 lines
17 KiB
C
/*
|
||
* gyro.c
|
||
*
|
||
* Created on: 16 sep. 2016
|
||
* Author: rsd12002
|
||
*/
|
||
|
||
#include <drivers/accel_gyro.h>
|
||
#include "drivers/spi.h"
|
||
|
||
spi_profile mpu6000_spi_profile;
|
||
uint8_t num_failed_receive = 0;
|
||
|
||
/***********************************************************************
|
||
* BRIEF: SPI1_Init initializes the SPI1 instance with predefined values*
|
||
* INFORMATION *
|
||
***********************************************************************/
|
||
bool spi1_init()
|
||
{
|
||
return spi_init(SPI1, &mpu6000_spi_profile, MPU6000_NSS_PIN, MPU6000_NSS_PORT);
|
||
}
|
||
|
||
/***********************************************************************
|
||
* BRIEF: mpu6000_Transmit transmits the specified register and data *
|
||
* to the mpu6000 *
|
||
* INFORMATION: *
|
||
* data[0] = register *
|
||
* data[1] = command *
|
||
***********************************************************************/
|
||
bool mpu6000_transmit(uint8_t* data, uint8_t length)
|
||
{
|
||
return spi_transmit(&mpu6000_spi_profile, data, length, 10);
|
||
}
|
||
|
||
/***********************************************************************
|
||
* BRIEF: mpu6000_TransmitReceive is used to request data from the *
|
||
* mpu6000 which it stores in data *
|
||
* INFORMATION: *
|
||
***********************************************************************/
|
||
bool mpu6000_transmit_receive(uint8_t reg, uint8_t* data, uint8_t length, uint32_t timeout_ms)
|
||
{
|
||
return spi_receive_reg_value(&mpu6000_spi_profile, reg, data, length, timeout_ms);
|
||
}
|
||
|
||
/***********************************************************************
|
||
* 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(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 */
|
||
|
||
if(!mpu6000_transmit_receive(MPU_RA_ACCEL_XOUT_H, dataA, 6, 10))
|
||
return HAL_ERROR;
|
||
|
||
if(!mpu6000_transmit_receive(MPU_RA_GYRO_XOUT_H, dataG, 6, 10))
|
||
return HAL_ERROR;
|
||
|
||
#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 HAL_OK;
|
||
}
|
||
|
||
/***********************************************************************
|
||
* 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 = <20> 2000<30>/s *
|
||
***********************************************************************/
|
||
bool mpu6000_init(gyro_t* gyro, accel_t* accel)
|
||
{
|
||
spi1_init();
|
||
|
||
uint8_t reg[2]; /* Register address and bit selection */
|
||
|
||
// Reset device
|
||
reg[0] = MPU_RA_PWR_MGMT_1;
|
||
reg[1] = BIT_H_RESET;
|
||
if(!mpu6000_transmit(reg, 2))
|
||
return false;
|
||
HAL_Delay(10);
|
||
|
||
// Reset Signal Path
|
||
reg[0] = MPU_RA_SIGNAL_PATH_RESET;
|
||
reg[1] = BIT_GYRO | BIT_ACC;
|
||
HAL_Delay(150);
|
||
if(!mpu6000_transmit(reg, 2))
|
||
return false;
|
||
|
||
// Wake up device and select GyroZ clock (better performance)
|
||
reg[0] = MPU_RA_PWR_MGMT_1;
|
||
reg[1] = 0b00001000 | MPU_CLK_SEL_PLLGYROZ;
|
||
if(!mpu6000_transmit(reg, 2))
|
||
return false;
|
||
|
||
// Disable I2C bus
|
||
reg[0] = MPU_RA_USER_CTRL;
|
||
reg[1] = 0x50;
|
||
if(!mpu6000_transmit(reg, 2))
|
||
return false;
|
||
|
||
// No standby mode
|
||
reg[0] = MPU_RA_PWR_MGMT_2;
|
||
reg[1] = 0x00;
|
||
if(!mpu6000_transmit(reg, 2))
|
||
return false;
|
||
|
||
// Sample rate
|
||
reg[0] = MPU_RA_SMPLRT_DIV;
|
||
reg[1] = 0x00;
|
||
if(!mpu6000_transmit(reg, 2))
|
||
return false;
|
||
|
||
// Digital Low Pass Filter (DLPF)
|
||
reg[0] = MPU_RA_CONFIG;
|
||
reg[1] = BITS_DLPF_CFG_256HZ;
|
||
if(!mpu6000_transmit(reg, 2))
|
||
return false;
|
||
|
||
// 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;
|
||
if(!mpu6000_transmit(reg, 2))
|
||
return false;
|
||
|
||
// Gyroscope 2000DPS
|
||
reg[0] = MPU_RA_GYRO_CONFIG;
|
||
reg[1] = BITS_FS_2000DPS;
|
||
gyro->scale = (1.0f / 16.4f);
|
||
if(!mpu6000_transmit(reg, 2))
|
||
return false;
|
||
|
||
// Accelerometer 16<31>G
|
||
reg[0] = MPU_RA_ACCEL_CONFIG;
|
||
reg[1] = BITS_FS_16G;
|
||
accel->accel1G = 2048; // (32768/16)/G
|
||
if(!mpu6000_transmit(reg, 2))
|
||
return false;
|
||
|
||
mpu6000_read_offset(gyro, accel);
|
||
|
||
return true;
|
||
}
|
||
|
||
/***********************************************************************
|
||
* BRIEF: mpu6000_ReadGyro reads the three axis of the gyroscope and *
|
||
* stores the data, in <20>/s format, in the gyro struct *
|
||
* INFORMATION: *
|
||
***********************************************************************/
|
||
bool mpu6000_read_gyro(gyro_t* gyro)
|
||
{
|
||
uint8_t data[6]; /* Temporary data variable used to receive gyroscope data */
|
||
HAL_StatusTypeDef err; /* SPI transmission status variable */
|
||
|
||
if(!mpu6000_transmit_receive(MPU_RA_GYRO_XOUT_H, data, 6, 10))
|
||
return false;
|
||
|
||
#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 true;
|
||
}
|
||
|
||
/***********************************************************************
|
||
* 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 *
|
||
***********************************************************************/
|
||
bool mpu6000_read_accel(accel_t* accel)
|
||
{
|
||
uint8_t data[6]; /* Temporary data variable used to receive accelerometer data */
|
||
|
||
if(!mpu6000_transmit_receive(MPU_RA_ACCEL_XOUT_H, data, 6, 10))
|
||
return false;
|
||
|
||
#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 true;
|
||
}
|
||
|
||
/***********************************************************************
|
||
* 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 *
|
||
***********************************************************************/
|
||
int mpu6000_read_fifo(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 */
|
||
uint8_t reg[2]; /* Register address and bit selection */
|
||
|
||
if(!mpu6000_transmit_receive(MPU_RA_FIFO_COUNTH, &countH, 1, 10))
|
||
return -1;
|
||
|
||
if(!mpu6000_transmit_receive(MPU_RA_FIFO_COUNTL, &countL, 1, 10))
|
||
return -1;
|
||
|
||
|
||
fifoCount = (uint16_t)((countH << 8) | countL);
|
||
if (fifoCount == 1024 || num_failed_receive > 20)
|
||
{
|
||
reg[0] = MPU_RA_USER_CTRL;
|
||
reg[1] = BIT_FIFO_RESET;
|
||
|
||
mpu6000_transmit(reg, 2);
|
||
|
||
reg[0] = MPU_RA_USER_CTRL;
|
||
reg[1] = 1<<6;
|
||
|
||
mpu6000_transmit(reg, 2);
|
||
return -2;
|
||
}
|
||
|
||
if (fifoCount < 6)
|
||
{
|
||
num_failed_receive++;
|
||
return -3;
|
||
}
|
||
else
|
||
num_failed_receive = 0;
|
||
|
||
/* Make sure that only complete sets of gyro data are read */
|
||
bytesRead = (fifoCount/6)*6;
|
||
|
||
uint8_t fifobuffer[bytesRead+1];
|
||
|
||
|
||
if(!mpu6000_transmit_receive(MPU_RA_FIFO_R_W, fifobuffer, bytesRead, 20))
|
||
return false;
|
||
|
||
|
||
uint8_t xL, xH, yL, yH, zL, zH;
|
||
for(int j = 0; 6+((j-1)*6) < bytesRead; j++)
|
||
{
|
||
xH = fifobuffer[0+((j-1)*6)];
|
||
xL = fifobuffer[1+((j-1)*6)];
|
||
yH = fifobuffer[2+((j-1)*6)];
|
||
yL = fifobuffer[3+((j-1)*6)];
|
||
zH = fifobuffer[4+((j-1)*6)];
|
||
zL = fifobuffer[5+((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()
|
||
{
|
||
uint8_t data = 0; /* Received data is placed in this variable */
|
||
|
||
if(!mpu6000_transmit_receive(MPU_RA_WHO_AM_I, &data, 1, 10))
|
||
return false;
|
||
|
||
if (data != MPU6000_WHO_AM_I_CONST)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
|
||
/* look for a product ID we recognize */
|
||
if(!mpu6000_transmit_receive(MPU_RA_PRODUCT_ID, &data, 1, 10))
|
||
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;
|
||
}
|