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

mpu6000 fix with both SPI changes and accel and gyro working + fifo not crashing ever

Browse Source
This commit is contained in:
Lennart Eriksson 2016-10-06 09:00:00 +02:00
parent 4d3801239c
commit 912f7d0d39
5 changed files with 435 additions and 199 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

82
UAV-ControlSystem/inc/drivers/accel_gyro.h
View File

@ -116,22 +116,6 @@ typedef struct accel_t {
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: *
@ -141,14 +125,14 @@ HAL_StatusTypeDef spi1_init(SPI_HandleTypeDef* hspi);
* 256Hz DLPF *
* Full scale range of the gyroscope = ± 2000°/s *
***********************************************************************/
HAL_StatusTypeDef mpu6000_init(SPI_HandleTypeDef *hspi, gyro_t* gyro, accel_t* accel);
bool mpu6000_init(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);
bool mpu6000_read_gyro(gyro_t* gyro);
/***********************************************************************
* BRIEF: mpu6000_ReadGyro reads the three axis of the accelerometer *
@ -156,7 +140,7 @@ HAL_StatusTypeDef mpu6000_read_gyro(SPI_HandleTypeDef *hspi, gyro_t* gyro);
* 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);
bool mpu6000_read_accel(accel_t* accel);
/***********************************************************************
* BRIEF: mpu6000_ReadFIFO read the X, Y, and Z gyro axis from the *
@ -170,7 +154,7 @@ HAL_StatusTypeDef mpu6000_read_accel(SPI_HandleTypeDef *hspi, accel_t* accel);
* -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);
int mpu6000_read_fifo(gyro_t* gyro, int16_t* data_out);
/***********************************************************************
* BRIEF: mpu6000_WhoAmI requests the product ID of the mpu6000 to *
@ -178,6 +162,62 @@ uint16_t mpu6000_read_fifo(SPI_HandleTypeDef* hspi, gyro_t* gyro, int16_t* data_
* INFORMATION: *
* returns true if correct device if found *
***********************************************************************/
bool mpu6000_who_am_i(SPI_HandleTypeDef *hspi);
bool mpu6000_who_am_i();
#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, dataz);
// 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 ------------
}
*/

78
UAV-ControlSystem/inc/drivers/spi.h Normal file
View File

@ -0,0 +1,78 @@
/***************************************************************************
* NAME: spi.h *
* AUTHOR: Lennart Eriksson *
* PURPOSE: Set up SPI communication and enable sending and receiving data *
* INFORMATION: *
* The SPI is implemented using the spi_profile struct that contains all *
* necessary data for transmission and reception of data. *
* *
* GLOBAL VARIABLES: *
* Variable Type Description *
* -------- ---- ----------- *
* *
**************************************************************************/
#ifndef DRIVERS_SPI_H_
#define DRIVERS_SPI_H_
#include "stm32f4xx.h"
// The struct to be initialized and used when sending or receiving data
typedef struct spi_profile
{
SPI_HandleTypeDef *profile; // The SPI handle
GPIO_TypeDef *spi_nss_port; // The Slave Select Port
uint16_t spi_nss; //The Slave Select Pin
}spi_profile;
/****************************************************************************
* BRIEF: initializes a slave on the selected SPI instance *
* INFORMATION: Initialize a slave on the SPI. The SPI bus will only be *
* initialized once only per bus and if several slaves are used on the *
* same bus this function will return the struct with the pre initialized *
* SPI bus and the correct pin and port. *
***************************************************************************/
bool spi_init(SPI_TypeDef *spi_instance, // The spi instance to be used i.e. SPI1 or SPI3 on the REVO
spi_profile *out_profile, // The output profile to be used when sending and receiving data
uint32_t nss_pin, // The Slave Select Pin
GPIO_TypeDef *nss_port); // The port of the Slave Select Pin
/***********************************************************************
* BRIEF: transmit data to the selected slave over SPI *
* INFORMATION: *
* data[0] = register *
* data[1..n] = command *
***********************************************************************/
bool spi_transmit(spi_profile *profile, // The profile to send data over
uint8_t *data, // The data to be sent data[0] = register, data[1..n] = the command
uint32_t length, // The length of the data, including the register byte
uint32_t timeout_ms); // The timeout before giving up the try given in milliseconds
/***********************************************************************
* BRIEF: request data from selected slave on specified register *
* INFORMATION: By passing in a register we can request specified *
* data over the SPI bus *
***********************************************************************/
bool spi_receive_reg_value(spi_profile *profile, // The profile to send data over
uint8_t reg, // The register to get data from
uint8_t *data, // The output data buffer
uint32_t length, // The length of the buffer
uint32_t timeout_ms); // The timeout before giving up the try given in milliseconds
/***********************************************************************
* BRIEF: Receive data from the selected slave over SPI without *
* requesting from what register i.e. just listen if a slave has *
* something to say *
* INFORMATION: By passing in a register we can request specified *
* data over the SPI bus *
***********************************************************************/
bool spi_receive(spi_profile *profile, // The profile to send data over
uint8_t *data, // The output data buffer
uint32_t length, // The length of the buffer
uint32_t timeout_ms); // The timeout before giving up the try given in milliseconds
#endif /* DRIVERS_SPI_H_ */

