//MPU6050 I2C library for ARM STM32F103xx Microcontrollers - Main source file //Has bit, byte and buffer I2C R/W functions // 23/05/2012 by Harinadha Reddy Chintalapalli // Changelog: // 2012-05-23 - initial release. Thanks to Jeff Rowberg for his AVR/Arduino // based MPU6050 development which inspired me & taken as reference to develop this. /* ============================================================================================ MPU6050 device I2C library code for ARM STM32F103xx is placed under the MIT license Copyright (c) 2012 Harinadha Reddy Chintalapalli Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. ================================================================================================ */ /* Includes */ #include "../Include/myGlobal_Vars.h" #include "../Include/myDefinition.h" #include "../Include/MPU6050.h" #include "../Include/myDelay.h" #include "../Include/myLcdText.h" #include "stm32f4xx_i2c.h" #include "math.h" #ifndef __cplusplus #include #endif volatile float Ax, Ay, Az, Gx, Gy, Gz; // data mentah dari gyroscope dan accelerometer volatile float GxOffset=0, GyOffset=0, GzOffset=0; // offset dari gyroscope int16_t DataIMU[6]; /** @defgroup MPU6050_Library * @{ */ void I2C_DMA_Read(u8 slaveAddr, u8 readAddr)//, u8 sensor) { /* Disable DMA channel*/ DMA_Cmd(DMA1_Stream0, DISABLE); /* Set current data number again to 14 for MPu6050, only possible after disabling the DMA channel */ DMA_SetCurrDataCounter(DMA1_Stream0, 14); /* While the bus is busy */ while(I2C_GetFlagStatus(MPU6050_I2C, I2C_FLAG_BUSY)){ lcd_clear(); lcd_gotoxy(0,1);lcd_int16(xfloat[0]); if(DMA_GetCmdStatus(DMA1_Stream0)==ENABLE) lcd_data('1'); else lcd_data('0'); if(DMA_GetFlagStatus(DMA1_Stream0, DMA_FLAG_FEIF0)==SET) lcd_data('1'); else lcd_data('0'); //I2C_GenerateSTOP(I2C1, ENABLE); }; /* Enable DMA NACK automatic generation */ I2C_DMALastTransferCmd(MPU6050_I2C, ENABLE); //Note this one, very important /* Send START condition */ I2C_GenerateSTART(MPU6050_I2C, ENABLE); /* Test on EV5 and clear it */ while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_MODE_SELECT)); /* Send MPU6050 address for write */ I2C_Send7bitAddress(MPU6050_I2C, slaveAddr, I2C_Direction_Transmitter); /* Test on EV6 and clear it */ while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)); /* Clear EV6 by setting again the PE bit */ I2C_Cmd(MPU6050_I2C, ENABLE); /* Send the MPU6050's internal address to write to */ I2C_SendData(MPU6050_I2C, readAddr); /* Test on EV8 and clear it */ while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_BYTE_TRANSMITTED)); /* Send STRAT condition a second time */ I2C_GenerateSTART(MPU6050_I2C, ENABLE); /* Test on EV5 and clear it */ while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_MODE_SELECT)); /* Send MPU6050 address for read */ I2C_Send7bitAddress(MPU6050_I2C, slaveAddr, I2C_Direction_Receiver); /* Test on EV6 and clear it */ while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED)); /* Start DMA to receive data from I2C */ DMA_Cmd(DMA1_Stream0, ENABLE); I2C_DMACmd(MPU6050_I2C, ENABLE); // When the data transmission is complete, it will automatically jump to DMA interrupt routine to finish the rest. //now go back to the main routine } void MPU6050_Write(uint8_t slaveAddr, uint8_t regAddr, uint8_t data) { uint8_t tmp; tmp = data; MPU6050_I2C_ByteWrite(slaveAddr,&tmp,regAddr); } /** Power on and prepare for general usage. * This will activate the device and take it out of sleep mode (which must be done * after start-up). This function also sets both the accelerometer and the gyroscope * to their most sensitive settings, namely +/- 2g and +/- 250 degrees/sec, and sets * the clock source to use the X Gyro for reference, which is slightly better than * the default internal clock source. */ void MPU6050_Initialize() { uint8_t x; delay_ms(50); MPU6050_SetSleepModeStatus(DISABLE); delay_ms(50); MPU6050_SetClockSource(MPU6050_CLOCK_PLL_XGYRO); x=4;//7;//9; MPU6050_I2C_ByteWrite(MPU6050_DEFAULT_ADDRESS, &x, MPU6050_RA_SMPLRT_DIV); MPU6050_WriteBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, MPU6050_DLPF_BW_42); MPU6050_SetFullScaleGyroRange(MPU6050_GYRO_FS_1000);//1000 MPU6050_SetFullScaleAccelRange(MPU6050_ACCEL_FS_2);//2 MPU6050_Write(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_INT_PIN_CFG, 1<<4); //interrupt status bits are cleared on any read operation MPU6050_Write(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_INT_ENABLE, 1<<0); //interupt occurs when data is ready. The interupt routine is in the receiver.c file. //MPU6050_Write(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_SIGNAL_PATH_RESET, 0x07);//reset gyro and accel sensor } //------------------------------------------------------------------ void MPU6050_InitializeX(void) { //MPU6050_Write(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, 1<<7);//reset the whole module first delay_ms(50); //wait for 50ms for the gyro to stable MPU6050_SetSleepModeStatus(DISABLE); delay_ms(50); MPU6050_Write(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, MPU6050_CLOCK_PLL_ZGYRO);//PLL with Z axis gyroscope reference MPU6050_Write(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_CONFIG, 0x01); //DLPF_CFG = 1: Fs=1khz; bandwidth=42hz MPU6050_Write(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_SMPLRT_DIV, 0x01); //500Hz sample rate ~ 2ms MPU6050_Write(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_GYRO_CONFIG, MPU6050_GYRO_FS_2000); //Gyro full scale setting MPU6050_Write(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACCEL_FS_16); //Accel full scale setting MPU6050_Write(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_INT_PIN_CFG, 1<<4); //interrupt status bits are cleared on any read operation MPU6050_Write(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_INT_ENABLE, 1<<0); //interupt occurs when data is ready. The interupt routine is in the receiver.c file. MPU6050_Write(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_SIGNAL_PATH_RESET, 0x07);//reset gyro and accel sensor } /** Verify the I2C connection. * Make sure the device is connected and responds as expected. * @return True if connection is valid, FALSE otherwise */ bool MPU6050_TestConnection() { if(MPU6050_GetDeviceID() == 0x34) //0b110100; 8-bit representation in hex = 0x34 return TRUE; else return FALSE; } // WHO_AM_I register /** Get Device ID. * This register is used to verify the identity of the device (0b110100). * @return Device ID (should be 0x68, 104 dec, 150 oct) * @see MPU6050_RA_WHO_AM_I * @see MPU6050_WHO_AM_I_BIT * @see MPU6050_WHO_AM_I_LENGTH */ uint8_t MPU6050_GetDeviceID() { uint8_t tmp; MPU6050_ReadBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, &tmp); return tmp; } /** Set clock source setting. * An internal 8MHz oscillator, gyroscope based clock, or external sources can * be selected as the MPU-60X0 clock source. When the internal 8 MHz oscillator * or an external source is chosen as the clock source, the MPU-60X0 can operate * in low power modes with the gyroscopes disabled. * * Upon power up, the MPU-60X0 clock source defaults to the internal oscillator. * However, it is highly recommended that the device be configured to use one of * the gyroscopes (or an external clock source) as the clock reference for * improved stability. The clock source can be selected according to the following table: * * 
 * CLK_SEL | Clock Source
 * --------+--------------------------------------
 * 0       | Internal oscillator
 * 1       | PLL with X Gyro reference
 * 2       | PLL with Y Gyro reference
 * 3       | PLL with Z Gyro reference
 * 4       | PLL with external 32.768kHz reference
 * 5       | PLL with external 19.2MHz reference
 * 6       | Reserved
 * 7       | Stops the clock and keeps the timing generator in reset
 *
