598 lines
22 KiB
C
598 lines
22 KiB
C
/*
|
||
* gyro.c
|
||
*
|
||
* Created on: 16 sep. 2016
|
||
* Author: rsd12002
|
||
*/
|
||
|
||
#include <drivers/accel_gyro.h>
|
||
#include "drivers/spi.h"
|
||
#include "utilities.h"
|
||
#include "math.h"
|
||
#include "drivers/system_clock.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 *
|
||
* 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_gyro_offset(gyro_t* gyro)
|
||
{
|
||
uint8_t dataG[6]; /* Temporary data variable used to receive gyroscope data */
|
||
int16_t dataGCheck[6] = {0}; /* checkval */
|
||
uint8_t maxDrift = 5;
|
||
bool calibrate_go = true;
|
||
bool calibrate_ok = false;
|
||
int countCalibrate = 0;
|
||
|
||
//small delay so that we know the gyro should give some values
|
||
HAL_Delay(100);
|
||
for (int i = 0; i < 100; i++)
|
||
{
|
||
//assume that we should calibrate at the start of each loop
|
||
calibrate_go = true;
|
||
|
||
//get new values
|
||
if(!mpu6000_transmit_receive(MPU_RA_GYRO_XOUT_H, dataG, 6, 10))
|
||
return HAL_ERROR;
|
||
|
||
//cheack the new values and see if they are within acceptable range compared to the previous values, so that it is realativelly still
|
||
for(int j = 0; j < 6; j ++)
|
||
{
|
||
int16_t currentDrift = (int16_t)abs(dataGCheck[j] - dataG[j]);
|
||
if (!(currentDrift < maxDrift))
|
||
calibrate_go = false;
|
||
|
||
dataGCheck[j] = dataG[j];
|
||
}
|
||
|
||
//if true, we have good values exit loop
|
||
if (calibrate_go)
|
||
{
|
||
countCalibrate++;
|
||
}
|
||
if (countCalibrate > 4)
|
||
{
|
||
calibrate_ok = true;
|
||
break;
|
||
}
|
||
//wait for a little bit so the checks are not to fast
|
||
HAL_Delay(200);
|
||
}
|
||
|
||
|
||
#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]);
|
||
|
||
#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]);
|
||
#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]);
|
||
#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]);
|
||
#endif
|
||
|
||
return HAL_OK;
|
||
}
|
||
|
||
/***********************************************************************
|
||
* BRIEF: mpu6000_read_offset reads and returns the offset of the *
|
||
* 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_acc_offset(accel_t* accel)
|
||
{
|
||
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;
|
||
|
||
|
||
#ifdef YAW_ROT_0
|
||
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)
|
||
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)
|
||
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)
|
||
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_gyro_offset(gyro);
|
||
//mpu6000_read_acc_offset(accel);
|
||
|
||
HAL_Delay(60);
|
||
|
||
accel->pitchAngle = 0;
|
||
accel->rollAngle = 0;
|
||
|
||
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;
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
/* Set the Gyro Weight for Gyro/Acc complementary filter
|
||
Increasing this value would reduce and delay Acc influence on the output of the filter*/
|
||
#define GYRO_ACC_DIV_FACTOR (2^16) // that means a CMP_FACTOR of 1024 (2^10)
|
||
#define ACCEL_LPF_FACTOR 16
|
||
|
||
#define GetMagnitude(x) (x*x)
|
||
#define Low_Magnitude (GetMagnitude(0.85))
|
||
#define High_Magnitude (GetMagnitude(1.15))
|
||
|
||
/* Returns the angle. If magnitude of acc is out of range we calculate on integrated gyro data */
|
||
void mpu6000_read_angle(accel_t* accel, gyro_t* gyro)
|
||
{
|
||
static uint32_t last_micros = 0; // Static stores micros measured from last iteration
|
||
uint32_t current_micros = clock_get_us();
|
||
uint32_t delta_t = current_micros - last_micros;
|
||
last_micros = current_micros;
|
||
|
||
float deltaGyroAngleFloat[3] = {0};
|
||
static float lpf_Acc[3] = {0};
|
||
static float smooth[3] = {0};
|
||
float sign[3] = {0};
|
||
float magnitude = 0;
|
||
|
||
/* We read the accelerometer to get fresh data */
|
||
mpu6000_read_accel(accel);
|
||
float accelConv[3] = {accel->accelXconv, accel->accelYconv, accel->accelZconv};
|
||
|
||
/* Filter part, go thorugh each axis */
|
||
for (int i = 0; i < 3; i ++)
|
||
{
|
||
//Calculate a new smooth value based on a factor of the LPF value
|
||
smooth[i] = lpf_Acc[i] / ACCEL_LPF_FACTOR;
|
||
|
||
//Save the sign(+/-) of the value
|
||
sign[i] = (accelConv[i]< 0) ? -1 : 1;
|
||
|
||
//Calculate the new LPF value based on the raw sensor data - the smoothing value
|
||
lpf_Acc[i] += sign[i]*sqrtf(ABS_FLOAT(accelConv[i])) - smooth[i];
|
||
|
||
//recalculate the accelerometer data based on the smooth value, since we had to take the square root off the original value we square it to get in the original size
|
||
accelConv[i] = smooth[i] * smooth[i] * sign[i];
|
||
|
||
//calculate the magnitude of the gravitation for all axis
|
||
magnitude += ABS_FLOAT(accelConv[i]) * ABS_FLOAT(accelConv[i]);
|
||
|
||
}
|
||
|
||
//Calculate the approximate angle increase based on the gyros probable movement since the last invoke
|
||
deltaGyroAngleFloat[0] = (delta_t * (float)gyro->gyroX / 1000000.0);
|
||
deltaGyroAngleFloat[1] = (delta_t * (float)gyro->gyroY / 1000000.0) ;
|
||
deltaGyroAngleFloat[2] = (delta_t * (float)gyro->gyroZ / 1000000.0);
|
||
|
||
//First integrate the gyro and add that to the angle calculation
|
||
accel->rollAngle += deltaGyroAngleFloat[1];
|
||
accel->pitchAngle += deltaGyroAngleFloat[0];
|
||
|
||
//If the g forces of the accelerometer is within the given magnitude we will also add accelerometer data to the calculation
|
||
if (Low_Magnitude < magnitude && magnitude < High_Magnitude)
|
||
{
|
||
//Calculate the pure angle given by the accelerometer data
|
||
float a_RollAngle = -atan2(accelConv[0], sqrt(accelConv[1]*accelConv[1] + accelConv[2]*accelConv[2]))*180/M_PI;
|
||
float a_PitchAngle = atan2( accelConv[1], sqrt(accelConv[2]*accelConv[2] + accelConv[0]*accelConv[0]))*180/M_PI;
|
||
|
||
//Check how much the accelerometer angle differs from the current calculated ange with the gyro. Add calculated factor to the real angle value
|
||
|
||
accel->rollAngle += (a_RollAngle - accel->rollAngle) / GYRO_ACC_DIV_FACTOR;
|
||
accel->pitchAngle += (a_PitchAngle - accel->pitchAngle) / GYRO_ACC_DIV_FACTOR;
|
||
|
||
}
|
||
|
||
}
|