diff --git a/Firmware/src/drivers/i2c.c b/Firmware/src/drivers/i2c.c index 8cdc3dc..ff12026 100644 --- a/Firmware/src/drivers/i2c.c +++ b/Firmware/src/drivers/i2c.c @@ -1,262 +1,262 @@ /* * Copyright (C) 2017 EPFL-LSRO (Laboratoire de Systemes Robotiques). * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "i2c.h" #include "stm32f4xx_i2c.h" #include "../lib/utils.h" -const uint32_t TIMEOUT = 3200; ///< Approximate maximum time for a I2C operation to complete (approx. 100us). +const uint32_t TIMEOUT = 1000; ///< Approximate maximum time for a I2C operation to complete (approx. 100us). /** * @brief Repeats until the expression returns false, or the timeout is reached. */ #define WAIT_WITH_TIMEOUT(x) \ do \ { \ uint32_t duration = 0;\ while((x)) \ { \ if(duration++ > TIMEOUT) \ { \ i2c_Reset(); \ if(ok != NULL) \ *ok = false; \ return; \ } \ } \ } while(0) /** * @brief Initializes the I2C bus. */ void i2c_Init(void) { GPIO_InitTypeDef GPIO_InitStruct; I2C_InitTypeDef I2C_InitStruct; // Enable the peripheral clock. RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE); // Setup the pins as I2C SDA/SCL. GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF; GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStruct.GPIO_OType = GPIO_OType_OD; GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL; GPIO_InitStruct.GPIO_Pin = I2C_SDA_Pin; GPIO_Init(I2C_SDA_Port, &GPIO_InitStruct); GPIO_InitStruct.GPIO_Pin = I2C_SCL_Pin; GPIO_Init(I2C_SCL_Port, &GPIO_InitStruct); GPIO_PinAFConfig(I2C_SDA_Port, I2C_SDA_PinSource, I2C_GPIO_AF); GPIO_PinAFConfig(I2C_SCL_Port, I2C_SCL_PinSource, I2C_GPIO_AF); // Setup the I2C peripheral. - I2C_InitStruct.I2C_ClockSpeed = 100000; // [Hz]. + I2C_InitStruct.I2C_ClockSpeed = 400000; // [Hz]. I2C_InitStruct.I2C_Mode = I2C_Mode_I2C; I2C_InitStruct.I2C_DutyCycle = I2C_DutyCycle_2; I2C_InitStruct.I2C_OwnAddress1 = 0x00; I2C_InitStruct.I2C_Ack = I2C_Ack_Enable; I2C_InitStruct.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; I2C_DeInit(I2C_PERIPH); I2C_Init(I2C_PERIPH, &I2C_InitStruct); I2C_Cmd(I2C_PERIPH, ENABLE); // utils_DelayUs(50); I2C_GenerateSTART(I2C_PERIPH, ENABLE); utils_DelayUs(50); I2C_GenerateSTART(I2C_PERIPH, DISABLE); utils_DelayUs(50); I2C_GenerateSTOP(I2C_PERIPH, ENABLE); utils_DelayUs(50); I2C_GenerateSTOP(I2C_PERIPH, DISABLE); utils_DelayUs(50); } /** * @brief Performs a stop condition, to abort the current transaction. */ void i2c_Reset(void) { I2C_GenerateSTOP(I2C_PERIPH, ENABLE); utils_DelayUs(50); I2C_GenerateSTOP(I2C_PERIPH, DISABLE); utils_DelayUs(50); } /** * @brief Writes a 8-bit value to a register. * @param slaveAddress 7-bit I2C slave address. * @param registerAddress 8-bit register address. * @param registerValue value to write to the 8-bit register. * @param ok the value pointed will be set to true if the operation completed * successfully, or to false if an error occured. If a NULL pointer is given, * then it is ignored. */ void i2c_WriteRegister(uint8_t slaveAddress, uint8_t registerAddress, uint8_t registerValue, bool *ok) { i2c_WriteMultiBytesRegister(slaveAddress, registerAddress, ®isterValue, 1, ok); } /** * @brief Writes several bytes to a register. * @param slaveAddress 7-bit I2C slave address. * @param registerAddress 8-bit register address. * @param registerValue values array to write to the 8-bit register. * @param registerSize number of bytes to write. * @param ok the value pointed will be set to true if the operation completed * successfully, or to false if an error occured. If a NULL pointer is given, * then it is ignored. */ void i2c_WriteMultiBytesRegister(uint8_t slaveAddress, uint8_t registerAddress, uint8_t const *registerValue, uint8_t registerSize, bool *ok) { int i; // Wait until the I2C peripheral is ready. WAIT_WITH_TIMEOUT(I2C_GetFlagStatus(I2C_PERIPH, I2C_FLAG_BUSY)); // Send a start condition, and wait until the slave has acknowledged. I2C_GenerateSTART(I2C_PERIPH, ENABLE); WAIT_WITH_TIMEOUT(!I2C_CheckEvent(I2C_PERIPH, I2C_EVENT_MASTER_MODE_SELECT)); // Send slave address and write mode, and wait until the slave has // acknowledged. I2C_Send7bitAddress(I2C_PERIPH, slaveAddress<<1, I2C_Direction_Transmitter); WAIT_WITH_TIMEOUT(!I2C_CheckEvent(I2C_PERIPH, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)); // Write the register address. I2C_SendData(I2C_PERIPH, registerAddress); WAIT_WITH_TIMEOUT(!I2C_CheckEvent(I2C_PERIPH, I2C_EVENT_MASTER_BYTE_TRANSMITTED)); // Write the register content. for(i=0; i #include "i2c.h" #include "../lib/utils.h" #define GRAVITY_INTENSITY 9.81f -#define SINT16_MAX_F ((float)SHRT_MAX) +#define SINT16_MAX_F ((float32_t)SHRT_MAX) #define SLAVE_ADDRESS 0x68 ///< 0x68 if the AD0 pin is low, 0x69 otherwise. #define REG_CONFIG 0x1A ///< Configuration register. #define REG_GYRO_CONFIG 0x1B ///< Gyroscope configuration register. #define REG_ACCEL_CONFIG 0x1C ///< Accelerometer configuration register. #define REG_SMPLRT_DIV 0x19 ///< Sample rate divider register. #define REG_ACCEL_VALUES 0x3b ///< Accelerometer measurements register. #define REG_TEMP_VALUES 0x41 ///< Temperature measurements register. #define REG_GYRO_VALUES 0x43 ///< Gyroscope measurements register. #define REG_SIG_PATH_RST 0x68 ///< Signal path reset register. #define REG_USER_CTRL 0x6A ///< User control register. #define REG_POWER_MGMT_1 0x6B ///< Power management 1 register. #define REG_WHO_AM_I 0x75 ///< Who am I register. #define WHO_AM_I_VAL 0x68 ///< Expected value of the REG_WHO_AM_I register. /// Gets the actual range value from a mpu_AccelRange enum value. -const float MPU_ACCEL_RANGE_REG_TO_CONV_FACTOR[] = +const float32_t MPU_ACCEL_RANGE_REG_TO_CONV_FACTOR[] = { 2.0f / SINT16_MAX_F * GRAVITY_INTENSITY, 4.0f / SINT16_MAX_F * GRAVITY_INTENSITY, 8.0f / SINT16_MAX_F * GRAVITY_INTENSITY, 16.0f / SINT16_MAX_F * GRAVITY_INTENSITY }; /// Gets the actual range value from a mpu_GyroRange enum value. -const float MPU_GYRO_RANGE_REG_TO_CONV_FACTOR[] = +const float32_t MPU_GYRO_RANGE_REG_TO_CONV_FACTOR[] = { 250.0f / SINT16_MAX_F, 500.0f / SINT16_MAX_F, 1000.0f / SINT16_MAX_F, 2000.0f / SINT16_MAX_F }; bool mpu_initialized = false; ///< True if the MPU-6050 is initialized, false otherwise. -float accelFactor, ///< Converts a raw 16-bit integer value to an acceleration [m/s^2]. - gyroFactor; ///< Converts a raw 16-bit integer value to an angular speed [deg/s]. +float32_t accelFactor, ///< Converts a raw 16-bit integer value to an acceleration [m/s^2]. + gyroFactor; ///< Converts a raw 16-bit integer value to an angular speed [deg/s]. /** * @brief Initializes the MPU-6050 device. * @param accelRange accelerometer range. * @param gyroRange gyroscope range. * @param bandwidth bandwith of the accelerometer and gyroscope. * @return true if the initialization was successful, false otherwise. */ bool mpu_Init(mpu_AccelRange accelRange, mpu_GyroRange gyroRange, mpu_Bandwidth bandwidth) { uint16_t id; // accelFactor = MPU_ACCEL_RANGE_REG_TO_CONV_FACTOR[accelRange]; gyroFactor = MPU_GYRO_RANGE_REG_TO_CONV_FACTOR[gyroRange]; // Reset the MPU-6050. i2c_WriteRegister(SLAVE_ADDRESS, REG_POWER_MGMT_1, (1<<7)|(1<<6), NULL); // Device reset. utils_DelayMs(100); // Test the communication with the chip. id = i2c_ReadRegister(SLAVE_ADDRESS, REG_WHO_AM_I, NULL); if(id != WHO_AM_I_VAL) // Error. return false; // Reset the MPU-6050 again. i2c_WriteRegister(SLAVE_ADDRESS, REG_POWER_MGMT_1, (1<<7)|(1<<6), NULL); // Device reset. utils_DelayMs(100); i2c_WriteRegister(SLAVE_ADDRESS, REG_SIG_PATH_RST, (1<<2)|(1<<1)|(1<<0), NULL); // Signal path reset. utils_DelayMs(100); // Setup the MPU-60X0 registers. i2c_WriteRegister(SLAVE_ADDRESS, REG_USER_CTRL, (0<<6) | // Disable FIFO. (0<<5) | // Disable master mode for the external I2C sensor. (0<<4) | // Do not disable the I2C slave interface (mandatory for the MPU-6050). (0<<2) | // Do not reset the FIFO. (0<<1) | // Do not reset the I2C. (0<<0), // Do not reset the signal paths for the sensors. NULL); i2c_WriteRegister(SLAVE_ADDRESS, REG_POWER_MGMT_1, (0<<7) | // Do not reset the device. (0<<6) | // Disable sleep. (0<<5) | // Disable the "cycle" mode. (0<<3) | // Do not disable the temperature sensor. (3<<0), // PLL with Z axis gyro ref. NULL); i2c_WriteRegister(SLAVE_ADDRESS, REG_GYRO_CONFIG, (0<<7) | // Do not perform self-test for axis X. (0<<6) | // Do not perform self-test for axis Y. (0<<5) | // Do not perform self-test for axis Z. (gyroRange<<3), // Gyroscope range. NULL); i2c_WriteRegister(SLAVE_ADDRESS, REG_ACCEL_CONFIG, (0<<7) | // Do not perform self-test for axis X. (0<<6) | // Do not perform self-test for axis Y. (0<<5) | // Do not perform self-test for axis Z. (accelRange<<3), // Accelerometer range. NULL); i2c_WriteRegister(SLAVE_ADDRESS, REG_CONFIG, (0<<3) | // Disable synchronisation with FSYNC pin. (bandwidth<<0), // Accelerometer and gyroscope bandwidth. NULL); if(bandwidth == MPU_DLPF_BW_256HZ) i2c_WriteRegister(SLAVE_ADDRESS, REG_SMPLRT_DIV, 7, NULL); else i2c_WriteRegister(SLAVE_ADDRESS, REG_SMPLRT_DIV, 0, NULL); // mpu_initialized = true; return true; } +/** + * @brief Acquires the value of a single axis, from the IMU. + * @param axis the IMU axis to get (see mpu_Axis). + * @param value variable to write the value on. The unit depends on the selected + * axis: [m/s^2] for an acceleration, [deg/s] for an angular speed, and + * [celsius] for the temperature. + * @remark if the MPU-6050 is not initialized (mpu_Init() was not called + * successfully), then this function returns 0.0f. + * @remark If the operation failed, then *value is not modified. + */ +void mpu_GetAxis(mpu_Axis axis, float32_t *value) +{ + bool ok; + uint8_t tempData[2]; + float32_t decimalValue; + + if(!mpu_initialized) + return; + + i2c_ReadMultiBytesRegister(SLAVE_ADDRESS, axis, &tempData[0], 2, &ok); + + if(!ok) + return; + + decimalValue = ((float32_t)(int16_t)((tempData[0]<<8) | tempData[1])); + + switch(axis) + { + case MPU_AXIS_ACCEL_X: + case MPU_AXIS_ACCEL_Y: + case MPU_AXIS_ACCEL_Z: + *value = decimalValue * accelFactor; + break; + + case MPU_AXIS_TEMPERATURE: + *value = decimalValue / 340.0f + 36.53f; // Coefs from the registers map spec. + break; + + case MPU_AXIS_GYRO_X: + case MPU_AXIS_GYRO_Y: + case MPU_AXIS_GYRO_Z: + *value = decimalValue * gyroFactor; + break; + + default: + break; + } +} + /** * @brief Acquires the acceleration from the IMU. * @param ax variable to write the X-axis acceleration on [m/s^2]. * @param ay variable to write the Y-axis acceleration on [m/s^2]. * @param az variable to write the Z-axis acceleration on [m/s^2]. * @remark ax, ay, and az pointer must be valid (they can't be NULL). * @remark if the MPU-6050 is not initialized (mpu_Init() was not called * successfully), then this function does nothing. * @remark If the operation failed, then *ax, *ay and *az are not modified. */ -void mpu_GetAcceleration(float *ax, float *ay, float *az) +void mpu_GetAcceleration(float32_t *ax, float32_t *ay, float32_t *az) { bool ok; uint8_t accelData[6]; if(!mpu_initialized) return; i2c_ReadMultiBytesRegister(SLAVE_ADDRESS, REG_ACCEL_VALUES, &accelData[0], 6, &ok); if(!ok) return; - *ax = ((float)(int16_t)((accelData[0]<<8) | accelData[1])) * accelFactor; - *ay = ((float)(int16_t)((accelData[2]<<8) | accelData[3])) * accelFactor; - *az = ((float)(int16_t)((accelData[4]<<8) | accelData[5])) * accelFactor; + *ax = ((float32_t)(int16_t)((accelData[0]<<8) | accelData[1])) + * accelFactor; + *ay = ((float32_t)(int16_t)((accelData[2]<<8) | accelData[3])) + * accelFactor; + *az = ((float32_t)(int16_t)((accelData[4]<<8) | accelData[5])) + * accelFactor; } /** * @brief Acquires the angular speed from the IMU. * @param gx variable to write the X-axis angular speed on [deg/s]. * @param gy variable to write the Y-axis angular speed on [deg/s]. * @param gz variable to write the Z-axis angular speed on [deg/s]. * @remark gx, gy, and gz pointer must be valid (they can't be NULL). * @remark if the MPU-6050 is not initialized (mpu_Init() was not called * successfully), then this function does nothing. * @remark If the operation failed, then *ax, *ay and *az are not modified. */ -void mpu_GetAngularSpeed(float *gx, float *gy, float *gz) +void mpu_GetAngularSpeed(float32_t *gx, float32_t *gy, float32_t *gz) { bool ok; uint8_t gyroData[6]; if(!mpu_initialized) return; i2c_ReadMultiBytesRegister(SLAVE_ADDRESS, REG_GYRO_VALUES, &gyroData[0], 6, &ok); if(!ok) return; - *gx = ((float)(int16_t)((gyroData[0]<<8) | gyroData[1])) * gyroFactor; - *gy = ((float)(int16_t)((gyroData[2]<<8) | gyroData[3])) * gyroFactor; - *gz = ((float)(int16_t)((gyroData[4]<<8) | gyroData[5])) * gyroFactor; + *gx = ((float32_t)(int16_t)((gyroData[0]<<8) | gyroData[1])) * gyroFactor; + *gy = ((float32_t)(int16_t)((gyroData[2]<<8) | gyroData[3])) * gyroFactor; + *gz = ((float32_t)(int16_t)((gyroData[4]<<8) | gyroData[5])) * gyroFactor; } /** * @brief Acquires the temperature from the IMU. - * @return the measured temperature [°C], or 0 if the operation failed. + * @return the measured temperature [celsius], or 0 if the operation failed. * @remark if the MPU-6050 is not initialized (mpu_Init() was not called - * successfully), then this function does nothing. + * successfully), then this function returns 0.0f. */ -float mpu_GetTemperature(void) +float32_t mpu_GetTemperature(void) { bool ok; uint8_t tempData[2]; if(!mpu_initialized) return 0.0f; i2c_ReadMultiBytesRegister(SLAVE_ADDRESS, REG_TEMP_VALUES, &tempData[0], 2, &ok); if(!ok) return 0.0f; - return ((float)(int16_t)((tempData[0]<<8) | tempData[1])) / 340.0f + 36.53f; // Coefs from the registers map spec. + return ((float32_t)(int16_t)((tempData[0]<<8) | tempData[1])) + / 340.0f + 36.53f; // Coefs from the registers map spec. } diff --git a/Firmware/src/drivers/mpu_6050.h b/Firmware/src/drivers/mpu_6050.h index 312b9ee..f575fe4 100644 --- a/Firmware/src/drivers/mpu_6050.h +++ b/Firmware/src/drivers/mpu_6050.h @@ -1,83 +1,98 @@ /* * Copyright (C) 2017 EPFL-LSRO (Laboratoire de Systemes Robotiques). * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef __MPU_6050_H #define __MPU_6050_H #include "../main.h" /** @defgroup MPU-6050 Driver / MPU-6050 * @brief Driver for the MPU-6050, a 6-axis IMU from Invensense. * * This driver uses the I2C bus of the digital extension connector. * * Call mpu_Init() first in the initialization code. Then, call mpu_Get() to * get the accelerations and angular speeds. * * @note This driver setup the MPU-6050 sampling rate to 1 kHz. Calling * mpu_Get() more often will return the same values. * * @addtogroup MPU-6050 * @{ */ /** * @brief Enumeration of the possible accelerometer ranges. */ typedef enum { MPU_ACCEL_RANGE_2G = 0, ///< +-2 G (+-19.62 m/s^2). MPU_ACCEL_RANGE_4G = 1, ///< +-4 G (+-39.24 m/s^2). MPU_ACCEL_RANGE_8G = 2, ///< +-8 G (+-78.48 m/s^2). MPU_ACCEL_RANGE_16G = 3 ///< +-16 G (+-156.96 m/s^2). } mpu_AccelRange; /** * @brief Enumeration of the possible gyrometer ranges. */ typedef enum { MPU_GYRO_RANGE_250DPS = 0, ///< +- 250 deg/s. MPU_GYRO_RANGE_500DPS = 1, ///< +- 500 deg/s. MPU_GYRO_RANGE_1000DPS = 2, ///< +- 1000 deg/s. MPU_GYRO_RANGE_2000DPS = 3 ///< +- 2000 deg/s. } mpu_GyroRange; /** - * @brief Enumeration of the possible bandwiths. + * @brief Enumeration of the possible bandwidths. */ typedef enum { MPU_DLPF_BW_256HZ = 0, ///< Cut-off freq: 256 Hz. IMU sampling freq: 8kHz. MPU_DLPF_BW_188HZ = 1, ///< Cut-off freq: 188 Hz. IMU sampling freq: 1kHz. MPU_DLPF_BW_98HZ = 2, ///< Cut-off freq: 98 Hz. IMU sampling freq: 1kHz. MPU_DLPF_BW_42HZ = 3, ///< Cut-off freq: 42 Hz. IMU sampling freq: 1kHz. MPU_DLPF_BW_20HZ = 4, ///< Cut-off freq: 20 Hz. IMU sampling freq: 1kHz. MPU_DLPF_BW_10HZ = 5, ///< Cut-off freq: 10 Hz. IMU sampling freq: 1kHz. MPU_DLPF_BW_5HZ = 6 ///< Cut-off freq: 5 Hz. IMU sampling freq: 1kHz. } mpu_Bandwidth; +/** + * @brief Enumeration of all the measurement registers. + */ +typedef enum +{ + MPU_AXIS_ACCEL_X = 0x3b, + MPU_AXIS_ACCEL_Y = 0x3d, + MPU_AXIS_ACCEL_Z = 0x3f, + MPU_AXIS_TEMPERATURE = 0x41, + MPU_AXIS_GYRO_X = 0x43, + MPU_AXIS_GYRO_Y = 0x45, + MPU_AXIS_GYRO_Z = 0x47 +} mpu_Axis; + bool mpu_Init(mpu_AccelRange accelRange, mpu_GyroRange gyroRange, mpu_Bandwidth bandwidth); -void mpu_GetAcceleration(float *ax, float *ay, float *az); -void mpu_GetAngularSpeed(float *gx, float *gy, float *gz); +void mpu_GetAxis(mpu_Axis axis, float32_t *value); +void mpu_GetAcceleration(float32_t *ax, float32_t *ay, float32_t *az); +void mpu_GetAngularSpeed(float32_t *gx, float32_t *gy, float32_t *gz); float mpu_GetTemperature(void); /** * @} */ #endif