228 lines
8.5 KiB
C
228 lines
8.5 KiB
C
/*
|
||
* 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_180
|
||
|
||
typedef struct gyro_t {
|
||
int16_t gyroX, gyroY, gyroZ; /* Gyroscope data converted into <20>/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 */
|
||
float rollAngle, pitchAngle;
|
||
} accel_t;
|
||
|
||
/***********************************************************************
|
||
* 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 = <20> 2000<30>/s *
|
||
***********************************************************************/
|
||
bool mpu6000_init(gyro_t* gyro, accel_t* accel);
|
||
|
||
/***********************************************************************
|
||
* 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);
|
||
|
||
/***********************************************************************
|
||
* 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 *
|
||
***********************************************************************/
|
||
bool mpu6000_read_accel(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 *
|
||
***********************************************************************/
|
||
int mpu6000_read_fifo(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();
|
||
|
||
void mpu6000_read_angle(accel_t* accel, gyro_t* gyro);
|
||
|
||
#endif /* DRIVERS_ACCEL_GYRO_H_ */
|
||
|
||
|
||
//---------------------------- GYRO FIFO EXAMPLE -------------------------
|
||
/*
|
||
int main(void)
|
||
{
|
||
//Init the system
|
||
init_system();
|
||
|
||
// Handle to the gyro and accelerometer types,
|
||
// These have information about starting positions so that scaling
|
||
// is handled in a good way
|
||
gyro_t g;
|
||
accel_t a;
|
||
|
||
// The usart handle to send data overt to look at
|
||
usart_profile usart;
|
||
|
||
// Initialize the usart
|
||
usart_init(USART1, &usart, 115200, STOP_BITS_1, PARITY_NONE);
|
||
|
||
// Initialize the mpu6000 module
|
||
mpu6000_init(&g, &a);
|
||
|
||
// The array to receive data to Maximum size is 1024
|
||
uint16_t data[1024] = {0x00};
|
||
|
||
while(1)
|
||
{
|
||
// Read the fifo queue, this returns the number of bytes read
|
||
int num = mpu6000_read_fifo(&g, data);
|
||
|
||
// if num returned is negative an error has occurred
|
||
if(num > 0)
|
||
usart_transmit(&usart, data, 6, 20000);
|
||
|
||
|
||
// -------------- ACCELEROMETER TEST ---------------
|
||
// mpu6000_read_accel(&a);
|
||
// data[0] = a.accelXraw >> 8;
|
||
// data[1] = a.accelXraw & 0xFF;
|
||
// usart_transmit(&usart, data, 2, 1000);
|
||
|
||
// data[0] = a.accelYraw >> 8;
|
||
// data[1] = a.accelYraw & 0xFF;
|
||
// usart_transmit(&usart, data, 2, 1000);
|
||
|
||
// data[0] = a.accelZraw >> 8;
|
||
// data[1] = a.accelZraw & 0xFF;
|
||
// usart_transmit(&usart, data, 2, 1000);
|
||
|
||
// HAL_Delay(100);
|
||
// --------------- END ACCELEROMETER TEST ------------
|
||
}
|
||
}
|
||
*/
|
||
|