shalabh bhatnagar
This acceleration sensor and gyro sensor is attached to human arm and hand which detect the motion of human hand and arm in 6 axis.Signal of acceleration sensor and gyro sensor is very volatile. To convert it into readable form gravitational equation were used and to make it stable several filters were used. This all processing was done using FRDM K64 board and an output signal was sent to Servo motor to give motion to robotic arm. Robotic arm is capable of picking an object and placing it in nearby location with 3 DOF by replicating motion of human arm.
Dependencies: ADXL345 Servo mbed
Fork of
frdm_robotiarm
by 
shalabh bhatnagar
Embed:
(wiki syntax)
Show/hide line numbers
MPU6050.h
00001 #ifndef MPU6050_H 00002 #define MPU6050_H 00003 00004 #include "mbed.h" 00005 #include "math.h" 00006 00007 // Define registers per MPU6050, Register Map and Descriptions, 6 DOF Motion sensor fusion device 00008 // See also MPU-6050 Register Map and Descriptions, Revision 4.0, RM-MPU-6050A-00, 9/12/2012 for registers not listed in 00009 // above document; the MPU6050 and MPU 9150 are virtually identical but the latter has an on-board magnetic sensor 00010 // 00011 #define XGOFFS_TC 0x00 // Bit 7 PWR_MODE, bits 6:1 XG_OFFS_TC, bit 0 OTP_BNK_VLD 00012 #define YGOFFS_TC 0x01 00013 #define ZGOFFS_TC 0x02 00014 #define X_FINE_GAIN 0x03 // [7:0] fine gain 00015 #define Y_FINE_GAIN 0x04 00016 #define Z_FINE_GAIN 0x05 00017 #define XA_OFFSET_H 0x06 // User-defined trim values for accelerometer 00018 #define XA_OFFSET_L_TC 0x07 00019 #define YA_OFFSET_H 0x08 00020 #define YA_OFFSET_L_TC 0x09 00021 #define ZA_OFFSET_H 0x0A 00022 #define ZA_OFFSET_L_TC 0x0B 00023 #define SELF_TEST_X 0x0D 00024 #define SELF_TEST_Y 0x0E 00025 #define SELF_TEST_Z 0x0F 00026 #define SELF_TEST_A 0x10 00027 #define XG_OFFS_USRH 0x13 // User-defined trim values for gyroscope; supported in MPU-6050? 00028 #define XG_OFFS_USRL 0x14 00029 #define YG_OFFS_USRH 0x15 00030 #define YG_OFFS_USRL 0x16 00031 #define ZG_OFFS_USRH 0x17 00032 #define ZG_OFFS_USRL 0x18 00033 #define SMPLRT_DIV 0x19 00034 #define CONFIG 0x1A 00035 #define GYRO_CONFIG 0x1B 00036 #define ACCEL_CONFIG 0x1C 00037 #define FF_THR 0x1D // Free-fall 00038 #define FF_DUR 0x1E // Free-fall 00039 #define MOT_THR 0x1F // Motion detection threshold bits [7:0] 00040 #define MOT_DUR 0x20 // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms 00041 #define ZMOT_THR 0x21 // Zero-motion detection threshold bits [7:0] 00042 #define ZRMOT_DUR 0x22 // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms 00043 #define FIFO_EN 0x23 00044 #define I2C_MST_CTRL 0x24 00045 #define I2C_SLV0_ADDR 0x25 00046 #define I2C_SLV0_REG 0x26 00047 #define I2C_SLV0_CTRL 0x27 00048 #define I2C_SLV1_ADDR 0x28 00049 #define I2C_SLV1_REG 0x29 00050 #define I2C_SLV1_CTRL 0x2A 00051 #define I2C_SLV2_ADDR 0x2B 00052 #define I2C_SLV2_REG 0x2C 00053 #define I2C_SLV2_CTRL 0x2D 00054 #define I2C_SLV3_ADDR 0x2E 00055 #define I2C_SLV3_REG 0x2F 00056 #define I2C_SLV3_CTRL 0x30 00057 #define I2C_SLV4_ADDR 0x31 00058 #define I2C_SLV4_REG 0x32 00059 #define I2C_SLV4_DO 0x33 00060 #define I2C_SLV4_CTRL 0x34 00061 #define I2C_SLV4_DI 0x35 00062 #define I2C_MST_STATUS 0x36 00063 #define INT_PIN_CFG 0x37 00064 #define INT_ENABLE 0x38 00065 #define DMP_INT_STATUS 0x39 // Check DMP interrupt 00066 #define INT_STATUS 0x3A 00067 #define ACCEL_XOUT_H 0x3B 00068 #define ACCEL_XOUT_L 0x3C 00069 #define ACCEL_YOUT_H 0x3D 00070 #define ACCEL_YOUT_L 0x3E 00071 #define ACCEL_ZOUT_H 0x3F 00072 #define ACCEL_ZOUT_L 0x40 00073 #define TEMP_OUT_H 0x41 00074 #define TEMP_OUT_L 0x42 00075 #define GYRO_XOUT_H 0x43 00076 #define GYRO_XOUT_L 0x44 00077 #define GYRO_YOUT_H 0x45 00078 #define GYRO_YOUT_L 0x46 00079 #define GYRO_ZOUT_H 0x47 00080 #define GYRO_ZOUT_L 0x48 00081 #define EXT_SENS_DATA_00 0x49 00082 #define EXT_SENS_DATA_01 0x4A 00083 #define EXT_SENS_DATA_02 0x4B 00084 #define EXT_SENS_DATA_03 0x4C 00085 #define EXT_SENS_DATA_04 0x4D 00086 #define EXT_SENS_DATA_05 0x4E 00087 #define EXT_SENS_DATA_06 0x4F 00088 #define EXT_SENS_DATA_07 0x50 00089 #define EXT_SENS_DATA_08 0x51 00090 #define EXT_SENS_DATA_09 0x52 00091 #define EXT_SENS_DATA_10 0x53 00092 #define EXT_SENS_DATA_11 0x54 00093 #define EXT_SENS_DATA_12 0x55 00094 #define EXT_SENS_DATA_13 0x56 00095 #define EXT_SENS_DATA_14 0x57 00096 #define EXT_SENS_DATA_15 0x58 00097 #define EXT_SENS_DATA_16 0x59 00098 #define EXT_SENS_DATA_17 0x5A 00099 #define EXT_SENS_DATA_18 0x5B 00100 #define EXT_SENS_DATA_19 0x5C 00101 #define EXT_SENS_DATA_20 0x5D 00102 #define EXT_SENS_DATA_21 0x5E 00103 #define EXT_SENS_DATA_22 0x5F 00104 #define EXT_SENS_DATA_23 0x60 00105 #define MOT_DETECT_STATUS 0x61 00106 #define I2C_SLV0_DO 0x63 00107 #define I2C_SLV1_DO 0x64 00108 #define I2C_SLV2_DO 0x65 00109 #define I2C_SLV3_DO 0x66 00110 #define I2C_MST_DELAY_CTRL 0x67 00111 #define SIGNAL_PATH_RESET 0x68 00112 #define MOT_DETECT_CTRL 0x69 00113 #define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP 00114 #define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode 00115 #define PWR_MGMT_2 0x6C 00116 #define DMP_BANK 0x6D // Activates a specific bank in the DMP 00117 #define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank 00118 #define DMP_REG 0x6F // Register in DMP from which to read or to which to write 00119 #define DMP_REG_1 0x70 00120 #define DMP_REG_2 0x71 00121 #define FIFO_COUNTH 0x72 00122 #define FIFO_COUNTL 0x73 00123 #define FIFO_R_W 0x74 00124 #define WHO_AM_I_MPU6050 0x75 // Should return 0x68 00125 00126 // Using the GY-521 breakout board, I set ADO to 0 by grounding through a 4k7 resistor 00127 // Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1 00128 #define ADO 0 00129 #if ADO 00130 #define MPU6050_ADDRESS 0x69<<1 // Device address when ADO = 1 00131 #else 00132 #define MPU6050_ADDRESS 0x68<<1 // Device address when ADO = 0 00133 #endif 00134 00135 // Set initial input parameters 00136 enum Ascale { 00137 AFS_2G = 0, 00138 AFS_4G, 00139 AFS_8G, 00140 AFS_16G 00141 }; 00142 00143 enum Gscale { 00144 GFS_250DPS = 0, 00145 GFS_500DPS, 00146 GFS_1000DPS, 00147 GFS_2000DPS 00148 }; 00149 00150 // Specify sensor full scale 00151 int Gscale = GFS_250DPS; 00152 int Ascale = AFS_2G; 00153 00154 //Set up I2C, (SDA,SCL) 00155 I2C i2c(I2C_SDA, I2C_SCL); 00156 00157 DigitalOut myled(LED1); 00158 00159 float aRes, gRes; // scale resolutions per LSB for the sensors 00160 00161 // Pin definitions 00162 int intPin = 12; // These can be changed, 2 and 3 are the Arduinos ext int pins 00163 00164 int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output 00165 float ax, ay, az; // Stores the real accel value in g's 00166 int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output 00167 float gx, gy, gz; // Stores the real gyro value in degrees per seconds 00168 float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}; // Bias corrections for gyro and accelerometer 00169 int16_t tempCount; // Stores the real internal chip temperature in degrees Celsius 00170 float temperature; 00171 float SelfTest[6]; 00172 00173 int delt_t = 0; // used to control display output rate 00174 int count = 0; // used to control display output rate 00175 00176 // parameters for 6 DoF sensor fusion calculations 00177 float PI = 3.14159265358979323846f; 00178 float GyroMeasError = PI * (60.0f / 180.0f); // gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3 00179 float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta 00180 float GyroMeasDrift = PI * (1.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) 00181 float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value 00182 float pitch, yaw, roll; 00183 float deltat = 0.0f; // integration interval for both filter schemes 00184 int lastUpdate = 0, firstUpdate = 0, Now = 0; // used to calculate integration interval // used to calculate integration interval 00185 float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion 00186 00187 class MPU6050 { 00188 00189 protected: 00190 00191 public: 00192 //=================================================================================================================== 00193 //====== Set of useful function to access acceleratio, gyroscope, and temperature data 00194 //=================================================================================================================== 00195 00196 void writeByte(uint8_t address, uint8_t subAddress, uint8_t data) 00197 { 00198 char data_write[2]; 00199 data_write[0] = subAddress; 00200 data_write[1] = data; 00201 i2c.write(address, data_write, 2, 0); 00202 } 00203 00204 char readByte(uint8_t address, uint8_t subAddress) 00205 { 00206 char data[1]; // `data` will store the register data 00207 char data_write[1]; 00208 data_write[0] = subAddress; 00209 i2c.write(address, data_write, 1, 1); // no stop 00210 i2c.read(address, data, 1, 0); 00211 return data[0]; 00212 } 00213 00214 void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest) 00215 { 00216 char data[14]; 00217 char data_write[1]; 00218 data_write[0] = subAddress; 00219 i2c.write(address, data_write, 1, 1); // no stop 00220 i2c.read(address, data, count, 0); 00221 for(int ii = 0; ii < count; ii++) { 00222 dest[ii] = data[ii]; 00223 } 00224 } 00225 00226 00227 void getGres() { 00228 switch (Gscale) 00229 { 00230 // Possible gyro scales (and their register bit settings) are: 00231 // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11). 00232 // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: 00233 case GFS_250DPS: 00234 gRes = 250.0/32768.0; 00235 break; 00236 case GFS_500DPS: 00237 gRes = 500.0/32768.0; 00238 break; 00239 case GFS_1000DPS: 00240 gRes = 1000.0/32768.0; 00241 break; 00242 case GFS_2000DPS: 00243 gRes = 2000.0/32768.0; 00244 break; 00245 } 00246 } 00247 00248 void getAres() { 00249 switch (Ascale) 00250 { 00251 // Possible accelerometer scales (and their register bit settings) are: 00252 // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11). 00253 // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: 00254 case AFS_2G: 00255 aRes = 2.0/32768.0; 00256 break; 00257 case AFS_4G: 00258 aRes = 4.0/32768.0; 00259 break; 00260 case AFS_8G: 00261 aRes = 8.0/32768.0; 00262 break; 00263 case AFS_16G: 00264 aRes = 16.0/32768.0; 00265 break; 00266 } 00267 } 00268 00269 00270 void readAccelData(int16_t * destination) 00271 { 00272 uint8_t rawData[6]; // x/y/z accel register data stored here 00273 readBytes(MPU6050_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array 00274 destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value 00275 destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ; 00276 destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; 00277 } 00278 00279 void readGyroData(int16_t * destination) 00280 { 00281 uint8_t rawData[6]; // x/y/z gyro register data stored here 00282 readBytes(MPU6050_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array 00283 destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value 00284 destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ; 00285 destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; 00286 } 00287 00288 int16_t readTempData() 00289 { 00290 uint8_t rawData[2]; // x/y/z gyro register data stored here 00291 readBytes(MPU6050_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); // Read the two raw data registers sequentially into data array 00292 return (int16_t)(((int16_t)rawData[0]) << 8 | rawData[1]) ; // Turn the MSB and LSB into a 16-bit value 00293 } 00294 00295 00296 00297 // Configure the motion detection control for low power accelerometer mode 00298 void LowPowerAccelOnly() 00299 { 00300 00301 // The sensor has a high-pass filter necessary to invoke to allow the sensor motion detection algorithms work properly 00302 // Motion detection occurs on free-fall (acceleration below a threshold for some time for all axes), motion (acceleration 00303 // above a threshold for some time on at least one axis), and zero-motion toggle (acceleration on each axis less than a 00304 // threshold for some time sets this flag, motion above the threshold turns it off). The high-pass filter takes gravity out 00305 // consideration for these threshold evaluations; otherwise, the flags would be set all the time! 00306 00307 uint8_t c = readByte(MPU6050_ADDRESS, PWR_MGMT_1); 00308 writeByte(MPU6050_ADDRESS, PWR_MGMT_1, c & ~0x30); // Clear sleep and cycle bits [5:6] 00309 writeByte(MPU6050_ADDRESS, PWR_MGMT_1, c | 0x30); // Set sleep and cycle bits [5:6] to zero to make sure accelerometer is running 00310 00311 c = readByte(MPU6050_ADDRESS, PWR_MGMT_2); 00312 writeByte(MPU6050_ADDRESS, PWR_MGMT_2, c & ~0x38); // Clear standby XA, YA, and ZA bits [3:5] 00313 writeByte(MPU6050_ADDRESS, PWR_MGMT_2, c | 0x00); // Set XA, YA, and ZA bits [3:5] to zero to make sure accelerometer is running 00314 00315 c = readByte(MPU6050_ADDRESS, ACCEL_CONFIG); 00316 writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c & ~0x07); // Clear high-pass filter bits [2:0] 00317 // Set high-pass filter to 0) reset (disable), 1) 5 Hz, 2) 2.5 Hz, 3) 1.25 Hz, 4) 0.63 Hz, or 7) Hold 00318 writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c | 0x00); // Set ACCEL_HPF to 0; reset mode disbaling high-pass filter 00319 00320 c = readByte(MPU6050_ADDRESS, CONFIG); 00321 writeByte(MPU6050_ADDRESS, CONFIG, c & ~0x07); // Clear low-pass filter bits [2:0] 00322 writeByte(MPU6050_ADDRESS, CONFIG, c | 0x00); // Set DLPD_CFG to 0; 260 Hz bandwidth, 1 kHz rate 00323 00324 c = readByte(MPU6050_ADDRESS, INT_ENABLE); 00325 writeByte(MPU6050_ADDRESS, INT_ENABLE, c & ~0xFF); // Clear all interrupts 00326 writeByte(MPU6050_ADDRESS, INT_ENABLE, 0x40); // Enable motion threshold (bits 5) interrupt only 00327 00328 // Motion detection interrupt requires the absolute value of any axis to lie above the detection threshold 00329 // for at least the counter duration 00330 writeByte(MPU6050_ADDRESS, MOT_THR, 0x80); // Set motion detection to 0.256 g; LSB = 2 mg 00331 writeByte(MPU6050_ADDRESS, MOT_DUR, 0x01); // Set motion detect duration to 1 ms; LSB is 1 ms @ 1 kHz rate 00332 00333 wait(0.1); // Add delay for accumulation of samples 00334 00335 c = readByte(MPU6050_ADDRESS, ACCEL_CONFIG); 00336 writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c & ~0x07); // Clear high-pass filter bits [2:0] 00337 writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c | 0x07); // Set ACCEL_HPF to 7; hold the initial accleration value as a referance 00338 00339 c = readByte(MPU6050_ADDRESS, PWR_MGMT_2); 00340 writeByte(MPU6050_ADDRESS, PWR_MGMT_2, c & ~0xC7); // Clear standby XA, YA, and ZA bits [3:5] and LP_WAKE_CTRL bits [6:7] 00341 writeByte(MPU6050_ADDRESS, PWR_MGMT_2, c | 0x47); // Set wakeup frequency to 5 Hz, and disable XG, YG, and ZG gyros (bits [0:2]) 00342 00343 c = readByte(MPU6050_ADDRESS, PWR_MGMT_1); 00344 writeByte(MPU6050_ADDRESS, PWR_MGMT_1, c & ~0x20); // Clear sleep and cycle bit 5 00345 writeByte(MPU6050_ADDRESS, PWR_MGMT_1, c | 0x20); // Set cycle bit 5 to begin low power accelerometer motion interrupts 00346 00347 } 00348 00349 00350 void resetMPU6050() { 00351 // reset device 00352 writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device 00353 wait(0.1); 00354 } 00355 00356 00357 void initMPU6050() 00358 { 00359 // Initialize MPU6050 device 00360 // wake up device 00361 writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors 00362 wait(0.1); // Delay 100 ms for PLL to get established on x-axis gyro; should check for PLL ready interrupt 00363 00364 // get stable time source 00365 writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x01); // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001 00366 00367 // Configure Gyro and Accelerometer 00368 // Disable FSYNC and set accelerometer and gyro bandwidth to 44 and 42 Hz, respectively; 00369 // DLPF_CFG = bits 2:0 = 010; this sets the sample rate at 1 kHz for both 00370 // Maximum delay is 4.9 ms which is just over a 200 Hz maximum rate 00371 writeByte(MPU6050_ADDRESS, CONFIG, 0x03); 00372 00373 // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV) 00374 writeByte(MPU6050_ADDRESS, SMPLRT_DIV, 0x04); // Use a 200 Hz rate; the same rate set in CONFIG above 00375 00376 // Set gyroscope full scale range 00377 // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3 00378 uint8_t c = readByte(MPU6050_ADDRESS, GYRO_CONFIG); 00379 writeByte(MPU6050_ADDRESS, GYRO_CONFIG, c & ~0xE0); // Clear self-test bits [7:5] 00380 writeByte(MPU6050_ADDRESS, GYRO_CONFIG, c & ~0x18); // Clear AFS bits [4:3] 00381 writeByte(MPU6050_ADDRESS, GYRO_CONFIG, c | Gscale << 3); // Set full scale range for the gyro 00382 00383 // Set accelerometer configuration 00384 c = readByte(MPU6050_ADDRESS, ACCEL_CONFIG); 00385 writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c & ~0xE0); // Clear self-test bits [7:5] 00386 writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c & ~0x18); // Clear AFS bits [4:3] 00387 writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c | Ascale << 3); // Set full scale range for the accelerometer 00388 00389 // Configure Interrupts and Bypass Enable 00390 // Set interrupt pin active high, push-pull, and clear on read of INT_STATUS, enable I2C_BYPASS_EN so additional chips 00391 // can join the I2C bus and all can be controlled by the Arduino as master 00392 writeByte(MPU6050_ADDRESS, INT_PIN_CFG, 0x22); 00393 writeByte(MPU6050_ADDRESS, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt 00394 } 00395 00396 // Function which accumulates gyro and accelerometer data after device initialization. It calculates the average 00397 // of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers. 00398 void calibrateMPU6050(float * dest1, float * dest2) 00399 { 00400 uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data 00401 uint16_t ii, packet_count, fifo_count; 00402 int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0}; 00403 00404 // reset device, reset all registers, clear gyro and accelerometer bias registers 00405 writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device 00406 wait(0.1); 00407 00408 // get stable time source 00409 // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001 00410 writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x01); 00411 writeByte(MPU6050_ADDRESS, PWR_MGMT_2, 0x00); 00412 wait(0.2); 00413 00414 // Configure device for bias calculation 00415 writeByte(MPU6050_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts 00416 writeByte(MPU6050_ADDRESS, FIFO_EN, 0x00); // Disable FIFO 00417 writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source 00418 writeByte(MPU6050_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master 00419 writeByte(MPU6050_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes 00420 writeByte(MPU6050_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP 00421 wait(0.015); 00422 00423 // Configure MPU6050 gyro and accelerometer for bias calculation 00424 writeByte(MPU6050_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz 00425 writeByte(MPU6050_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz 00426 writeByte(MPU6050_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity 00427 writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity 00428 00429 uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec 00430 uint16_t accelsensitivity = 16384; // = 16384 LSB/g 00431 00432 // Configure FIFO to capture accelerometer and gyro data for bias calculation 00433 writeByte(MPU6050_ADDRESS, USER_CTRL, 0x40); // Enable FIFO 00434 writeByte(MPU6050_ADDRESS, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 1024 bytes in MPU-6050) 00435 wait(0.08); // accumulate 80 samples in 80 milliseconds = 960 bytes 00436 00437 // At end of sample accumulation, turn off FIFO sensor read 00438 writeByte(MPU6050_ADDRESS, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO 00439 readBytes(MPU6050_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count 00440 fifo_count = ((uint16_t)data[0] << 8) | data[1]; 00441 packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging 00442 00443 for (ii = 0; ii < packet_count; ii++) { 00444 int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0}; 00445 readBytes(MPU6050_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging 00446 accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ; // Form signed 16-bit integer for each sample in FIFO 00447 accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ; 00448 accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ; 00449 gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ; 00450 gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ; 00451 gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ; 00452 00453 accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases 00454 accel_bias[1] += (int32_t) accel_temp[1]; 00455 accel_bias[2] += (int32_t) accel_temp[2]; 00456 gyro_bias[0] += (int32_t) gyro_temp[0]; 00457 gyro_bias[1] += (int32_t) gyro_temp[1]; 00458 gyro_bias[2] += (int32_t) gyro_temp[2]; 00459 00460 } 00461 accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases 00462 accel_bias[1] /= (int32_t) packet_count; 00463 accel_bias[2] /= (int32_t) packet_count; 00464 gyro_bias[0] /= (int32_t) packet_count; 00465 gyro_bias[1] /= (int32_t) packet_count; 00466 gyro_bias[2] /= (int32_t) packet_count; 00467 00468 if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;} // Remove gravity from the z-axis accelerometer bias calculation 00469 else {accel_bias[2] += (int32_t) accelsensitivity;} 00470 00471 // Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup 00472 data[0] = (-gyro_bias[0]/4 >> 8) & 0xFF; // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format 00473 data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases 00474 data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF; 00475 data[3] = (-gyro_bias[1]/4) & 0xFF; 00476 data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF; 00477 data[5] = (-gyro_bias[2]/4) & 0xFF; 00478 00479 // Push gyro biases to hardware registers 00480 writeByte(MPU6050_ADDRESS, XG_OFFS_USRH, data[0]); 00481 writeByte(MPU6050_ADDRESS, XG_OFFS_USRL, data[1]); 00482 writeByte(MPU6050_ADDRESS, YG_OFFS_USRH, data[2]); 00483 writeByte(MPU6050_ADDRESS, YG_OFFS_USRL, data[3]); 00484 writeByte(MPU6050_ADDRESS, ZG_OFFS_USRH, data[4]); 00485 writeByte(MPU6050_ADDRESS, ZG_OFFS_USRL, data[5]); 00486 00487 dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity; // construct gyro bias in deg/s for later manual subtraction 00488 dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity; 00489 dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity; 00490 00491 // Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain 00492 // factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold 00493 // non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature 00494 // compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that 00495 // the accelerometer biases calculated above must be divided by 8. 00496 00497 int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases 00498 readBytes(MPU6050_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values 00499 accel_bias_reg[0] = (int16_t) ((int16_t)data[0] << 8) | data[1]; 00500 readBytes(MPU6050_ADDRESS, YA_OFFSET_H, 2, &data[0]); 00501 accel_bias_reg[1] = (int16_t) ((int16_t)data[0] << 8) | data[1]; 00502 readBytes(MPU6050_ADDRESS, ZA_OFFSET_H, 2, &data[0]); 00503 accel_bias_reg[2] = (int16_t) ((int16_t)data[0] << 8) | data[1]; 00504 00505 uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers 00506 uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis 00507 00508 for(ii = 0; ii < 3; ii++) { 00509 if(accel_bias_reg[ii] & mask) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit 00510 } 00511 00512 // Construct total accelerometer bias, including calculated average accelerometer bias from above 00513 accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale) 00514 accel_bias_reg[1] -= (accel_bias[1]/8); 00515 accel_bias_reg[2] -= (accel_bias[2]/8); 00516 00517 data[0] = (accel_bias_reg[0] >> 8) & 0xFF; 00518 data[1] = (accel_bias_reg[0]) & 0xFF; 00519 data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers 00520 data[2] = (accel_bias_reg[1] >> 8) & 0xFF; 00521 data[3] = (accel_bias_reg[1]) & 0xFF; 00522 data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers 00523 data[4] = (accel_bias_reg[2] >> 8) & 0xFF; 00524 data[5] = (accel_bias_reg[2]) & 0xFF; 00525 data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers 00526 00527 // Push accelerometer biases to hardware registers 00528 // writeByte(MPU6050_ADDRESS, XA_OFFSET_H, data[0]); 00529 // writeByte(MPU6050_ADDRESS, XA_OFFSET_L_TC, data[1]); 00530 // writeByte(MPU6050_ADDRESS, YA_OFFSET_H, data[2]); 00531 // writeByte(MPU6050_ADDRESS, YA_OFFSET_L_TC, data[3]); 00532 // writeByte(MPU6050_ADDRESS, ZA_OFFSET_H, data[4]); 00533 // writeByte(MPU6050_ADDRESS, ZA_OFFSET_L_TC, data[5]); 00534 00535 // Output scaled accelerometer biases for manual subtraction in the main program 00536 dest2[0] = (float)accel_bias[0]/(float)accelsensitivity; 00537 dest2[1] = (float)accel_bias[1]/(float)accelsensitivity; 00538 dest2[2] = (float)accel_bias[2]/(float)accelsensitivity; 00539 } 00540 00541 00542 // Accelerometer and gyroscope self test; check calibration wrt factory settings 00543 void MPU6050SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass 00544 { 00545 uint8_t rawData[4] = {0, 0, 0, 0}; 00546 uint8_t selfTest[6]; 00547 float factoryTrim[6]; 00548 00549 // Configure the accelerometer for self-test 00550 writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, 0xF0); // Enable self test on all three axes and set accelerometer range to +/- 8 g 00551 writeByte(MPU6050_ADDRESS, GYRO_CONFIG, 0xE0); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s 00552 wait(0.25); // Delay a while to let the device execute the self-test 00553 rawData[0] = readByte(MPU6050_ADDRESS, SELF_TEST_X); // X-axis self-test results 00554 rawData[1] = readByte(MPU6050_ADDRESS, SELF_TEST_Y); // Y-axis self-test results 00555 rawData[2] = readByte(MPU6050_ADDRESS, SELF_TEST_Z); // Z-axis self-test results 00556 rawData[3] = readByte(MPU6050_ADDRESS, SELF_TEST_A); // Mixed-axis self-test results 00557 // Extract the acceleration test results first 00558 selfTest[0] = (rawData[0] >> 3) | (rawData[3] & 0x30) >> 4 ; // XA_TEST result is a five-bit unsigned integer 00559 selfTest[1] = (rawData[1] >> 3) | (rawData[3] & 0x0C) >> 4 ; // YA_TEST result is a five-bit unsigned integer 00560 selfTest[2] = (rawData[2] >> 3) | (rawData[3] & 0x03) >> 4 ; // ZA_TEST result is a five-bit unsigned integer 00561 // Extract the gyration test results first 00562 selfTest[3] = rawData[0] & 0x1F ; // XG_TEST result is a five-bit unsigned integer 00563 selfTest[4] = rawData[1] & 0x1F ; // YG_TEST result is a five-bit unsigned integer 00564 selfTest[5] = rawData[2] & 0x1F ; // ZG_TEST result is a five-bit unsigned integer 00565 // Process results to allow final comparison with factory set values 00566 factoryTrim[0] = (4096.0f*0.34f)*(pow( (0.92f/0.34f) , ((selfTest[0] - 1.0f)/30.0f))); // FT[Xa] factory trim calculation 00567 factoryTrim[1] = (4096.0f*0.34f)*(pow( (0.92f/0.34f) , ((selfTest[1] - 1.0f)/30.0f))); // FT[Ya] factory trim calculation 00568 factoryTrim[2] = (4096.0f*0.34f)*(pow( (0.92f/0.34f) , ((selfTest[2] - 1.0f)/30.0f))); // FT[Za] factory trim calculation 00569 factoryTrim[3] = ( 25.0f*131.0f)*(pow( 1.046f , (selfTest[3] - 1.0f) )); // FT[Xg] factory trim calculation 00570 factoryTrim[4] = (-25.0f*131.0f)*(pow( 1.046f , (selfTest[4] - 1.0f) )); // FT[Yg] factory trim calculation 00571 factoryTrim[5] = ( 25.0f*131.0f)*(pow( 1.046f , (selfTest[5] - 1.0f) )); // FT[Zg] factory trim calculation 00572 00573 // Output self-test results and factory trim calculation if desired 00574 // Serial.println(selfTest[0]); Serial.println(selfTest[1]); Serial.println(selfTest[2]); 00575 // Serial.println(selfTest[3]); Serial.println(selfTest[4]); Serial.println(selfTest[5]); 00576 // Serial.println(factoryTrim[0]); Serial.println(factoryTrim[1]); Serial.println(factoryTrim[2]); 00577 // Serial.println(factoryTrim[3]); Serial.println(factoryTrim[4]); Serial.println(factoryTrim[5]); 00578 00579 // Report results as a ratio of (STR - FT)/FT; the change from Factory Trim of the Self-Test Response 00580 // To get to percent, must multiply by 100 and subtract result from 100 00581 for (int i = 0; i < 6; i++) { 00582 destination[i] = 100.0f + 100.0f*(selfTest[i] - factoryTrim[i])/factoryTrim[i]; // Report percent differences 00583 } 00584 00585 } 00586 00587 00588 // Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays" 00589 // which fuses acceleration and rotation rate to produce a quaternion-based estimate of relative 00590 // device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc. 00591 // The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms 00592 // but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz! 00593 void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz) 00594 { 00595 float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability 00596 float norm; // vector norm 00597 float f1, f2, f3; // objective funcyion elements 00598 float J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33; // objective function Jacobian elements 00599 float qDot1, qDot2, qDot3, qDot4; 00600 float hatDot1, hatDot2, hatDot3, hatDot4; 00601 float gerrx, gerry, gerrz, gbiasx, gbiasy, gbiasz; // gyro bias error 00602 00603 // Auxiliary variables to avoid repeated arithmetic 00604 float _halfq1 = 0.5f * q1; 00605 float _halfq2 = 0.5f * q2; 00606 float _halfq3 = 0.5f * q3; 00607 float _halfq4 = 0.5f * q4; 00608 float _2q1 = 2.0f * q1; 00609 float _2q2 = 2.0f * q2; 00610 float _2q3 = 2.0f * q3; 00611 float _2q4 = 2.0f * q4; 00612 // float _2q1q3 = 2.0f * q1 * q3; 00613 // float _2q3q4 = 2.0f * q3 * q4; 00614 00615 // Normalise accelerometer measurement 00616 norm = sqrt(ax * ax + ay * ay + az * az); 00617 if (norm == 0.0f) return; // handle NaN 00618 norm = 1.0f/norm; 00619 ax *= norm; 00620 ay *= norm; 00621 az *= norm; 00622 00623 // Compute the objective function and Jacobian 00624 f1 = _2q2 * q4 - _2q1 * q3 - ax; 00625 f2 = _2q1 * q2 + _2q3 * q4 - ay; 00626 f3 = 1.0f - _2q2 * q2 - _2q3 * q3 - az; 00627 J_11or24 = _2q3; 00628 J_12or23 = _2q4; 00629 J_13or22 = _2q1; 00630 J_14or21 = _2q2; 00631 J_32 = 2.0f * J_14or21; 00632 J_33 = 2.0f * J_11or24; 00633 00634 // Compute the gradient (matrix multiplication) 00635 hatDot1 = J_14or21 * f2 - J_11or24 * f1; 00636 hatDot2 = J_12or23 * f1 + J_13or22 * f2 - J_32 * f3; 00637 hatDot3 = J_12or23 * f2 - J_33 *f3 - J_13or22 * f1; 00638 hatDot4 = J_14or21 * f1 + J_11or24 * f2; 00639 00640 // Normalize the gradient 00641 norm = sqrt(hatDot1 * hatDot1 + hatDot2 * hatDot2 + hatDot3 * hatDot3 + hatDot4 * hatDot4); 00642 hatDot1 /= norm; 00643 hatDot2 /= norm; 00644 hatDot3 /= norm; 00645 hatDot4 /= norm; 00646 00647 // Compute estimated gyroscope biases 00648 gerrx = _2q1 * hatDot2 - _2q2 * hatDot1 - _2q3 * hatDot4 + _2q4 * hatDot3; 00649 gerry = _2q1 * hatDot3 + _2q2 * hatDot4 - _2q3 * hatDot1 - _2q4 * hatDot2; 00650 gerrz = _2q1 * hatDot4 - _2q2 * hatDot3 + _2q3 * hatDot2 - _2q4 * hatDot1; 00651 00652 // Compute and remove gyroscope biases 00653 gbiasx += gerrx * deltat * zeta; 00654 gbiasy += gerry * deltat * zeta; 00655 gbiasz += gerrz * deltat * zeta; 00656 // gx -= gbiasx; 00657 // gy -= gbiasy; 00658 // gz -= gbiasz; 00659 00660 // Compute the quaternion derivative 00661 qDot1 = -_halfq2 * gx - _halfq3 * gy - _halfq4 * gz; 00662 qDot2 = _halfq1 * gx + _halfq3 * gz - _halfq4 * gy; 00663 qDot3 = _halfq1 * gy - _halfq2 * gz + _halfq4 * gx; 00664 qDot4 = _halfq1 * gz + _halfq2 * gy - _halfq3 * gx; 00665 00666 // Compute then integrate estimated quaternion derivative 00667 q1 += (qDot1 -(beta * hatDot1)) * deltat; 00668 q2 += (qDot2 -(beta * hatDot2)) * deltat; 00669 q3 += (qDot3 -(beta * hatDot3)) * deltat; 00670 q4 += (qDot4 -(beta * hatDot4)) * deltat; 00671 00672 // Normalize the quaternion 00673 norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion 00674 norm = 1.0f/norm; 00675 q[0] = q1 * norm; 00676 q[1] = q2 * norm; 00677 q[2] = q3 * norm; 00678 q[3] = q4 * norm; 00679 00680 } 00681 00682 00683 }; 00684 #endif
Generated on Fri Jul 22 2022 11:29:03 by
1.7.2