I'm trying to built Self Balancing Robot with STM32F303 Micro controller and for position measurement i'm using MPU6050. While data is being transmitted and received with I2C transmission protocol. I have previous experience with using STM32F303 and also familiar with I2C protocol and by consulting the data sheet of MPU6050, STM32F303 and some online resources I have built a code using Standard Peripheral Libraries provided by ST Electronics. The problem is that i'm getting the readings but the response time is too slow. On average it should consume maximum 2-3ms but here it is consuming more than 400ms and i'm totally unable to figure it out. Since timing is key of this project therefore I need guideline regarding this issue. I'm using Atollic as my IDE and the code files are as follows Main.c
#include "stm32f30x.h"
#include "stm32f30x_gpio.h"
#include "stm32f30x_i2c.h"
#include "stm32f30x_rcc.h"
#include "stm32f30x_spi.h"
#include "stm32f30x_misc.h"
#include "math.h"
#include "mpu6050.h"
#define I2C_SDA_PIN GPIO_Pin_9 // PORTB
#define I2C_SCL_PIN GPIO_Pin_8 // PORTB
#define degconvert 57.2957786 // There are like 57 degrees in a radian.
MPU6050_errorstatus err;
static int i = 0;
float gyro_xdata;
float gyro_ydata;
float gyro_zdata;
float accel_xdata;
float accel_ydata;
float accel_zdata;
float roll;
float pitch;
void initializeGPIO();
void initializeI2C();
int main(void)
{
initializeGPIO();
initializeI2C();
err = MPU6050_Initialization();
// Getting Raw Values
err = MPU6050_Get_Gyro_Data(&gyro_xdata, &gyro_ydata, &gyro_zdata);
err = MPU6050_Get_Accel_Data(&accel_xdata, &accel_ydata, &accel_zdata);
// Calculate Pitch and Roll
roll = atan2f(accel_ydata, accel_zdata) * degconvert;
pitch = atan2f(-accel_xdata, accel_zdata) * degconvert;
while(1)
{
// Collect Raw Data from Sensor
err = MPU6050_Get_Gyro_Data(&gyro_xdata, &gyro_ydata, &gyro_zdata);
err = MPU6050_Get_Accel_Data(&accel_xdata, &accel_ydata, &accel_zdata);
// The next two lines calculate the orientation of the Accelerometer relative
// to the earth and convert the output of atan2 from Radians to Degrees
// We will use this data to correct any Cumulative errors in the Orientation
// that the gyroscope develops.
roll = atan2f(accel_ydata, accel_zdata) * degconvert;
pitch = atan2f(-accel_xdata, accel_zdata) * degconvert;
}
}
MPU6050.c
#include "mpu6050.h"
uint32_t MPU6050_Timeout = MPU6050_FLAG_TIMEOUT;
MPU6050_dataStruct dataStruct;
/* @brief Sets up MPU6050 internal clock and sensors sensitivity rate
* This function must be called before using the sensor!
*
* @retval @MPU6050_errorstatus
*/
MPU6050_errorstatus MPU6050_Initialization(void){
MPU6050_errorstatus errorstatus;
/* Set Clock source for the chip
* possible values @pwr_mngt_1
*/
errorstatus = MPU6050_Set_Clock(MPU6050_PLL_X_GYRO);
if(errorstatus != 0) return errorstatus;
/* Set Gyroscope's full scope range
* possible values @gyro_scale_range
*/
errorstatus = MPU6050_Gyro_Set_Range(MPU6050_GYRO_250);
if(errorstatus != 0) return errorstatus;
/* Set Accelerometer's full scope range
* possible values @accel_scale_range
*/
errorstatus = MPU6050_Accel_Set_Range(MPU6050_ACCEL_2g);
if(errorstatus != 0) return errorstatus;
return MPU6050_NO_ERROR;
}
/* @brief Test if chip is visible on I2C line
* Reads the WHO_AM_I register
*
* @retval @MPU6050_errorstatus
*/
MPU6050_errorstatus MPU6050_Test(void){
MPU6050_errorstatus errorstatus;
uint8_t tmp;
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, WHO_AM_I, &tmp, 1);
if(tmp != (uint8_t)0x68){
return errorstatus;
}
return MPU6050_NO_ERROR;
}
/* @brief Get Gyroscope's full scale range
* Reads the GYRO_CONFIG register and returns the value of gyro's range
*
* @retval tmp - value of gyro's range
*/
uint8_t MPU6050_Gyro_Get_Range(void){
MPU6050_errorstatus errorstatus;
uint8_t tmp;
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, GYRO_CONFIG, &tmp, 1);
if(errorstatus != 0){
return 1;
}
else return tmp;
}
/* @brief Set Gyroscope's full scale range
* @param range - check @MPU6050_Gyro_Range
* @retval @MPU6050_errorstatus
*/
MPU6050_errorstatus MPU6050_Gyro_Set_Range(MPU6050_Gyro_Range range){
MPU6050_errorstatus errorstatus;
dataStruct.gyroMul = range;
errorstatus = MPU6050_Write((MPU6050_ADDRESS & 0x7f) << 1, GYRO_CONFIG, &range);
if(errorstatus != 0){
return errorstatus;
}
else return MPU6050_NO_ERROR;
}
/* @brief Get Accelerometer full scale range
* Reads the Accel_CONFIG register and returns the value of accelerometer's range
*
* @retval tmp - value of accelerometer's range
*/
uint8_t MPU6050_Accel_Get_Range(void){
MPU6050_errorstatus errorstatus;
uint8_t tmp;
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, ACCEL_CONFIG, &tmp, 1);
if(errorstatus != 0){
return 1;
}
else return tmp;
}
/* @brief Set Accelerometer full scale range
* @param range - check @MPU6050_Accel_Range
* @retval @MPU6050_errorstatus
*/
MPU6050_errorstatus MPU6050_Accel_Set_Range(MPU6050_Accel_Range range){
MPU6050_errorstatus errorstatus;
dataStruct.accelMul = range;
errorstatus = MPU6050_Write((MPU6050_ADDRESS & 0x7f) << 1, ACCEL_CONFIG, &range);
if(errorstatus != 0){
return errorstatus;
}
else return MPU6050_NO_ERROR;
}
/* @brief Set MPU6050 clock source
* @param clock - check @MPU6050_Clock_Select
* @retval @MPU6050_errorstatus
*/
MPU6050_errorstatus MPU6050_Set_Clock(MPU6050_Clock_Select clock){
MPU6050_errorstatus errorstatus;
errorstatus = MPU6050_Write((MPU6050_ADDRESS & 0x7f) << 1, PWR_MGMT_1, &clock);
if(errorstatus != 0){
return errorstatus;
}
else return MPU6050_NO_ERROR;
}
/* @brief Read MPU6050 temperature
* @retval temp_celsius - temperature in degrees celsius
*/
int16_t MPU6050_Get_Temperature(void){
MPU6050_errorstatus errorstatus;
uint8_t temp_low;
uint8_t temp_high;
int16_t temp;
int16_t temp_celsius;
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, TEMP_OUT_L, &temp_low, 1);
if(errorstatus != 0){
return 1;
}
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, TEMP_OUT_H, &temp_high, 1);
if(errorstatus != 0){
return 1;
}
temp = (uint16_t)(temp_high << 8 | temp_low);
temp_celsius = temp/340 + 36;
return temp_celsius;
}
/* @brief Get Gyroscope X,Y,Z raw data
*
* @param X - sensor roll on X axis
* @param Y - sensor pitch on Y axis
* @param Z - sensor jaw on Z axis
*
* @retval @MPU6050_errorstatus
*/
MPU6050_errorstatus MPU6050_Get_Gyro_Data_Raw(int16_t* X, int16_t* Y, int16_t* Z){
MPU6050_errorstatus errorstatus;
uint8_t xlow, xhigh, ylow, yhigh, zlow, zhigh;
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, GYRO_XOUT_L, &xlow, 1);
if(errorstatus != 0){
return errorstatus;
}
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, GYRO_XOUT_H, &xhigh, 1);
if(errorstatus != 0){
return errorstatus;
}
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, GYRO_YOUT_L, &ylow, 1);
if(errorstatus != 0){
return errorstatus;
}
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, GYRO_YOUT_H, &yhigh, 1);
if(errorstatus != 0){
return errorstatus;
}
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, GYRO_ZOUT_L, &zlow, 1);
if(errorstatus != 0){
return errorstatus;
}
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, GYRO_ZOUT_H, &zhigh, 1);
if(errorstatus != 0){
return errorstatus;
}
*X = (int16_t)(xhigh << 8 | xlow);
*Y = (int16_t)(yhigh << 8 | ylow);
*Z = (int16_t)(zhigh << 8 | zlow);
return MPU6050_NO_ERROR;
}
/* @brief Get Accelerometer X,Y,Z raw data
*
* @param X - sensor accel on X axis
* @param Y - sensor accel on Y axis
* @param Z - sensor accel on Z axis
*
* @retval @MPU6050_errorstatus
*/
MPU6050_errorstatus MPU6050_Get_Accel_Data_Raw(int16_t* X, int16_t* Y, int16_t* Z){
MPU6050_errorstatus errorstatus;
uint8_t xlow, xhigh, ylow, yhigh, zlow, zhigh;
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, ACCEL_XOUT_L, &xlow, 1);
if(errorstatus != 0){
return errorstatus;
}
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, ACCEL_XOUT_H, &xhigh, 1);
if(errorstatus != 0){
return errorstatus;
}
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, ACCEL_YOUT_L, &ylow, 1);
if(errorstatus != 0){
return errorstatus;
}
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, ACCEL_YOUT_H, &yhigh, 1);
if(errorstatus != 0){
return errorstatus;
}
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, ACCEL_ZOUT_L, &zlow, 1);
if(errorstatus != 0){
return errorstatus;
}
errorstatus = MPU6050_Read((MPU6050_ADDRESS & 0x7f) << 1, ACCEL_ZOUT_H, &zhigh, 1);
if(errorstatus != 0){
return errorstatus;
}
*X = (int16_t)(xhigh << 8 | xlow);
*Y = (int16_t)(yhigh << 8 | ylow);
*Z = (int16_t)(zhigh << 8 | zlow);
return MPU6050_NO_ERROR;
}
/* @brief Get Gyroscope X,Y,Z calculated data
*
* @param X - sensor roll on X axis
* @param Y - sensor pitch on Y axis
* @param Z - sensor jaw on Z axis
*
* @retval @MPU6050_errorstatus
*/
MPU6050_errorstatus MPU6050_Get_Gyro_Data(float* X, float* Y, float* Z){
MPU6050_errorstatus errorstatus;
float mult;
int16_t gyro_x, gyro_y, gyro_z;
errorstatus = MPU6050_Get_Gyro_Data_Raw(&gyro_x, &gyro_y, &gyro_z);
if(dataStruct.gyroMul == MPU6050_GYRO_250){
mult = (float)(1/MPU6050_GYRO_RANGE_250);
}
else if(dataStruct.gyroMul == MPU6050_GYRO_500){
mult = (float)(1/MPU6050_GYRO_RANGE_500);
}
else if(dataStruct.gyroMul == MPU6050_GYRO_1000){
mult = (float)(1/MPU6050_GYRO_RANGE_1000);
}
else mult = (float)(1/MPU6050_GYRO_RANGE_2000);
*X = (float)(gyro_x*mult);
*Y = (float)(gyro_y*mult);
*Z = (float)(gyro_z*mult);
return MPU6050_NO_ERROR;
}
/* @brief Get Accelerometer X,Y,Z calculated data
*
* @param X - sensor accel on X axis
* @param Y - sensor accel on Y axis
* @param Z - sensor accel on Z axis
*
* @retval @MPU6050_errorstatus
*/
MPU6050_errorstatus MPU6050_Get_Accel_Data(float* X, float* Y, float* Z){
MPU6050_errorstatus errorstatus;
float mult;
int16_t accel_x, accel_y, accel_z;
errorstatus = MPU6050_Get_Accel_Data_Raw(&accel_x, &accel_y, &accel_z);
if(dataStruct.accelMul == MPU6050_ACCEL_2g){
mult = (float)(1/MPU6050_ACCEL_RANGE_2g);
}
else if(dataStruct.accelMul == MPU6050_ACCEL_2g){
mult = (float)(1/MPU6050_ACCEL_RANGE_4g);
}
else if(dataStruct.accelMul == MPU6050_ACCEL_2g){
mult = (float)(1/MPU6050_ACCEL_RANGE_8g);
}
else mult = (float)(1/MPU6050_ACCEL_RANGE_16g);
*X = (float)(accel_x*mult);
*Y = (float)(accel_y*mult);
*Z = (float)(accel_z*mult);
return MPU6050_NO_ERROR;
}
/* @brief Reads bytes from MPU6050
*
* @param SlaveAddr - Slave I2C address
* @param RegAddr - register address
* @param pBuffer - buffer to write to
* @ param NumByteToRead - number of bytes to read
*
* @retval @MPU6050_errorstatus
*/
MPU6050_errorstatus MPU6050_Read(uint8_t SlaveAddr, uint8_t RegAddr, uint8_t* pBuffer, uint16_t NumByteToRead)
{
/* Test if SDA line busy */
MPU6050_Timeout = MPU6050_LONG_TIMEOUT;
while(I2C_GetFlagStatus(I2C1, I2C_FLAG_BUSY) != RESET)
{
if((MPU6050_Timeout--) == 0) return MPU6050_I2C_ERROR;
}
I2C_TransferHandling(I2C1, SlaveAddr, 1, I2C_SoftEnd_Mode, I2C_Generate_Start_Write);
MPU6050_Timeout = MPU6050_LONG_TIMEOUT;
while(I2C_GetFlagStatus(I2C1, I2C_FLAG_TXIS) == RESET)
{
if((MPU6050_Timeout--) == 0) return MPU6050_I2C_ERROR;
}
if(NumByteToRead>1)
RegAddr |= 0x80;
I2C_SendData(I2C1, (uint8_t)RegAddr);
MPU6050_Timeout = MPU6050_LONG_TIMEOUT;
while(I2C_GetFlagStatus(I2C1, I2C_FLAG_TC) == RESET)
{
if((MPU6050_Timeout--) == 0) return MPU6050_I2C_TX_ERROR;
}
I2C_TransferHandling(I2C1, SlaveAddr, NumByteToRead, I2C_AutoEnd_Mode, I2C_Generate_Start_Read);
while (NumByteToRead)
{
MPU6050_Timeout = MPU6050_LONG_TIMEOUT;
while(I2C_GetFlagStatus(I2C1, I2C_FLAG_RXNE) == RESET)
{
if((MPU6050_Timeout--) == 0) return MPU6050_I2C_RX_ERROR;
}
*pBuffer = I2C_ReceiveData(I2C1);
pBuffer++;
NumByteToRead--;
}
MPU6050_Timeout = MPU6050_LONG_TIMEOUT;
while(I2C_GetFlagStatus(I2C1, I2C_FLAG_STOPF) == RESET)
{
if((MPU6050_Timeout--) == 0) return MPU6050_I2C_ERROR;
}
I2C_ClearFlag(I2C1, I2C_FLAG_STOPF);
return MPU6050_NO_ERROR;
}
/* @brief Writes bytes to MPU6050
*
* @param SlaveAddr - Slave I2C address
* @param RegAddr - register address
* @param pBuffer - buffer to write from
*
* @retval @MPU6050_errorstatus
*/
MPU6050_errorstatus MPU6050_Write(uint8_t SlaveAddr, uint8_t RegAddr, uint8_t* pBuffer)
{
/* Test if SDA line busy */
MPU6050_Timeout = MPU6050_LONG_TIMEOUT;
while(I2C_GetFlagStatus(I2C1, I2C_FLAG_BUSY) != RESET)
{
if((MPU6050_Timeout--) == 0) return MPU6050_I2C_ERROR;
}
I2C_TransferHandling(I2C1, SlaveAddr, 1, I2C_Reload_Mode, I2C_Generate_Start_Write);
MPU6050_Timeout = MPU6050_LONG_TIMEOUT;
while(I2C_GetFlagStatus(I2C1, I2C_FLAG_TXIS) == RESET)
{
if((MPU6050_Timeout--) == 0) return MPU6050_I2C_ERROR;
}
I2C_SendData(I2C1, (uint8_t) RegAddr);
MPU6050_Timeout = MPU6050_LONG_TIMEOUT;
while(I2C_GetFlagStatus(I2C1, I2C_FLAG_TCR) == RESET)
{
if((MPU6050_Timeout--) == 0) return MPU6050_I2C_ERROR;
}
I2C_TransferHandling(I2C1, SlaveAddr, 1, I2C_AutoEnd_Mode, I2C_No_StartStop);
MPU6050_Timeout = MPU6050_LONG_TIMEOUT;
while(I2C_GetFlagStatus(I2C1, I2C_FLAG_TXIS) == RESET)
{
if((MPU6050_Timeout--) == 0) return MPU6050_I2C_ERROR;
}
I2C_SendData(I2C1, *pBuffer);
MPU6050_Timeout = MPU6050_LONG_TIMEOUT;
while(I2C_GetFlagStatus(I2C1, I2C_FLAG_STOPF) == RESET)
{
if((MPU6050_Timeout--) == 0) return MPU6050_I2C_ERROR;
}
I2C_ClearFlag(I2C1, I2C_FLAG_STOPF);
return MPU6050_NO_ERROR;
}
I have found the answer of my problem. When i tried to debug it, it was found that the clock frequency of I2C protocol is very less and was about 1 Khz where as it should be in range of 4 Khz for proper operation and performance efficiency. I tried to consult the datasheet and i found that the issue was with Timing Register of I2C module. In the I2C timing register the last 4bits are dedicated to clock frequency prescaler and my case it was set to full and hence the frequency was divided to maximum extent possible. so in the initialization segment of the I2C module
void initializeI2C(){
RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE);
I2C_InitTypeDef I2C_InitStructure;
I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;
I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
I2C_InitStructure.I2C_OwnAddress1 = 0;
I2C_InitStructure.I2C_AnalogFilter = I2C_AnalogFilter_Enable;
I2C_InitStructure.I2C_DigitalFilter = 0x00;
I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
I2C_InitStructure.I2C_Timing = 0xF062121F;
I2C_Init(I2C1, &I2C_InitStructure);
I2C_Cmd(I2C1, ENABLE);
}
I changed the value of I2C_Timing register to 0x0062121F and the clock frequency of the I2C was restored and hence I was able to get maximum sampling time of almost 4 ms
I2C_InitStructure.I2C_Timing = 0x0062121F;