24
UAV-ControlSystem/inc/stm32f4xx_revo.h
View File

@ -92,6 +92,28 @@
#define I2C2_PORT GPIOB
#define SPI1_MISO GPIO_PIN_6
#define SPI1_MISO_PORT GPIOA
#define SPI1_MOSI GPIO_PIN_7
#define SPI1_MOSI_PORT GPIOA
#define SPI1_SCK GPIO_PIN_5
#define SPI1_SCK_PORT GPIOA
#define SPI3_MISO GPIO_PIN_11
#define SPI3_MISO_PORT GPIOC
#define SPI3_MOSI GPIO_PIN_12
#define SPI3_MOSI_PORT GPIOC
#define SPI3_SCK GPIO_PIN_10
#define SPI3_SCK_PORT GPIOC
#define SPI3_NSS GPIO_PIN_15
#define SPI3_NSS_PORT GPIOA
/* Gyro */
#define GYRO
#define MPU6000_CS_PIN GPIO_PIN_4
@ -114,6 +136,8 @@
/* Baro */
//#define BARO
#define MPU6000_NSS_PIN GPIO_PIN_4
#define MPU6000_NSS_PORT GPIOA
/* Compass */
//#define COMPASS

251
UAV-ControlSystem/src/drivers/accel_gyro.c
View File