* * @param source New clock source setting * @see MPU6050_GetClockSource() * @see MPU6050_RA_PWR_MGMT_1 * @see MPU6050_PWR1_CLKSEL_BIT * @see MPU6050_PWR1_CLKSEL_LENGTH */ void MPU6050_SetClockSource(uint8_t source) { MPU6050_WriteBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source); } /** Set full-scale gyroscope range. * @param range New full-scale gyroscope range value * @see MPU6050_GetFullScaleGyroRange() * @see MPU6050_GYRO_FS_250 * @see MPU6050_RA_GYRO_CONFIG * @see MPU6050_GCONFIG_FS_SEL_BIT * @see MPU6050_GCONFIG_FS_SEL_LENGTH */ void MPU6050_SetFullScaleGyroRange(uint8_t range) { MPU6050_WriteBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, range); } // GYRO_CONFIG register /** Get full-scale gyroscope range. * The FS_SEL parameter allows setting the full-scale range of the gyro sensors, * as described in the table below. * * 
 * 0 = +/- 250 degrees/sec
 * 1 = +/- 500 degrees/sec
 * 2 = +/- 1000 degrees/sec
 * 3 = +/- 2000 degrees/sec
 *
* * @return Current full-scale gyroscope range setting * @see MPU6050_GYRO_FS_250 * @see MPU6050_RA_GYRO_CONFIG * @see MPU6050_GCONFIG_FS_SEL_BIT * @see MPU6050_GCONFIG_FS_SEL_LENGTH */ uint8_t MPU6050_GetFullScaleGyroRange() { uint8_t tmp; MPU6050_ReadBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, &tmp); return tmp; } /** Get full-scale accelerometer range. * The FS_SEL parameter allows setting the full-scale range of the accelerometer * sensors, as described in the table below. * * 
 * 0 = +/- 2g
 * 1 = +/- 4g
 * 2 = +/- 8g
 * 3 = +/- 16g
 *
* * @return Current full-scale accelerometer range setting * @see MPU6050_ACCEL_FS_2 * @see MPU6050_RA_ACCEL_CONFIG * @see MPU6050_ACONFIG_AFS_SEL_BIT * @see MPU6050_ACONFIG_AFS_SEL_LENGTH */ uint8_t MPU6050_GetFullScaleAccelRange() { uint8_t tmp; MPU6050_ReadBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, &tmp); return tmp; } /** Set full-scale accelerometer range. * @param range New full-scale accelerometer range setting * @see MPU6050_GetFullScaleAccelRange() */ void MPU6050_SetFullScaleAccelRange(uint8_t range) { MPU6050_WriteBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, range); } /** Get sleep mode status. * Setting the SLEEP bit in the register puts the device into very low power * sleep mode. In this mode, only the serial interface and internal registers * remain active, allowing for a very low standby current. Clearing this bit * puts the device back into normal mode. To save power, the individual standby * selections for each of the gyros should be used if any gyro axis is not used * by the application. * @return Current sleep mode enabled status * @see MPU6050_RA_PWR_MGMT_1 * @see MPU6050_PWR1_SLEEP_BIT */ bool MPU6050_GetSleepModeStatus() { uint8_t tmp; MPU6050_ReadBit(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, &tmp); if(tmp == 0x00) return FALSE; else return TRUE; } /** Set sleep mode status. * @param enabled New sleep mode enabled status * @see MPU6050_GetSleepModeStatus() * @see MPU6050_RA_PWR_MGMT_1 * @see MPU6050_PWR1_SLEEP_BIT */ void MPU6050_SetSleepModeStatus(FunctionalState NewState) { MPU6050_WriteBit(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, NewState); } /** Get raw 6-axis motion sensor readings (accel/gyro). * Retrieves all currently available motion sensor values. * @param AccelGyro 16-bit signed integer array of length 6 * @see MPU6050_RA_ACCEL_XOUT_H */ void MPU6050_GetRawAccelGyro(s16* AccelGyro) { int i; u8 tmpBuffer[14]; MPU6050_I2C_BufferRead(MPU6050_DEFAULT_ADDRESS, tmpBuffer, MPU6050_RA_ACCEL_XOUT_H, 14); /* Get acceleration */ for(i=0; i<3; i++) AccelGyro[i]=((s16)((u16)tmpBuffer[2*i] << 8) + tmpBuffer[2*i+1]); /* Get Angular rate */ for(i=4; i<7; i++) AccelGyro[i-1]=((s16)((u16)tmpBuffer[2*i] << 8) + tmpBuffer[2*i+1]); } /** Write multiple bits in an 8-bit device register. * @param slaveAddr I2C slave device address * @param regAddr Register regAddr to write to * @param bitStart First bit position to write (0-7) * @param length Number of bits to write (not more than 8) * @param data Right-aligned value to write */ void MPU6050_WriteBits(uint8_t slaveAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) { uint8_t mask; // 010 value to write // 76543210 bit numbers // xxx args: bitStart=4, length=3 // 00011100 mask byte // 10101111 original value (sample) // 10100011 original & ~mask // 10101011 masked | value uint8_t tmp; MPU6050_I2C_BufferRead(slaveAddr, &tmp, regAddr, 1); mask = ((1 << length) - 1) << (bitStart - length + 1); data <<= (bitStart - length + 1); // shift data into correct position data &= mask; // zero all non-important bits in data tmp &= ~(mask); // zero all important bits in existing byte tmp |= data; // combine data with existing byte MPU6050_I2C_ByteWrite(slaveAddr,&tmp,regAddr); } /** write a single bit in an 8-bit device register. * @param slaveAddr I2C slave device address * @param regAddr Register regAddr to write to * @param bitNum Bit position to write (0-7) * @param value New bit value to write */ void MPU6050_WriteBit(uint8_t slaveAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data) { uint8_t tmp; MPU6050_I2C_BufferRead(slaveAddr, &tmp, regAddr, 1); tmp = (data != 0) ? (tmp | (1 << bitNum)) : (tmp & ~(1 << bitNum)); MPU6050_I2C_ByteWrite(slaveAddr,&tmp,regAddr); } /** Read multiple bits from an 8-bit device register. * @param slaveAddr I2C slave device address * @param regAddr Register regAddr to read from * @param bitStart First bit position to read (0-7) * @param length Number of bits to read (not more than 8) * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05) * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in readTimeout) */ void MPU6050_ReadBits(uint8_t slaveAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data) { uint8_t mask; // 01101001 read byte // 76543210 bit numbers // xxx args: bitStart=4, length=3 // 010 masked // -> 010 shifted uint8_t tmp; MPU6050_I2C_BufferRead(slaveAddr, &tmp, regAddr, 1); mask = ((1 << length) - 1) << (bitStart - length + 1); tmp &= mask; tmp >>= (bitStart - length + 1); *data = tmp; } /** Read a single bit from an 8-bit device register. * @param slaveAddr I2C slave device address * @param regAddr Register regAddr to read from * @param bitNum Bit position to read (0-7) * @param data Container for single bit value * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in readTimeout) */ void MPU6050_ReadBit(uint8_t slaveAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data) { uint8_t tmp; MPU6050_I2C_BufferRead(slaveAddr, &tmp, regAddr, 1); *data = tmp & (1 << bitNum); } /** * @brief Initializes the I2C peripheral used to drive the MPU6050 * @param None * @return None */ /** * @brief Writes one byte to the MPU6050. * @param slaveAddr : slave address MPU6050_DEFAULT_ADDRESS * @param pBuffer : pointer to the buffer containing the data to be written to the MPU6050. * @param writeAddr : address of the register in which the data will be written * @return None */ void MPU6050_I2C_ByteWrite(u8 slaveAddr, u8* pBuffer, u8 writeAddr) { // ENTR_CRT_SECTION(); /* Send START condition */ I2C_GenerateSTART(MPU6050_I2C, ENABLE); /* Test on EV5 and clear it */ while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_MODE_SELECT)); /* Send MPU6050 address for write */ I2C_Send7bitAddress(MPU6050_I2C, slaveAddr, I2C_Direction_Transmitter); /* Test on EV6 and clear it */ while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)); /* Send the MPU6050's internal address to write to */ I2C_SendData(MPU6050_I2C, writeAddr); /* Test on EV8 and clear it */ while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_BYTE_TRANSMITTED)); /* Send the byte to be written */ I2C_SendData(MPU6050_I2C, *pBuffer); /* Test on EV8 and clear it */ while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_BYTE_TRANSMITTED)); /* Send STOP condition */ I2C_GenerateSTOP(MPU6050_I2C, ENABLE); // EXT_CRT_SECTION(); } void MPU6050_I2C_ByteWriteX(u8 slaveAddr, u8* pBuffer, u8 writeAddr) { /* Send START condition */ I2C_GenerateSTART(MPU6050_I2C, ENABLE); /* Test on EV5 and clear it */ while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_MODE_SELECT)); /* Send MPU6050 address for write */ I2C_Send7bitAddress(MPU6050_I2C, slaveAddr, I2C_Direction_Transmitter); /* Test on EV6 and clear it */ while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)); /* Send the MPU6050's internal address to write to */ I2C_SendData(MPU6050_I2C, writeAddr); /* Test on EV8 and clear it */ //while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_BYTE_TRANSMITTING)); /* Send the byte to be written */ if (pBuffer!=0) I2C_SendData(MPU6050_I2C, *pBuffer); /* Test on EV8_2 and clear it */ while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_BYTE_TRANSMITTED)); /* Send STOP condition */ I2C_GenerateSTOP(MPU6050_I2C, ENABLE); } /** * @brief Reads a block of data from the MPU6050. * @param slaveAddr : slave address MPU6050_DEFAULT_ADDRESS * @param pBuffer : pointer to the buffer that receives the data read from the MPU6050. * @param readAddr : MPU6050's internal address to read from. * @param NumByteToRead : number of bytes to read from the MPU6050 ( NumByteToRead >1 only for the Mgnetometer readinf). * @return None */ void MPU6050_I2C_BufferRead(u8 slaveAddr, u8* pBuffer, u8 readAddr, u16 NumByteToRead) { // ENTR_CRT_SECTION(); /* While the bus is busy */ while(I2C_GetFlagStatus(MPU6050_I2C, I2C_FLAG_BUSY)); /* Send START condition */ I2C_GenerateSTART(MPU6050_I2C, ENABLE); /* Test on EV5 and clear it */ while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_MODE_SELECT)); /* Send MPU6050 address for write */ I2C_Send7bitAddress(MPU6050_I2C, slaveAddr, I2C_Direction_Transmitter); /* Test on EV6 and clear it */ while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)); /* Clear EV6 by setting again the PE bit */ I2C_Cmd(MPU6050_I2C, ENABLE); /* Send the MPU6050's internal address to write to */ I2C_SendData(MPU6050_I2C, readAddr); /* Test on EV8 and clear it */ while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_BYTE_TRANSMITTED)); /* Send STRAT condition a second time */ I2C_GenerateSTART(MPU6050_I2C, ENABLE); /* Test on EV5 and clear it */ while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_MODE_SELECT)); /* Send MPU6050 address for read */ I2C_Send7bitAddress(MPU6050_I2C, slaveAddr, I2C_Direction_Receiver); /* Test on EV6 and clear it */ while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED)); /* While there is data to be read */ while(NumByteToRead) { if(NumByteToRead == 1) { /* Disable Acknowledgement */ I2C_AcknowledgeConfig(MPU6050_I2C, DISABLE); /* Send STOP Condition */ I2C_GenerateSTOP(MPU6050_I2C, ENABLE); } /* Test on EV7 and clear it */ if(I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_BYTE_RECEIVED)) { /* Read a byte from the MPU6050 */ *pBuffer = I2C_ReceiveData(MPU6050_I2C); /* Point to the next location where the byte read will be saved */ pBuffer++; /* Decrement the read bytes counter */ NumByteToRead--; } } /* Enable Acknowledgement to be ready for another reception */ I2C_AcknowledgeConfig(MPU6050_I2C, ENABLE); // EXT_CRT_SECTION(); } // Membaca data IMU 3 Axis void BacaIMU(void) { int window = 0; MPU6050_GetRawAccelGyro(&DataIMU[0]); Ax = DataIMU[0]; Ay = DataIMU[1]; Az = DataIMU[2]; /* if (DataIMU[0]>-window && DataIMU[0]-window && DataIMU[1]-window && DataIMU[2]-window && DataIMU[4]-window && DataIMU[3]-window && DataIMU[5]>1); y = *(float*)&i; y = y * (1.5f - (halfx * y * y)); return y; } float inverse(float x) { // re-interpret as a 32 bit integer unsigned int *i = (unsigned int*)&x; // adjust exponent *i = 0x7F000000 - *i; //*i = 0x7EEEEEEE - *i; return x; } #define Q_accelero 0.001f //0.0001f //0.0001f //0.001f #define Q_gyro 0.00000000000000003f//0.0000000003f//0.000000003f//0.0000003f //0.0003f //0.001f //0.003f #define R_measure 0.03f //0.03f //0.03f //0.03f float AccPitch; // Perhitungan sudut Pitch (Perputaran dgn poros sumbu X) dengan menggunakan Kalman Filter void HitungKalmanPitch(float delta) { static float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0; float K_0, K_1, buffer; BacaIMU(); //time_sampling = (float)Dt/1000; //time_sampling = (float)Dt/1000000; time_sampling = delta; xfloat[0] = time_sampling; _time = dead; Dt=0; buffer = time_sampling * P_11; P_00 += time_sampling * (buffer - P_10 - P_01 + Q_accelero); P_01 -= buffer; P_10 -= buffer; P_11 += (time_sampling * Q_gyro); buffer = 1.0f/(P_00 + R_measure); // buffer = S K_0 = P_00 * buffer; K_1 = P_10 * buffer; #if PosisiBoard == TIDUR Pitch +=((float)Gy-GyOffset)*(time_sampling/MPU6050_GGAIN); //0.000609756097561f; if(Az<0) { AccPitch = atan((float)Ax * invSqrt((float)Az*(float)Az + (float)Ay*(float)Ay))*RAD2DEG;} else { //AccPitch = atan2((float)Ax, (float)-Az) * RAD2DEG; if(Ax>0){ AccPitch = 90.0f + atan((float)Az * invSqrt((float)Ax*(float)Ax + (float)Ay*(float)Ay))*RAD2DEG; } else { AccPitch = -90.0f - atan((float)Az * invSqrt((float)Ax*(float)Ax + (float)Ay*(float)Ay))*RAD2DEG; } } #else Pitch +=((float)Gy-GyOffset)*(time_sampling/MPU6050_GGAIN); //0.000609756097561f; if(Ax>0) { AccPitch = atan((float)Az * invSqrt((float)Ax*(float)Ax + (float)Ay*(float)Ay))*RAD2DEG;} else { if(Az>0){ AccPitch = 90.0f - atan((float)Ax * invSqrt((float)Az*(float)Az + (float)Ay*(float)Ay))*RAD2DEG; } else { AccPitch = atan((float)Ax * invSqrt((float)Az*(float)Az + (float)Ay*(float)Ay))*RAD2DEG - 90.0f; } } #endif time_sampling=0; _time = __start; GyPitch=Pitch; buffer = AccPitch - Pitch; Pitch += K_0 * buffer; GyOffset += K_1 * buffer; P_00 -= K_0 * P_00; P_01 -= K_0 * P_01; P_10 -= K_1 * P_00; P_11 -= K_1 * P_01; //fNewData = 1; /* Buffer[0]=125; if((int)AccPitch<-90) Buffer[1]=(int)-90; else if((int)AccPitch>90) Buffer[1]=90; else Buffer[1]=(int)AccPitch; if((int)GyPitch<-90) Buffer[2]=(char)-90; else if((int)GyPitch>90) Buffer[2]=90; else Buffer[2]=(int)GyPitch; if((int)Pitch<-90) Buffer[3]=(int)-90; else if(Pitch>90) Buffer[3]=90; else Buffer[3]=(int)Pitch; */ } void HitungYaw(void) { BacaIMU(); time_sampling = (float)Dt/1000; _time = dead; Dt=0; #if PosisiBoard == TIDUR Yaw +=((float)Gz-GzOffset)*(time_sampling/MPU6050_GGAIN); #elif PosisiBoard == TEGAK Yaw +=((float)Gx-GxOffset)*(time_sampling/MPU6050_GGAIN); #endif time_sampling=0; _time = __start; } /** * @} */ /* end of group MPU6050_Library */