/* * gyro.c * * Created on: 16 sep. 2016 * Author: rsd12002 */ #include /*********************************************************************** * 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) { 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); } /*********************************************************************** * BRIEF: mpu6000_Transmit transmits the specified register and data * * to the mpu6000 * * INFORMATION: * * data[0] = register * * data[1] = command * ***********************************************************************/ HAL_StatusTypeDef mpu6000_transmit(SPI_HandleTypeDef *hspi, uint8_t* data) { 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; } /*********************************************************************** * BRIEF: mpu6000_TransmitReceive is used to request data from the * * mpu6000 which it stores in data * * INFORMATION: * ***********************************************************************/ HAL_StatusTypeDef mpu6000_transmit_receive(SPI_HandleTypeDef *hspi, uint8_t reg, uint8_t* data, uint8_t length) { 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; } /*********************************************************************** * BRIEF: mpu6000_read_offset reads and returns the offset of the * * gyroscope and accelerometer * * INFORMATION: * * Is automatically called when mpu6000_init is called * * The flight controller needs to be stationary when this function is * * called * * When the UAV is finished this data could be saved so that the * * offset doesn't need to be read every time * ***********************************************************************/ HAL_StatusTypeDef mpu6000_read_offset(SPI_HandleTypeDef *hspi, 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; err = mpu6000_transmit_receive(hspi, MPU_RA_GYRO_XOUT_H, dataG, 6); if(err != HAL_OK) return err; #ifdef YAW_ROT_0 gyro->offsetX = -(((int16_t)dataG[0] << 8) | dataG[1]); gyro->offsetY = -(((int16_t)dataG[2] << 8) | dataG[3]); gyro->offsetZ = (((int16_t)dataG[4] << 8) | dataG[5]); accel->offsetX = -((int16_t)dataA[0] << 8) | dataA[1]; accel->offsetY = -((int16_t)dataA[2] << 8) | dataA[3]; accel->offsetZ = accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]); #elif defined(YAW_ROT_90) gyro->offsetX = -(((int16_t)dataG[2] << 8) | dataG[3]); gyro->offsetY = (((int16_t)dataG[0] << 8) | dataG[1]); gyro->offsetZ = (((int16_t)dataG[4] << 8) | dataG[5]); accel->offsetX = -((int16_t)dataA[2] << 8) | dataA[3]; accel->offsetY = ((int16_t)dataA[0] << 8) | dataA[1]; accel->offsetZ = accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]); #elif defined(YAW_ROT_180) gyro->offsetX = (((int16_t)dataG[0] << 8) | dataG[1]); gyro->offsetY = (((int16_t)dataG[2] << 8) | dataG[3]); gyro->offsetZ = (((int16_t)dataG[4] << 8) | dataG[5]); accel->offsetX = ((int16_t)dataA[0] << 8) | dataA[1]; accel->offsetY = ((int16_t)dataA[2] << 8) | dataA[3]; accel->offsetZ = accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]); #elif defined(YAW_ROT_270) gyro->offsetX = (((int16_t)dataG[2] << 8) | dataG[3]); gyro->offsetY = -(((int16_t)dataG[0] << 8) | dataG[1]); gyro->offsetZ = (((int16_t)dataG[4] << 8) | dataG[5]); accel->offsetX = ((int16_t)dataA[2] << 8) | dataA[3]; accel->offsetY = -((int16_t)dataA[0] << 8) | dataA[1]; accel->offsetZ = accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]); #endif return err; } /*********************************************************************** * 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 = ± 2000°/s * ***********************************************************************/ HAL_StatusTypeDef mpu6000_init(SPI_HandleTypeDef *hspi, gyro_t* gyro, accel_t* accel) { HAL_StatusTypeDef err; /* SPI transmission status variable */ 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; // 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; // 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; // Disable I2C bus reg[0] = MPU_RA_USER_CTRL; reg[1] = 0x50; err = mpu6000_transmit(hspi, reg); if(err != HAL_OK) return err; // No standby mode reg[0] = MPU_RA_PWR_MGMT_2; reg[1] = 0x00; err = mpu6000_transmit(hspi, reg); if(err != HAL_OK) return err; // Sample rate reg[0] = MPU_RA_SMPLRT_DIV; reg[1] = 0x00; err = mpu6000_transmit(hspi, reg); if(err != HAL_OK) return err; // 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; // 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; // 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; // 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; mpu6000_read_offset(hspi, gyro, accel); return HAL_OK; } /*********************************************************************** * 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) { 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; #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 err; } /*********************************************************************** * 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 * ***********************************************************************/ HAL_StatusTypeDef mpu6000_read_accel(SPI_HandleTypeDef *hspi, 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); #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 err; } /*********************************************************************** * 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 * ***********************************************************************/ uint16_t mpu6000_read_fifo(SPI_HandleTypeDef* hspi, 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) return -1; err = mpu6000_transmit_receive(hspi, MPU_RA_FIFO_COUNTL, &countL, 1); if(err != HAL_OK) return -1; fifoCount = (uint16_t)((countH << 8) | countL); if (fifoCount == 1024) { reg[0] = MPU_RA_USER_CTRL; reg[1] = BIT_FIFO_RESET; mpu6000_transmit(hspi, reg); return -2; } if (fifoCount < 6) return -3; /* Make sure that only complete sets of gyro data are read */ bytesRead = (fifoCount/6)*6; 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; uint8_t xL, xH, yL, yH, zL, zH; for(int j = 1; 6+((j-1)*6) < bytesRead+1; 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)]; #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(SPI_HandleTypeDef *hspi) { 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) return false; if (data != MPU6000_WHO_AM_I_CONST) { return false; } /* look for a product ID we recognize */ err = mpu6000_transmit_receive(hspi, MPU_RA_PRODUCT_ID, &data, 1); if(err != HAL_OK) 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; }