From 912f7d0d3924c313adf97c4b8bb92daa713ebc39 Mon Sep 17 00:00:00 2001 From: Lennart Eriksson Date: Thu, 6 Oct 2016 09:00:00 +0200 Subject: [PATCH] mpu6000 fix with both SPI changes and accel and gyro working + fifo not crashing ever --- UAV-ControlSystem/inc/drivers/accel_gyro.h | 82 +++++-- UAV-ControlSystem/inc/drivers/spi.h | 78 +++++++ UAV-ControlSystem/inc/stm32f4xx_revo.h | 24 ++ UAV-ControlSystem/src/drivers/accel_gyro.c | 251 ++++++--------------- UAV-ControlSystem/src/drivers/spi.c | 199 ++++++++++++++++ 5 files changed, 435 insertions(+), 199 deletions(-) create mode 100644 UAV-ControlSystem/inc/drivers/spi.h create mode 100644 UAV-ControlSystem/src/drivers/spi.c diff --git a/UAV-ControlSystem/inc/drivers/accel_gyro.h b/UAV-ControlSystem/inc/drivers/accel_gyro.h index 43ecb2c..a5a39e5 100644 --- a/UAV-ControlSystem/inc/drivers/accel_gyro.h +++ b/UAV-ControlSystem/inc/drivers/accel_gyro.h @@ -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 ------------ + } + */ + diff --git a/UAV-ControlSystem/inc/drivers/spi.h b/UAV-ControlSystem/inc/drivers/spi.h new file mode 100644 index 0000000..da58f56 --- /dev/null +++ b/UAV-ControlSystem/inc/drivers/spi.h @@ -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_ */ diff --git a/UAV-ControlSystem/inc/stm32f4xx_revo.h b/UAV-ControlSystem/inc/stm32f4xx_revo.h index a47dab6..b562cae 100644 --- a/UAV-ControlSystem/inc/stm32f4xx_revo.h +++ b/UAV-ControlSystem/inc/stm32f4xx_revo.h @@ -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 diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c index be17729..cb5060e 100644 --- a/UAV-ControlSystem/src/drivers/accel_gyro.c +++ b/UAV-ControlSystem/src/drivers/accel_gyro.c @@ -6,74 +6,17 @@ */ #include +#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 diff --git a/UAV-ControlSystem/src/drivers/spi.c b/UAV-ControlSystem/src/drivers/spi.c new file mode 100644 index 0000000..cae9c0d --- /dev/null +++ b/UAV-ControlSystem/src/drivers/spi.c @@ -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); +}