@ -6,74 +6,17 @@
*/
#include <drivers/accel_gyro.h>
#include "drivers/spi.h"
spi_profile mpu6000_spi_profile;
/***********************************************************************
* 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; *
* INFORMATION *
***********************************************************************/
HAL_StatusTypeDef spi1_init(SPI_HandleTypeDef* hspi)
bool spi1_init()
{
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);
return spi_init(SPI1, &mpu6000_spi_profile, MPU6000_NSS_PIN, MPU6000_NSS_PORT);
}
/***********************************************************************
@ -83,16 +26,9 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef *hspi)
* data[0] = register *
* data[1] = command *
***********************************************************************/
HAL_StatusTypeDef mpu6000_transmit(SPI_HandleTypeDef *hspi, uint8_t* data)
bool mpu6000_transmit(uint8_t* data, uint8_t length)
{
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;
return spi_transmit(&mpu6000_spi_profile, data, length, 10);
}
/***********************************************************************
@ -100,33 +36,9 @@ HAL_StatusTypeDef mpu6000_transmit(SPI_HandleTypeDef *hspi, uint8_t* data)
* mpu6000 which it stores in data *
* INFORMATION: *
***********************************************************************/
HAL_StatusTypeDef mpu6000_transmit_receive(SPI_HandleTypeDef *hspi, uint8_t reg, uint8_t* data, uint8_t length)
bool mpu6000_transmit_receive(uint8_t reg, uint8_t* data, uint8_t length, uint32_t timeout_ms)
{
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;
return spi_receive_reg_value(&mpu6000_spi_profile, reg, data, length, timeout_ms);
}
/***********************************************************************
@ -139,19 +51,16 @@ HAL_StatusTypeDef mpu6000_transmit_receive(SPI_HandleTypeDef *hspi, uint8_t reg,
* 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)
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 */
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;
if(!mpu6000_transmit_receive(MPU_RA_ACCEL_XOUT_H, dataA, 6, 10))
return HAL_ERROR;
err = mpu6000_transmit_receive(hspi, MPU_RA_GYRO_XOUT_H, dataG, 6);
if(err != HAL_OK)
return err;
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]);
@ -187,7 +96,7 @@ HAL_StatusTypeDef mpu6000_read_offset(SPI_HandleTypeDef *hspi, gyro_t* gyro, acc
accel->offsetZ = accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]);
#endif
return err;
return HAL_OK;
}
/***********************************************************************
@ -199,89 +108,80 @@ HAL_StatusTypeDef mpu6000_read_offset(SPI_HandleTypeDef *hspi, gyro_t* gyro, acc
* 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)
bool mpu6000_init(gyro_t* gyro, accel_t* accel)
{
HAL_StatusTypeDef err; /* SPI transmission status variable */
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;
err = mpu6000_transmit(hspi, reg);
if(err != HAL_OK)
return err;
if(!mpu6000_transmit(reg, 2))
return false;
HAL_Delay(10);
// 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;
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;
err = mpu6000_transmit(hspi, reg);
if(err != HAL_OK)
return err;
if(!mpu6000_transmit(reg, 2))
return false;
// Disable I2C bus
reg[0] = MPU_RA_USER_CTRL;
reg[1] = 0x50;
err = mpu6000_transmit(hspi, reg);
if(err != HAL_OK)
return err;
if(!mpu6000_transmit(reg, 2))
return false;
// No standby mode
reg[0] = MPU_RA_PWR_MGMT_2;
reg[1] = 0x00;
err = mpu6000_transmit(hspi, reg);
if(err != HAL_OK)
return err;
if(!mpu6000_transmit(reg, 2))
return false;
// Sample rate
reg[0] = MPU_RA_SMPLRT_DIV;
reg[1] = 0x00;
err = mpu6000_transmit(hspi, reg);
if(err != HAL_OK)
return err;
if(!mpu6000_transmit(reg, 2))
return false;
// 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;
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;
err = mpu6000_transmit(hspi, reg);
if(err != HAL_OK)
return err;
if(!mpu6000_transmit(reg, 2))
return false;
// 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;
if(!mpu6000_transmit(reg, 2))
return false;
// 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;
if(!mpu6000_transmit(reg, 2))
return false;
mpu6000_read_offset(hspi, gyro, accel);
mpu6000_read_offset(gyro, accel);
return HAL_OK;
return true;
}
/***********************************************************************
@ -289,14 +189,13 @@ HAL_StatusTypeDef mpu6000_init(SPI_HandleTypeDef *hspi, gyro_t* gyro, accel_t* a
* stores the data, in °/s format, in the gyro struct *
* INFORMATION: *
***********************************************************************/
HAL_StatusTypeDef mpu6000_read_gyro(SPI_HandleTypeDef *hspi, gyro_t* gyro)
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 */
err = mpu6000_transmit_receive(hspi, MPU_RA_GYRO_XOUT_H, data, 6);
if(err != HAL_OK)
return err;
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]);
@ -330,7 +229,7 @@ HAL_StatusTypeDef mpu6000_read_gyro(SPI_HandleTypeDef *hspi, gyro_t* gyro)
No Yaw Direction Defined
#endif
return err;
return true;
}
/***********************************************************************
@ -339,12 +238,12 @@ HAL_StatusTypeDef mpu6000_read_gyro(SPI_HandleTypeDef *hspi, gyro_t* gyro)
* 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)
bool mpu6000_read_accel(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);
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];
@ -392,7 +291,7 @@ HAL_StatusTypeDef mpu6000_read_accel(SPI_HandleTypeDef *hspi, accel_t* accel)
accel->accelZconv = ((float)accel->accelZraw / accel->accel1G);
#endif
return err;
return true;
}
/***********************************************************************
@ -407,21 +306,18 @@ HAL_StatusTypeDef mpu6000_read_accel(SPI_HandleTypeDef *hspi, accel_t* accel)
* -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)
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 */
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)
if(!mpu6000_transmit_receive(MPU_RA_FIFO_COUNTH, &countH, 1, 10))
return -1;
err = mpu6000_transmit_receive(hspi, MPU_RA_FIFO_COUNTL, &countL, 1);
if(err != HAL_OK)
if(!mpu6000_transmit_receive(MPU_RA_FIFO_COUNTL, &countL, 1, 10))
return -1;
@ -430,7 +326,13 @@ uint16_t mpu6000_read_fifo(SPI_HandleTypeDef* hspi, gyro_t* gyro, int16_t* data_
{
reg[0] = MPU_RA_USER_CTRL;
reg[1] = BIT_FIFO_RESET;
mpu6000_transmit(hspi, reg);
mpu6000_transmit(reg, 2);
reg[0] = MPU_RA_USER_CTRL;
reg[1] = 1<<6;
mpu6000_transmit(reg, 2);
return -2;
}
@ -442,23 +344,20 @@ uint16_t mpu6000_read_fifo(SPI_HandleTypeDef* hspi, gyro_t* gyro, int16_t* data_
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;
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 = 1; 6+((j-1)*6) < bytesRead+1; j++)
for(int j = 0; 6+((j-1)*6) < bytesRead; 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)];
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
@ -510,14 +409,11 @@ uint16_t mpu6000_read_fifo(SPI_HandleTypeDef* hspi, gyro_t* gyro, int16_t* data_
* INFORMATION: *
* returns true if correct device and revision if found *
***********************************************************************/
bool mpu6000_who_am_i(SPI_HandleTypeDef *hspi)
bool mpu6000_who_am_i()
{
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)
if(!mpu6000_transmit_receive(MPU_RA_WHO_AM_I, &data, 1, 10))
return false;
if (data != MPU6000_WHO_AM_I_CONST)
@ -527,8 +423,7 @@ bool mpu6000_who_am_i(SPI_HandleTypeDef *hspi)
/* look for a product ID we recognize */
err = mpu6000_transmit_receive(hspi, MPU_RA_PRODUCT_ID, &data, 1);
if(err != HAL_OK)
if(!mpu6000_transmit_receive(MPU_RA_PRODUCT_ID, &data, 1, 10))
return false;
// verify product revision

199
UAV-ControlSystem/src/drivers/spi.c Normal file
View File

@ -0,0 +1,199 @@
/*
* spi.c
*
* Created on: 3 okt. 2016
* Author: len12007
*/
#include "drivers/spi.h"
#include "stm32f4xx_revo.h"
// handles to SPI1 and SPI3
SPI_HandleTypeDef spi_channels[2];
bool spi_handel_initialized[2] = { false };
/****************************************************************************
* BRIEF: initializes a slave on the selected SPI instance *
* INFORMATION: Initialize a slave on the SPI. The SPI bus will only be *
* initialized once only per bus and if several slaves are used on the *
* same bus this function will return the struct with the pre initialized *
* SPI bus and the correct pin and port. *
***************************************************************************/
bool spi_init(SPI_TypeDef *spi_instance, // The spi instance to be used i.e. SPI1 or SPI3 on the REVO
spi_profile *out_profile, // The output profile to be used when sending and receiving data
uint32_t nss_pin, // The Slave Select Pin
GPIO_TypeDef *nss_port) // The port of the Slave Select Pin
{
uint32_t spi_af, spi_miso, spi_mosi, spi_sck;
GPIO_TypeDef *spi_miso_port;
uint8_t spi_handle;
out_profile->spi_nss = nss_pin;
out_profile->spi_nss_port = nss_port;
// Get the pins, ports and alternate functions for the seleceted SPI
if(spi_instance == SPI1)
{
spi_handle = 0; // The index to SPI1 bus to initialize and attache to the out_profile struct
spi_miso = SPI1_MISO;
spi_mosi = SPI1_MOSI;
spi_sck = SPI1_SCK;
spi_miso_port = SPI1_MISO_PORT;
spi_af = GPIO_AF5_SPI1;
// Start the spi clock if not already started
if(__SPI1_IS_CLK_DISABLED())
__SPI1_CLK_ENABLE();
}
else if(spi_instance == SPI3)
{
spi_handle = 1;
spi_miso = SPI3_MISO;
spi_mosi = SPI3_MOSI;
spi_sck = SPI3_SCK;
spi_af = GPIO_AF6_SPI3;
spi_miso_port = SPI3_MISO_PORT;
if(__SPI3_IS_CLK_DISABLED())
__SPI3_CLK_ENABLE();
}
else
return false; // If the given SPI Instance is not available on the REVO board
out_profile->profile = &spi_channels[spi_handle];
// Set up the NSS pin
GPIO_InitTypeDef gpioInit;
gpioInit.Pin = out_profile->spi_nss;
gpioInit.Mode = GPIO_MODE_OUTPUT_PP;
gpioInit.Pull = GPIO_PULLUP;
gpioInit.Speed = GPIO_SPEED_LOW;
gpioInit.Alternate = 0;
HAL_GPIO_Init(out_profile->spi_nss_port, &gpioInit);
// If the SPI bus is already initialized we can finish here
if(spi_handel_initialized[spi_handle])
return true;
// setup spi miso, mosi and clock pins
gpioInit.Pin = spi_miso | spi_mosi | spi_sck;
gpioInit.Mode = GPIO_MODE_AF_PP;
gpioInit.Pull = GPIO_NOPULL;
gpioInit.Speed = GPIO_SPEED_LOW;
gpioInit.Alternate = spi_af;
HAL_GPIO_Init(spi_miso_port, &gpioInit);
// Set up all parameters for the SPI communication
out_profile->profile->Instance = spi_instance;
out_profile->profile->Init.Mode = SPI_MODE_MASTER;
out_profile->profile->Init.Direction = SPI_DIRECTION_2LINES;
out_profile->profile->Init.DataSize = SPI_DATASIZE_8BIT;
out_profile->profile->Init.CLKPolarity = SPI_POLARITY_LOW;
out_profile->profile->Init.CLKPhase = SPI_PHASE_1EDGE;
out_profile->profile->Init.NSS = SPI_NSS_HARD_OUTPUT;
out_profile->profile->Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_128;
out_profile->profile->Init.FirstBit = SPI_FIRSTBIT_MSB;
out_profile->profile->Init.TIMode = SPI_TIMODE_DISABLE;
out_profile->profile->Init.CRCCalculation = SPI_CRCCALCULATION_DISABLED;
// Initialize the spi
if(HAL_SPI_Init(out_profile->profile) != HAL_OK)
return false;
// Set the initialized variable to true, this make sure that the SPI bus is
// only set up once.
spi_handel_initialized[spi_handle] = true;
return true;
}
/***********************************************************************
* BRIEF: transmit data to the selected slave over SPI *
* INFORMATION: *
* data[0] = register *
* data[1..n] = command *
***********************************************************************/
bool spi_transmit(spi_profile *profile,
uint8_t *data,
uint32_t length,
uint32_t timeout_ms)
{
HAL_StatusTypeDef err; /* SPI transmission status variable */
// HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_RESET);
// err = HAL_SPI_Transmit(profile->profile, data, 2, 10);
// HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_SET);
// Tell the selected slave that the data is intended for him/her (hen)
HAL_GPIO_WritePin(profile->spi_nss_port, profile->spi_nss, GPIO_PIN_RESET);
// Send the data
err = HAL_SPI_Transmit(profile->profile, data, length, timeout_ms);
// Tell the selected slave that we are done for now
HAL_GPIO_WritePin(profile->spi_nss_port, profile->spi_nss, GPIO_PIN_SET);
return (err == HAL_OK);
}
/***********************************************************************
* BRIEF: request data from selected slave on specified register *
* INFORMATION: By passing in a register we can request specified *
* data over the SPI bus *
***********************************************************************/
bool spi_receive_reg_value(spi_profile *profile,
uint8_t reg,
uint8_t *data,
uint32_t length,
uint32_t timeout_ms)
{
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 */
// Tell the selected slave that the data is intended for him/her (hen)
HAL_GPIO_WritePin(profile->spi_nss_port, profile->spi_nss, GPIO_PIN_RESET);
// Send request and receive the response
err = HAL_SPI_TransmitReceive(profile->profile, tmpData, tmpData, length + 1, timeout_ms);
// Tell the selected slave that we are done for now
HAL_GPIO_WritePin(profile->spi_nss_port, profile->spi_nss, GPIO_PIN_SET);
if(err != HAL_OK)
return false;
// copy the data to the output data buffer
for(int i = 1; i < length + 1; i++)
{
data[i-1] = tmpData[i];
}
return true;
}
/***********************************************************************
* BRIEF: Receive data from the selected slave over SPI without *
* requesting from what register i.e. just listen if a slave has *
* something to say *
* INFORMATION: By passing in a register we can request specified *
* data over the SPI bus *
***********************************************************************/
bool spi_receive(spi_profile *profile,
uint8_t *data,
uint32_t length,
uint32_t timeout_ms)
{
HAL_StatusTypeDef err; /* SPI transmission status variable */
// Tell the selected slave that the data is intended for him/her (hen)
HAL_GPIO_WritePin(profile->spi_nss_port, profile->spi_nss, GPIO_PIN_RESET);
// Receive data if available from the slave
err = HAL_SPI_Receive(profile->profile, data, length, timeout_ms);
// Tell the selected slave that we are done for now
HAL_GPIO_WritePin(profile->spi_nss_port, profile->spi_nss, GPIO_PIN_SET);
return (err == HAL_OK);
}