Panusorn Chinsakuljaroen
imu for l432kc
Diff: MPU6050.cpp
- Revision:
- 2:01ca44dd3908
diff -r f8ed70330216 -r 01ca44dd3908 MPU6050.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6050.cpp Tue Dec 04 20:24:04 2018 +0000
@@ -0,0 +1,275 @@
+/**
+ * Includes
+ */
+#include "MPU6050.h"
+
+MPU6050::MPU6050(PinName sda, PinName scl) : connection(sda, scl) {
+ this->setSleepMode(false);
+
+ //Initializations:
+ currentGyroRange = 0;
+ currentAcceleroRange=0;
+ MPU6050_ADDRESS=0x68;
+ //connection.frequency(400000); //mesures ne fonctionnent pas
+ // test I2C connexion and update MPU6050_ADDRESS
+ isFindMPU6050=I2C_finder();
+ // if MPU6050 not found stop the program !!!!!
+ if(!isFindMPU6050)
+ {
+ DigitalOut myled(LED1);
+ printf("MPU6050 doesn't exist\n\r");
+ printf("Check Connexion SCL and SDA and Power supply of your sensor\n\r");
+ while(1) {
+ myled = 1;
+ wait(0.2);
+ myled = 0;
+ wait(0.2);
+ }
+ }
+}
+
+//--------------------------------------------------
+//-------------------General------------------------
+//--------------------------------------------------
+
+bool MPU6050::I2C_finder(){
+ int i,isOK=1;
+ char data;
+ bool isFindSlave=false;
+ printf("test I2C\n\r");
+ for(i=1;i<127;i++)
+ { wait(0.001);
+ isOK=connection.read(i*2,&data,1,false);
+ if(isOK==0){
+ printf("slave I2C find : 0X%X\n\r",i);
+ if(i==MPU6050_ADDRESS || i==MPU6050_ADDRESS+1){
+ printf("MPU6050 has been found : 0x%X\n\r",i);
+ MPU6050_ADDRESS=i; // on met l'adresse du MPU6050 = 0x68 ou 0x69
+ isFindSlave=true;
+ }
+ }
+ }
+ return isFindSlave;
+}
+
+void MPU6050::write(char address, char data) {
+ char temp[2];
+ temp[0]=address;
+ temp[1]=data;
+
+ connection.write(MPU6050_ADDRESS * 2,temp,2);
+}
+
+char MPU6050::read(char address) {
+ char retval;
+ connection.write(MPU6050_ADDRESS * 2, &address, 1, true);
+ connection.read(MPU6050_ADDRESS * 2, &retval, 1);
+ return retval;
+}
+
+void MPU6050::read(char address, char *data, int length) {
+ connection.write(MPU6050_ADDRESS * 2, &address, 1, true);
+ connection.read(MPU6050_ADDRESS * 2, data, length);
+}
+
+void MPU6050::setSleepMode(bool state) {
+ char temp;
+ temp = this->read(MPU6050_PWR_MGMT_1_REG);
+ if (state == true)
+ temp |= 1<<MPU6050_SLP_BIT;
+ if (state == false)
+ temp &= ~(1<<MPU6050_SLP_BIT);
+ this->write(MPU6050_PWR_MGMT_1_REG, temp);
+}
+
+bool MPU6050::testConnection( void ) {
+ char temp=0;
+ temp = this->read(MPU6050_WHO_AM_I_REG);
+ printf(" WHO AM I : 0X%X\n\r",temp);
+ return (temp == (MPU6050_ADDRESS & 0xFE));
+}
+
+void MPU6050::setBW(char BW) {
+ char temp;
+ BW=BW & 0x07;
+ temp = this->read(MPU6050_CONFIG_REG);
+ temp &= 0xF8;
+ temp = temp + BW;
+ this->write(MPU6050_CONFIG_REG, temp);
+}
+
+void MPU6050::setI2CBypass(bool state) {
+ char temp;
+ temp = this->read(MPU6050_INT_PIN_CFG);
+ if (state == true)
+ temp |= 1<<MPU6050_BYPASS_BIT;
+ if (state == false)
+ temp &= ~(1<<MPU6050_BYPASS_BIT);
+ this->write(MPU6050_INT_PIN_CFG, temp);
+}
+
+//--------------------------------------------------
+//----------------Accelerometer---------------------
+//--------------------------------------------------
+
+void MPU6050::setAcceleroRange( char range ) {
+ char temp;
+ range = range & 0x03;
+ currentAcceleroRange = range;
+
+ temp = this->read(MPU6050_ACCELERO_CONFIG_REG);
+ temp &= ~(3<<3);
+ temp = temp + (range<<3);
+ this->write(MPU6050_ACCELERO_CONFIG_REG, temp);
+}
+
+int MPU6050::getAcceleroRawX( void ) {
+ short retval;
+ char data[2];
+ this->read(MPU6050_ACCEL_XOUT_H_REG, data, 2);
+ retval = (data[0]<<8) + data[1];
+ return (int)retval;
+}
+
+int MPU6050::getAcceleroRawY( void ) {
+ short retval;
+ char data[2];
+ this->read(MPU6050_ACCEL_YOUT_H_REG, data, 2);
+ retval = (data[0]<<8) + data[1];
+ return (int)retval;
+}
+
+int MPU6050::getAcceleroRawZ( void ) {
+ short retval;
+ char data[2];
+ this->read(MPU6050_ACCEL_ZOUT_H_REG, data, 2);
+ retval = (data[0]<<8) + data[1];
+ return (int)retval;
+}
+
+void MPU6050::getAcceleroRaw( int *data ) {
+ char temp[6];
+ this->read(MPU6050_ACCEL_XOUT_H_REG, temp, 6);
+ data[0] = (int)(short)((temp[0]<<8) + temp[1]);
+ data[1] = (int)(short)((temp[2]<<8) + temp[3]);
+ data[2] = (int)(short)((temp[4]<<8) + temp[5]);
+}
+
+void MPU6050::getAccelero( float *data ) {
+ int temp[3];
+ this->getAcceleroRaw(temp);
+ if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_2G) {
+ data[0]=(float)temp[0] / 16384.0 * 9.81;
+ data[1]=(float)temp[1] / 16384.0 * 9.81;
+ data[2]=(float)temp[2] / 16384.0 * 9.81;
+ }
+ if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_4G){
+ data[0]=(float)temp[0] / 8192.0 * 9.81;
+ data[1]=(float)temp[1] / 8192.0 * 9.81;
+ data[2]=(float)temp[2] / 8192.0 * 9.81;
+ }
+ if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_8G){
+ data[0]=(float)temp[0] / 4096.0 * 9.81;
+ data[1]=(float)temp[1] / 4096.0 * 9.81;
+ data[2]=(float)temp[2] / 4096.0 * 9.81;
+ }
+ if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_16G){
+ data[0]=(float)temp[0] / 2048.0 * 9.81;
+ data[1]=(float)temp[1] / 2048.0 * 9.81;
+ data[2]=(float)temp[2] / 2048.0 * 9.81;
+ }
+
+ #ifdef DOUBLE_ACCELERO
+ data[0]*=2;
+ data[1]*=2;
+ data[2]*=2;
+ #endif
+}
+
+//--------------------------------------------------
+//------------------Gyroscope-----------------------
+//--------------------------------------------------
+void MPU6050::setGyroRange( char range ) {
+ char temp;
+ currentGyroRange = range;
+ range = range & 0x03;
+ temp = this->read(MPU6050_GYRO_CONFIG_REG);
+ temp &= ~(3<<3);
+ temp = temp + range<<3;
+ this->write(MPU6050_GYRO_CONFIG_REG, temp);
+}
+
+int MPU6050::getGyroRawX( void ) {
+ short retval;
+ char data[2];
+ this->read(MPU6050_GYRO_XOUT_H_REG, data, 2);
+ retval = (data[0]<<8) + data[1];
+ return (int)retval;
+}
+
+int MPU6050::getGyroRawY( void ) {
+ short retval;
+ char data[2];
+ this->read(MPU6050_GYRO_YOUT_H_REG, data, 2);
+ retval = (data[0]<<8) + data[1];
+ return (int)retval;
+}
+
+int MPU6050::getGyroRawZ( void ) {
+ short retval;
+ char data[2];
+ this->read(MPU6050_GYRO_ZOUT_H_REG, data, 2);
+ retval = (data[0]<<8) + data[1];
+ return (int)retval;
+}
+
+void MPU6050::getGyroRaw( int *data ) {
+ char temp[6];
+ this->read(MPU6050_GYRO_XOUT_H_REG, temp, 6);
+ data[0] = (int)(short)((temp[0]<<8) + temp[1]);
+ data[1] = (int)(short)((temp[2]<<8) + temp[3]);
+ data[2] = (int)(short)((temp[4]<<8) + temp[5]);
+}
+
+void MPU6050::getGyro( float *data ) {
+ int temp[3];
+ this->getGyroRaw(temp);
+ if (currentGyroRange == MPU6050_GYRO_RANGE_250) {
+ data[0]=(float)temp[0] / 7505.7;
+ data[1]=(float)temp[1] / 7505.7;
+ data[2]=(float)temp[2] / 7505.7;
+ }
+ if (currentGyroRange == MPU6050_GYRO_RANGE_500){
+ data[0]=(float)temp[0] / 3752.9;
+ data[1]=(float)temp[1] / 3752.9;
+ data[2]=(float)temp[2] / 3752.9;
+ }
+ if (currentGyroRange == MPU6050_GYRO_RANGE_1000){
+ data[0]=(float)temp[0] / 1879.3;;
+ data[1]=(float)temp[1] / 1879.3;
+ data[2]=(float)temp[2] / 1879.3;
+ }
+ if (currentGyroRange == MPU6050_GYRO_RANGE_2000){
+ data[0]=(float)temp[0] / 939.7;
+ data[1]=(float)temp[1] / 939.7;
+ data[2]=(float)temp[2] / 939.7;
+ }
+}
+//--------------------------------------------------
+//-------------------Temperature--------------------
+//--------------------------------------------------
+int MPU6050::getTempRaw( void ) {
+ short retval;
+ char data[2];
+ this->read(MPU6050_TEMP_H_REG, data, 2);
+ retval = (data[0]<<8) + data[1];
+ return (int)retval;
+}
+
+float MPU6050::getTemp( void ) {
+ float retval;
+ retval=(float)this->getTempRaw();
+ retval=(retval+521.0)/340.0+35.0;
+ return retval;
+}
+