CORE / Mbed 2 deprecated GeminiQUEST_ptarmigan

ロケット用プログラム

Dependencies:   mbed

MPU6050/MPU6050.cpp@0:d58274531d38, 2019-09-11 (annotated)

Committer:
ishiyamayuto
Date:
Wed Sep 11 06:37:20 2019 +0000
Revision:
0:d58274531d38
BMP180,MPU6050

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ishiyamayuto 0:d58274531d38 1 /**
ishiyamayuto 0:d58274531d38 2 * Includes
ishiyamayuto 0:d58274531d38 3 */
ishiyamayuto 0:d58274531d38 4 #include "MPU6050.h"
ishiyamayuto 0:d58274531d38 5
ishiyamayuto 0:d58274531d38 6 MPU6050::MPU6050(PinName sda, PinName scl) : connection(sda, scl) {
ishiyamayuto 0:d58274531d38 7 this->setSleepMode(false);
ishiyamayuto 0:d58274531d38 8
ishiyamayuto 0:d58274531d38 9 //Initializations:
ishiyamayuto 0:d58274531d38 10 currentGyroRange = 0;
ishiyamayuto 0:d58274531d38 11 currentAcceleroRange=0;
ishiyamayuto 0:d58274531d38 12 }
ishiyamayuto 0:d58274531d38 13
ishiyamayuto 0:d58274531d38 14 //--------------------------------------------------
ishiyamayuto 0:d58274531d38 15 //-------------------General------------------------
ishiyamayuto 0:d58274531d38 16 //--------------------------------------------------
ishiyamayuto 0:d58274531d38 17
ishiyamayuto 0:d58274531d38 18 void MPU6050::write(char address, char data) {
ishiyamayuto 0:d58274531d38 19 char temp[2];
ishiyamayuto 0:d58274531d38 20 temp[0]=address;
ishiyamayuto 0:d58274531d38 21 temp[1]=data;
ishiyamayuto 0:d58274531d38 22
ishiyamayuto 0:d58274531d38 23 connection.write(MPU6050_ADDRESS * 2,temp,2);
ishiyamayuto 0:d58274531d38 24 }
ishiyamayuto 0:d58274531d38 25
ishiyamayuto 0:d58274531d38 26 char MPU6050::read(char address) {
ishiyamayuto 0:d58274531d38 27 char retval;
ishiyamayuto 0:d58274531d38 28 connection.write(MPU6050_ADDRESS * 2, &address, 1, true);
ishiyamayuto 0:d58274531d38 29 connection.read(MPU6050_ADDRESS * 2, &retval, 1);
ishiyamayuto 0:d58274531d38 30 return retval;
ishiyamayuto 0:d58274531d38 31 }
ishiyamayuto 0:d58274531d38 32
ishiyamayuto 0:d58274531d38 33 void MPU6050::read(char address, char *data, int length) {
ishiyamayuto 0:d58274531d38 34 connection.write(MPU6050_ADDRESS * 2, &address, 1, true);
ishiyamayuto 0:d58274531d38 35 connection.read(MPU6050_ADDRESS * 2, data, length);
ishiyamayuto 0:d58274531d38 36 }
ishiyamayuto 0:d58274531d38 37
ishiyamayuto 0:d58274531d38 38 void MPU6050::setSleepMode(bool state) {
ishiyamayuto 0:d58274531d38 39 char temp;
ishiyamayuto 0:d58274531d38 40 temp = this->read(MPU6050_PWR_MGMT_1_REG);
ishiyamayuto 0:d58274531d38 41 if (state == true)
ishiyamayuto 0:d58274531d38 42 temp |= 1<<MPU6050_SLP_BIT;
ishiyamayuto 0:d58274531d38 43 if (state == false)
ishiyamayuto 0:d58274531d38 44 temp &= ~(1<<MPU6050_SLP_BIT);
ishiyamayuto 0:d58274531d38 45 this->write(MPU6050_PWR_MGMT_1_REG, temp);
ishiyamayuto 0:d58274531d38 46 }
ishiyamayuto 0:d58274531d38 47
ishiyamayuto 0:d58274531d38 48 bool MPU6050::testConnection( void ) {
ishiyamayuto 0:d58274531d38 49 char temp;
ishiyamayuto 0:d58274531d38 50 temp = this->read(MPU6050_WHO_AM_I_REG);
ishiyamayuto 0:d58274531d38 51 return (temp == (MPU6050_ADDRESS & 0xFE));
ishiyamayuto 0:d58274531d38 52 }
ishiyamayuto 0:d58274531d38 53
ishiyamayuto 0:d58274531d38 54 void MPU6050::setBW(char BW) {
ishiyamayuto 0:d58274531d38 55 char temp;
ishiyamayuto 0:d58274531d38 56 BW=BW & 0x07;
ishiyamayuto 0:d58274531d38 57 temp = this->read(MPU6050_CONFIG_REG);
ishiyamayuto 0:d58274531d38 58 temp &= 0xF8;
ishiyamayuto 0:d58274531d38 59 temp = temp + BW;
ishiyamayuto 0:d58274531d38 60 this->write(MPU6050_CONFIG_REG, temp);
ishiyamayuto 0:d58274531d38 61 }
ishiyamayuto 0:d58274531d38 62
ishiyamayuto 0:d58274531d38 63 void MPU6050::setI2CBypass(bool state) {
ishiyamayuto 0:d58274531d38 64 char temp;
ishiyamayuto 0:d58274531d38 65 temp = this->read(MPU6050_INT_PIN_CFG);
ishiyamayuto 0:d58274531d38 66 if (state == true)
ishiyamayuto 0:d58274531d38 67 temp |= 1<<MPU6050_BYPASS_BIT;
ishiyamayuto 0:d58274531d38 68 if (state == false)
ishiyamayuto 0:d58274531d38 69 temp &= ~(1<<MPU6050_BYPASS_BIT);
ishiyamayuto 0:d58274531d38 70 this->write(MPU6050_INT_PIN_CFG, temp);
ishiyamayuto 0:d58274531d38 71 }
ishiyamayuto 0:d58274531d38 72
ishiyamayuto 0:d58274531d38 73 //--------------------------------------------------
ishiyamayuto 0:d58274531d38 74 //----------------Accelerometer---------------------
ishiyamayuto 0:d58274531d38 75 //--------------------------------------------------
ishiyamayuto 0:d58274531d38 76
ishiyamayuto 0:d58274531d38 77 void MPU6050::setAcceleroRange( char range ) {
ishiyamayuto 0:d58274531d38 78 char temp;
ishiyamayuto 0:d58274531d38 79 range = range & 0x03;
ishiyamayuto 0:d58274531d38 80 currentAcceleroRange = range;
ishiyamayuto 0:d58274531d38 81
ishiyamayuto 0:d58274531d38 82 temp = this->read(MPU6050_ACCELERO_CONFIG_REG);
ishiyamayuto 0:d58274531d38 83 temp &= ~(3<<3);
ishiyamayuto 0:d58274531d38 84 temp = temp + (range<<3);
ishiyamayuto 0:d58274531d38 85 this->write(MPU6050_ACCELERO_CONFIG_REG, temp);
ishiyamayuto 0:d58274531d38 86 }
ishiyamayuto 0:d58274531d38 87
ishiyamayuto 0:d58274531d38 88 int MPU6050::getAcceleroRawX( void ) {
ishiyamayuto 0:d58274531d38 89 short retval;
ishiyamayuto 0:d58274531d38 90 char data[2];
ishiyamayuto 0:d58274531d38 91 this->read(MPU6050_ACCEL_XOUT_H_REG, data, 2);
ishiyamayuto 0:d58274531d38 92 retval = (data[0]<<8) + data[1];
ishiyamayuto 0:d58274531d38 93 return (int)retval;
ishiyamayuto 0:d58274531d38 94 }
ishiyamayuto 0:d58274531d38 95
ishiyamayuto 0:d58274531d38 96 int MPU6050::getAcceleroRawY( void ) {
ishiyamayuto 0:d58274531d38 97 short retval;
ishiyamayuto 0:d58274531d38 98 char data[2];
ishiyamayuto 0:d58274531d38 99 this->read(MPU6050_ACCEL_YOUT_H_REG, data, 2);
ishiyamayuto 0:d58274531d38 100 retval = (data[0]<<8) + data[1];
ishiyamayuto 0:d58274531d38 101 return (int)retval;
ishiyamayuto 0:d58274531d38 102 }
ishiyamayuto 0:d58274531d38 103
ishiyamayuto 0:d58274531d38 104 int MPU6050::getAcceleroRawZ( void ) {
ishiyamayuto 0:d58274531d38 105 short retval;
ishiyamayuto 0:d58274531d38 106 char data[2];
ishiyamayuto 0:d58274531d38 107 this->read(MPU6050_ACCEL_ZOUT_H_REG, data, 2);
ishiyamayuto 0:d58274531d38 108 retval = (data[0]<<8) + data[1];
ishiyamayuto 0:d58274531d38 109 return (int)retval;
ishiyamayuto 0:d58274531d38 110 }
ishiyamayuto 0:d58274531d38 111
ishiyamayuto 0:d58274531d38 112 void MPU6050::getAcceleroRaw( int *data ) {
ishiyamayuto 0:d58274531d38 113 char temp[6];
ishiyamayuto 0:d58274531d38 114 this->read(MPU6050_ACCEL_XOUT_H_REG, temp, 6);
ishiyamayuto 0:d58274531d38 115 data[0] = (int)(short)((temp[0]<<8) + temp[1]);
ishiyamayuto 0:d58274531d38 116 data[1] = (int)(short)((temp[2]<<8) + temp[3]);
ishiyamayuto 0:d58274531d38 117 data[2] = (int)(short)((temp[4]<<8) + temp[5]);
ishiyamayuto 0:d58274531d38 118 }
ishiyamayuto 0:d58274531d38 119
ishiyamayuto 0:d58274531d38 120 void MPU6050::getAccelero( float *data ) {
ishiyamayuto 0:d58274531d38 121 int temp[3];
ishiyamayuto 0:d58274531d38 122 this->getAcceleroRaw(temp);
ishiyamayuto 0:d58274531d38 123 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_2G) {
ishiyamayuto 0:d58274531d38 124 data[0]=(float)temp[0] / 16384.0 * 9.81;
ishiyamayuto 0:d58274531d38 125 data[1]=(float)temp[1] / 16384.0 * 9.81;
ishiyamayuto 0:d58274531d38 126 data[2]=(float)temp[2] / 16384.0 * 9.81;
ishiyamayuto 0:d58274531d38 127 }
ishiyamayuto 0:d58274531d38 128 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_4G){
ishiyamayuto 0:d58274531d38 129 data[0]=(float)temp[0] / 8192.0 * 9.81;
ishiyamayuto 0:d58274531d38 130 data[1]=(float)temp[1] / 8192.0 * 9.81;
ishiyamayuto 0:d58274531d38 131 data[2]=(float)temp[2] / 8192.0 * 9.81;
ishiyamayuto 0:d58274531d38 132 }
ishiyamayuto 0:d58274531d38 133 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_8G){
ishiyamayuto 0:d58274531d38 134 data[0]=(float)temp[0] / 4096.0 * 9.81;
ishiyamayuto 0:d58274531d38 135 data[1]=(float)temp[1] / 4096.0 * 9.81;
ishiyamayuto 0:d58274531d38 136 data[2]=(float)temp[2] / 4096.0 * 9.81;
ishiyamayuto 0:d58274531d38 137 }
ishiyamayuto 0:d58274531d38 138 if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_16G){
ishiyamayuto 0:d58274531d38 139 data[0]=(float)temp[0] / 2048.0 * 9.81;
ishiyamayuto 0:d58274531d38 140 data[1]=(float)temp[1] / 2048.0 * 9.81;
ishiyamayuto 0:d58274531d38 141 data[2]=(float)temp[2] / 2048.0 * 9.81;
ishiyamayuto 0:d58274531d38 142 }
ishiyamayuto 0:d58274531d38 143
ishiyamayuto 0:d58274531d38 144 #ifdef DOUBLE_ACCELERO
ishiyamayuto 0:d58274531d38 145 data[0]*=2;
ishiyamayuto 0:d58274531d38 146 data[1]*=2;
ishiyamayuto 0:d58274531d38 147 data[2]*=2;
ishiyamayuto 0:d58274531d38 148 #endif
ishiyamayuto 0:d58274531d38 149 }
ishiyamayuto 0:d58274531d38 150
ishiyamayuto 0:d58274531d38 151 //--------------------------------------------------
ishiyamayuto 0:d58274531d38 152 //------------------Gyroscope-----------------------
ishiyamayuto 0:d58274531d38 153 //--------------------------------------------------
ishiyamayuto 0:d58274531d38 154 void MPU6050::setGyroRange( char range ) {
ishiyamayuto 0:d58274531d38 155 char temp;
ishiyamayuto 0:d58274531d38 156 currentGyroRange = range;
ishiyamayuto 0:d58274531d38 157 range = range & 0x03;
ishiyamayuto 0:d58274531d38 158 temp = this->read(MPU6050_GYRO_CONFIG_REG);
ishiyamayuto 0:d58274531d38 159 temp &= ~(3<<3);
ishiyamayuto 0:d58274531d38 160 temp = temp + range<<3;
ishiyamayuto 0:d58274531d38 161 this->write(MPU6050_GYRO_CONFIG_REG, temp);
ishiyamayuto 0:d58274531d38 162 }
ishiyamayuto 0:d58274531d38 163
ishiyamayuto 0:d58274531d38 164 int MPU6050::getGyroRawX( void ) {
ishiyamayuto 0:d58274531d38 165 short retval;
ishiyamayuto 0:d58274531d38 166 char data[2];
ishiyamayuto 0:d58274531d38 167 this->read(MPU6050_GYRO_XOUT_H_REG, data, 2);
ishiyamayuto 0:d58274531d38 168 retval = (data[0]<<8) + data[1];
ishiyamayuto 0:d58274531d38 169 return (int)retval;
ishiyamayuto 0:d58274531d38 170 }
ishiyamayuto 0:d58274531d38 171
ishiyamayuto 0:d58274531d38 172 int MPU6050::getGyroRawY( void ) {
ishiyamayuto 0:d58274531d38 173 short retval;
ishiyamayuto 0:d58274531d38 174 char data[2];
ishiyamayuto 0:d58274531d38 175 this->read(MPU6050_GYRO_YOUT_H_REG, data, 2);
ishiyamayuto 0:d58274531d38 176 retval = (data[0]<<8) + data[1];
ishiyamayuto 0:d58274531d38 177 return (int)retval;
ishiyamayuto 0:d58274531d38 178 }
ishiyamayuto 0:d58274531d38 179
ishiyamayuto 0:d58274531d38 180 int MPU6050::getGyroRawZ( void ) {
ishiyamayuto 0:d58274531d38 181 short retval;
ishiyamayuto 0:d58274531d38 182 char data[2];
ishiyamayuto 0:d58274531d38 183 this->read(MPU6050_GYRO_ZOUT_H_REG, data, 2);
ishiyamayuto 0:d58274531d38 184 retval = (data[0]<<8) + data[1];
ishiyamayuto 0:d58274531d38 185 return (int)retval;
ishiyamayuto 0:d58274531d38 186 }
ishiyamayuto 0:d58274531d38 187
ishiyamayuto 0:d58274531d38 188 void MPU6050::getGyroRaw( int *data ) {
ishiyamayuto 0:d58274531d38 189 char temp[6];
ishiyamayuto 0:d58274531d38 190 this->read(MPU6050_GYRO_XOUT_H_REG, temp, 6);
ishiyamayuto 0:d58274531d38 191 data[0] = (int)(short)((temp[0]<<8) + temp[1]);
ishiyamayuto 0:d58274531d38 192 data[1] = (int)(short)((temp[2]<<8) + temp[3]);
ishiyamayuto 0:d58274531d38 193 data[2] = (int)(short)((temp[4]<<8) + temp[5]);
ishiyamayuto 0:d58274531d38 194 }
ishiyamayuto 0:d58274531d38 195
ishiyamayuto 0:d58274531d38 196 void MPU6050::getGyro( float *data ) {
ishiyamayuto 0:d58274531d38 197 int temp[3];
ishiyamayuto 0:d58274531d38 198 this->getGyroRaw(temp);
ishiyamayuto 0:d58274531d38 199 if (currentGyroRange == MPU6050_GYRO_RANGE_250) {
ishiyamayuto 0:d58274531d38 200 data[0]=(float)temp[0] / 7505.7;
ishiyamayuto 0:d58274531d38 201 data[1]=(float)temp[1] / 7505.7;
ishiyamayuto 0:d58274531d38 202 data[2]=(float)temp[2] / 7505.7;
ishiyamayuto 0:d58274531d38 203 }
ishiyamayuto 0:d58274531d38 204 if (currentGyroRange == MPU6050_GYRO_RANGE_500){
ishiyamayuto 0:d58274531d38 205 data[0]=(float)temp[0] / 3752.9;
ishiyamayuto 0:d58274531d38 206 data[1]=(float)temp[1] / 3752.9;
ishiyamayuto 0:d58274531d38 207 data[2]=(float)temp[2] / 3752.9;
ishiyamayuto 0:d58274531d38 208 }
ishiyamayuto 0:d58274531d38 209 if (currentGyroRange == MPU6050_GYRO_RANGE_1000){
ishiyamayuto 0:d58274531d38 210 data[0]=(float)temp[0] / 1879.3;;
ishiyamayuto 0:d58274531d38 211 data[1]=(float)temp[1] / 1879.3;
ishiyamayuto 0:d58274531d38 212 data[2]=(float)temp[2] / 1879.3;
ishiyamayuto 0:d58274531d38 213 }
ishiyamayuto 0:d58274531d38 214 if (currentGyroRange == MPU6050_GYRO_RANGE_2000){
ishiyamayuto 0:d58274531d38 215 data[0]=(float)temp[0] / 939.7;
ishiyamayuto 0:d58274531d38 216 data[1]=(float)temp[1] / 939.7;
ishiyamayuto 0:d58274531d38 217 data[2]=(float)temp[2] / 939.7;
ishiyamayuto 0:d58274531d38 218 }
ishiyamayuto 0:d58274531d38 219 }
ishiyamayuto 0:d58274531d38 220 //--------------------------------------------------
ishiyamayuto 0:d58274531d38 221 //-------------------Temperature--------------------
ishiyamayuto 0:d58274531d38 222 //--------------------------------------------------
ishiyamayuto 0:d58274531d38 223 int MPU6050::getTempRaw( void ) {
ishiyamayuto 0:d58274531d38 224 short retval;
ishiyamayuto 0:d58274531d38 225 char data[2];
ishiyamayuto 0:d58274531d38 226 this->read(MPU6050_TEMP_H_REG, data, 2);
ishiyamayuto 0:d58274531d38 227 retval = (data[0]<<8) + data[1];
ishiyamayuto 0:d58274531d38 228 return (int)retval;
ishiyamayuto 0:d58274531d38 229 }
ishiyamayuto 0:d58274531d38 230
ishiyamayuto 0:d58274531d38 231 float MPU6050::getTemp( void ) {
ishiyamayuto 0:d58274531d38 232 float retval;
ishiyamayuto 0:d58274531d38 233 retval=(float)this->getTempRaw();
ishiyamayuto 0:d58274531d38 234 retval=(retval+521.0)/340.0+35.0;
ishiyamayuto 0:d58274531d38 235 return retval;
ishiyamayuto 0:d58274531d38 236 }
ishiyamayuto 0:d58274531d38 237