shalabh bhatnagar / Mbed 2 deprecated frdm_Helpinhand

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

/media/uploads/sdloh/img_0220.mov

MPU6050.h@2:8eb96314ced5, 2016-04-12 (annotated)

Committer:
sdloh
Date:
Tue Apr 12 16:24:49 2016 +0000
Revision:
2:8eb96314ced5
Parent:
1:33b6c490b3c9 
MPU6050 with FRDMk64

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sdloh 0:6100f19d230b 1 #ifndef MPU6050_H
sdloh 0:6100f19d230b 2 #define MPU6050_H
sdloh 0:6100f19d230b 3
sdloh 0:6100f19d230b 4 #include "mbed.h"
sdloh 0:6100f19d230b 5 #include "math.h"
sdloh 0:6100f19d230b 6
sdloh 1:33b6c490b3c9 7 // Define registers per MPU6050, Register Map and Descriptions, 6 DOF Motion sensor fusion device
sdloh 0:6100f19d230b 8 // See also MPU-6050 Register Map and Descriptions, Revision 4.0, RM-MPU-6050A-00, 9/12/2012 for registers not listed in
sdloh 0:6100f19d230b 9 // above document; the MPU6050 and MPU 9150 are virtually identical but the latter has an on-board magnetic sensor
sdloh 0:6100f19d230b 10 //
sdloh 0:6100f19d230b 11 #define XGOFFS_TC 0x00 // Bit 7 PWR_MODE, bits 6:1 XG_OFFS_TC, bit 0 OTP_BNK_VLD
sdloh 0:6100f19d230b 12 #define YGOFFS_TC 0x01
sdloh 0:6100f19d230b 13 #define ZGOFFS_TC 0x02
sdloh 0:6100f19d230b 14 #define X_FINE_GAIN 0x03 // [7:0] fine gain
sdloh 0:6100f19d230b 15 #define Y_FINE_GAIN 0x04
sdloh 0:6100f19d230b 16 #define Z_FINE_GAIN 0x05
sdloh 0:6100f19d230b 17 #define XA_OFFSET_H 0x06 // User-defined trim values for accelerometer
sdloh 0:6100f19d230b 18 #define XA_OFFSET_L_TC 0x07
sdloh 0:6100f19d230b 19 #define YA_OFFSET_H 0x08
sdloh 0:6100f19d230b 20 #define YA_OFFSET_L_TC 0x09
sdloh 0:6100f19d230b 21 #define ZA_OFFSET_H 0x0A
sdloh 0:6100f19d230b 22 #define ZA_OFFSET_L_TC 0x0B
sdloh 0:6100f19d230b 23 #define SELF_TEST_X 0x0D
sdloh 0:6100f19d230b 24 #define SELF_TEST_Y 0x0E
sdloh 0:6100f19d230b 25 #define SELF_TEST_Z 0x0F
sdloh 0:6100f19d230b 26 #define SELF_TEST_A 0x10
sdloh 0:6100f19d230b 27 #define XG_OFFS_USRH 0x13 // User-defined trim values for gyroscope; supported in MPU-6050?
sdloh 0:6100f19d230b 28 #define XG_OFFS_USRL 0x14
sdloh 0:6100f19d230b 29 #define YG_OFFS_USRH 0x15
sdloh 0:6100f19d230b 30 #define YG_OFFS_USRL 0x16
sdloh 0:6100f19d230b 31 #define ZG_OFFS_USRH 0x17
sdloh 0:6100f19d230b 32 #define ZG_OFFS_USRL 0x18
sdloh 0:6100f19d230b 33 #define SMPLRT_DIV 0x19
sdloh 0:6100f19d230b 34 #define CONFIG 0x1A
sdloh 0:6100f19d230b 35 #define GYRO_CONFIG 0x1B
sdloh 0:6100f19d230b 36 #define ACCEL_CONFIG 0x1C
sdloh 0:6100f19d230b 37 #define FF_THR 0x1D // Free-fall
sdloh 0:6100f19d230b 38 #define FF_DUR 0x1E // Free-fall
sdloh 0:6100f19d230b 39 #define MOT_THR 0x1F // Motion detection threshold bits [7:0]
sdloh 0:6100f19d230b 40 #define MOT_DUR 0x20 // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms
sdloh 0:6100f19d230b 41 #define ZMOT_THR 0x21 // Zero-motion detection threshold bits [7:0]
sdloh 0:6100f19d230b 42 #define ZRMOT_DUR 0x22 // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms
sdloh 0:6100f19d230b 43 #define FIFO_EN 0x23
sdloh 0:6100f19d230b 44 #define I2C_MST_CTRL 0x24
sdloh 0:6100f19d230b 45 #define I2C_SLV0_ADDR 0x25
sdloh 0:6100f19d230b 46 #define I2C_SLV0_REG 0x26
sdloh 0:6100f19d230b 47 #define I2C_SLV0_CTRL 0x27
sdloh 0:6100f19d230b 48 #define I2C_SLV1_ADDR 0x28
sdloh 0:6100f19d230b 49 #define I2C_SLV1_REG 0x29
sdloh 0:6100f19d230b 50 #define I2C_SLV1_CTRL 0x2A
sdloh 0:6100f19d230b 51 #define I2C_SLV2_ADDR 0x2B
sdloh 0:6100f19d230b 52 #define I2C_SLV2_REG 0x2C
sdloh 0:6100f19d230b 53 #define I2C_SLV2_CTRL 0x2D
sdloh 0:6100f19d230b 54 #define I2C_SLV3_ADDR 0x2E
sdloh 0:6100f19d230b 55 #define I2C_SLV3_REG 0x2F
sdloh 0:6100f19d230b 56 #define I2C_SLV3_CTRL 0x30
sdloh 0:6100f19d230b 57 #define I2C_SLV4_ADDR 0x31
sdloh 0:6100f19d230b 58 #define I2C_SLV4_REG 0x32
sdloh 0:6100f19d230b 59 #define I2C_SLV4_DO 0x33
sdloh 0:6100f19d230b 60 #define I2C_SLV4_CTRL 0x34
sdloh 0:6100f19d230b 61 #define I2C_SLV4_DI 0x35
sdloh 0:6100f19d230b 62 #define I2C_MST_STATUS 0x36
sdloh 0:6100f19d230b 63 #define INT_PIN_CFG 0x37
sdloh 0:6100f19d230b 64 #define INT_ENABLE 0x38
sdloh 0:6100f19d230b 65 #define DMP_INT_STATUS 0x39 // Check DMP interrupt
sdloh 0:6100f19d230b 66 #define INT_STATUS 0x3A
sdloh 0:6100f19d230b 67 #define ACCEL_XOUT_H 0x3B
sdloh 0:6100f19d230b 68 #define ACCEL_XOUT_L 0x3C
sdloh 0:6100f19d230b 69 #define ACCEL_YOUT_H 0x3D
sdloh 0:6100f19d230b 70 #define ACCEL_YOUT_L 0x3E
sdloh 0:6100f19d230b 71 #define ACCEL_ZOUT_H 0x3F
sdloh 0:6100f19d230b 72 #define ACCEL_ZOUT_L 0x40
sdloh 0:6100f19d230b 73 #define TEMP_OUT_H 0x41
sdloh 0:6100f19d230b 74 #define TEMP_OUT_L 0x42
sdloh 0:6100f19d230b 75 #define GYRO_XOUT_H 0x43
sdloh 0:6100f19d230b 76 #define GYRO_XOUT_L 0x44
sdloh 0:6100f19d230b 77 #define GYRO_YOUT_H 0x45
sdloh 0:6100f19d230b 78 #define GYRO_YOUT_L 0x46
sdloh 0:6100f19d230b 79 #define GYRO_ZOUT_H 0x47
sdloh 0:6100f19d230b 80 #define GYRO_ZOUT_L 0x48
sdloh 0:6100f19d230b 81 #define EXT_SENS_DATA_00 0x49
sdloh 0:6100f19d230b 82 #define EXT_SENS_DATA_01 0x4A
sdloh 0:6100f19d230b 83 #define EXT_SENS_DATA_02 0x4B
sdloh 0:6100f19d230b 84 #define EXT_SENS_DATA_03 0x4C
sdloh 0:6100f19d230b 85 #define EXT_SENS_DATA_04 0x4D
sdloh 0:6100f19d230b 86 #define EXT_SENS_DATA_05 0x4E
sdloh 0:6100f19d230b 87 #define EXT_SENS_DATA_06 0x4F
sdloh 0:6100f19d230b 88 #define EXT_SENS_DATA_07 0x50
sdloh 0:6100f19d230b 89 #define EXT_SENS_DATA_08 0x51
sdloh 0:6100f19d230b 90 #define EXT_SENS_DATA_09 0x52
sdloh 0:6100f19d230b 91 #define EXT_SENS_DATA_10 0x53
sdloh 0:6100f19d230b 92 #define EXT_SENS_DATA_11 0x54
sdloh 0:6100f19d230b 93 #define EXT_SENS_DATA_12 0x55
sdloh 0:6100f19d230b 94 #define EXT_SENS_DATA_13 0x56
sdloh 0:6100f19d230b 95 #define EXT_SENS_DATA_14 0x57
sdloh 0:6100f19d230b 96 #define EXT_SENS_DATA_15 0x58
sdloh 0:6100f19d230b 97 #define EXT_SENS_DATA_16 0x59
sdloh 0:6100f19d230b 98 #define EXT_SENS_DATA_17 0x5A
sdloh 0:6100f19d230b 99 #define EXT_SENS_DATA_18 0x5B
sdloh 0:6100f19d230b 100 #define EXT_SENS_DATA_19 0x5C
sdloh 0:6100f19d230b 101 #define EXT_SENS_DATA_20 0x5D
sdloh 0:6100f19d230b 102 #define EXT_SENS_DATA_21 0x5E
sdloh 0:6100f19d230b 103 #define EXT_SENS_DATA_22 0x5F
sdloh 0:6100f19d230b 104 #define EXT_SENS_DATA_23 0x60
sdloh 0:6100f19d230b 105 #define MOT_DETECT_STATUS 0x61
sdloh 0:6100f19d230b 106 #define I2C_SLV0_DO 0x63
sdloh 0:6100f19d230b 107 #define I2C_SLV1_DO 0x64
sdloh 0:6100f19d230b 108 #define I2C_SLV2_DO 0x65
sdloh 0:6100f19d230b 109 #define I2C_SLV3_DO 0x66
sdloh 0:6100f19d230b 110 #define I2C_MST_DELAY_CTRL 0x67
sdloh 0:6100f19d230b 111 #define SIGNAL_PATH_RESET 0x68
sdloh 0:6100f19d230b 112 #define MOT_DETECT_CTRL 0x69
sdloh 0:6100f19d230b 113 #define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP
sdloh 0:6100f19d230b 114 #define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode
sdloh 0:6100f19d230b 115 #define PWR_MGMT_2 0x6C
sdloh 0:6100f19d230b 116 #define DMP_BANK 0x6D // Activates a specific bank in the DMP
sdloh 0:6100f19d230b 117 #define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank
sdloh 0:6100f19d230b 118 #define DMP_REG 0x6F // Register in DMP from which to read or to which to write
sdloh 0:6100f19d230b 119 #define DMP_REG_1 0x70
sdloh 0:6100f19d230b 120 #define DMP_REG_2 0x71
sdloh 0:6100f19d230b 121 #define FIFO_COUNTH 0x72
sdloh 0:6100f19d230b 122 #define FIFO_COUNTL 0x73
sdloh 0:6100f19d230b 123 #define FIFO_R_W 0x74
sdloh 0:6100f19d230b 124 #define WHO_AM_I_MPU6050 0x75 // Should return 0x68
sdloh 0:6100f19d230b 125
sdloh 0:6100f19d230b 126 // Using the GY-521 breakout board, I set ADO to 0 by grounding through a 4k7 resistor
sdloh 0:6100f19d230b 127 // Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1
sdloh 0:6100f19d230b 128 #define ADO 0
sdloh 0:6100f19d230b 129 #if ADO
sdloh 0:6100f19d230b 130 #define MPU6050_ADDRESS 0x69<<1 // Device address when ADO = 1
sdloh 0:6100f19d230b 131 #else
sdloh 0:6100f19d230b 132 #define MPU6050_ADDRESS 0x68<<1 // Device address when ADO = 0
sdloh 0:6100f19d230b 133 #endif
sdloh 0:6100f19d230b 134
sdloh 0:6100f19d230b 135 // Set initial input parameters
sdloh 0:6100f19d230b 136 enum Ascale {
sdloh 0:6100f19d230b 137 AFS_2G = 0,
sdloh 0:6100f19d230b 138 AFS_4G,
sdloh 0:6100f19d230b 139 AFS_8G,
sdloh 0:6100f19d230b 140 AFS_16G
sdloh 0:6100f19d230b 141 };
sdloh 0:6100f19d230b 142
sdloh 0:6100f19d230b 143 enum Gscale {
sdloh 0:6100f19d230b 144 GFS_250DPS = 0,
sdloh 0:6100f19d230b 145 GFS_500DPS,
sdloh 0:6100f19d230b 146 GFS_1000DPS,
sdloh 0:6100f19d230b 147 GFS_2000DPS
sdloh 0:6100f19d230b 148 };
sdloh 0:6100f19d230b 149
sdloh 0:6100f19d230b 150 // Specify sensor full scale
sdloh 0:6100f19d230b 151 int Gscale = GFS_250DPS;
sdloh 0:6100f19d230b 152 int Ascale = AFS_2G;
sdloh 0:6100f19d230b 153
sdloh 0:6100f19d230b 154 //Set up I2C, (SDA,SCL)
sdloh 0:6100f19d230b 155 I2C i2c(I2C_SDA, I2C_SCL);
sdloh 0:6100f19d230b 156
sdloh 0:6100f19d230b 157 DigitalOut myled(LED1);
sdloh 0:6100f19d230b 158
sdloh 0:6100f19d230b 159 float aRes, gRes; // scale resolutions per LSB for the sensors
sdloh 0:6100f19d230b 160
sdloh 0:6100f19d230b 161 // Pin definitions
sdloh 0:6100f19d230b 162 int intPin = 12; // These can be changed, 2 and 3 are the Arduinos ext int pins
sdloh 0:6100f19d230b 163
sdloh 0:6100f19d230b 164 int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output
sdloh 0:6100f19d230b 165 float ax, ay, az; // Stores the real accel value in g's
sdloh 0:6100f19d230b 166 int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output
sdloh 0:6100f19d230b 167 float gx, gy, gz; // Stores the real gyro value in degrees per seconds
sdloh 0:6100f19d230b 168 float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}; // Bias corrections for gyro and accelerometer
sdloh 0:6100f19d230b 169 int16_t tempCount; // Stores the real internal chip temperature in degrees Celsius
sdloh 0:6100f19d230b 170 float temperature;
sdloh 0:6100f19d230b 171 float SelfTest[6];
sdloh 0:6100f19d230b 172
sdloh 0:6100f19d230b 173 int delt_t = 0; // used to control display output rate
sdloh 0:6100f19d230b 174 int count = 0; // used to control display output rate
sdloh 0:6100f19d230b 175
sdloh 0:6100f19d230b 176 // parameters for 6 DoF sensor fusion calculations
sdloh 0:6100f19d230b 177 float PI = 3.14159265358979323846f;
sdloh 0:6100f19d230b 178 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
sdloh 0:6100f19d230b 179 float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta
sdloh 0:6100f19d230b 180 float GyroMeasDrift = PI * (1.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
sdloh 0:6100f19d230b 181 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
sdloh 0:6100f19d230b 182 float pitch, yaw, roll;
sdloh 0:6100f19d230b 183 float deltat = 0.0f; // integration interval for both filter schemes
sdloh 0:6100f19d230b 184 int lastUpdate = 0, firstUpdate = 0, Now = 0; // used to calculate integration interval // used to calculate integration interval
sdloh 0:6100f19d230b 185 float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion
sdloh 0:6100f19d230b 186
sdloh 0:6100f19d230b 187 class MPU6050 {
sdloh 0:6100f19d230b 188
sdloh 0:6100f19d230b 189 protected:
sdloh 0:6100f19d230b 190
sdloh 0:6100f19d230b 191 public:
sdloh 0:6100f19d230b 192 //===================================================================================================================
sdloh 0:6100f19d230b 193 //====== Set of useful function to access acceleratio, gyroscope, and temperature data
sdloh 0:6100f19d230b 194 //===================================================================================================================
sdloh 0:6100f19d230b 195
sdloh 0:6100f19d230b 196 void writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
sdloh 0:6100f19d230b 197 {
sdloh 0:6100f19d230b 198 char data_write[2];
sdloh 0:6100f19d230b 199 data_write[0] = subAddress;
sdloh 0:6100f19d230b 200 data_write[1] = data;
sdloh 0:6100f19d230b 201 i2c.write(address, data_write, 2, 0);
sdloh 0:6100f19d230b 202 }
sdloh 0:6100f19d230b 203
sdloh 0:6100f19d230b 204 char readByte(uint8_t address, uint8_t subAddress)
sdloh 0:6100f19d230b 205 {
sdloh 0:6100f19d230b 206 char data[1]; // `data` will store the register data
sdloh 0:6100f19d230b 207 char data_write[1];
sdloh 0:6100f19d230b 208 data_write[0] = subAddress;
sdloh 0:6100f19d230b 209 i2c.write(address, data_write, 1, 1); // no stop
sdloh 0:6100f19d230b 210 i2c.read(address, data, 1, 0);
sdloh 0:6100f19d230b 211 return data[0];
sdloh 0:6100f19d230b 212 }
sdloh 0:6100f19d230b 213
sdloh 0:6100f19d230b 214 void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
sdloh 0:6100f19d230b 215 {
sdloh 0:6100f19d230b 216 char data[14];
sdloh 0:6100f19d230b 217 char data_write[1];
sdloh 0:6100f19d230b 218 data_write[0] = subAddress;
sdloh 0:6100f19d230b 219 i2c.write(address, data_write, 1, 1); // no stop
sdloh 0:6100f19d230b 220 i2c.read(address, data, count, 0);
sdloh 0:6100f19d230b 221 for(int ii = 0; ii < count; ii++) {
sdloh 0:6100f19d230b 222 dest[ii] = data[ii];
sdloh 0:6100f19d230b 223 }
sdloh 0:6100f19d230b 224 }
sdloh 0:6100f19d230b 225
sdloh 0:6100f19d230b 226
sdloh 0:6100f19d230b 227 void getGres() {
sdloh 0:6100f19d230b 228 switch (Gscale)
sdloh 0:6100f19d230b 229 {
sdloh 0:6100f19d230b 230 // Possible gyro scales (and their register bit settings) are:
sdloh 0:6100f19d230b 231 // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11).
sdloh 0:6100f19d230b 232 // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
sdloh 0:6100f19d230b 233 case GFS_250DPS:
sdloh 0:6100f19d230b 234 gRes = 250.0/32768.0;
sdloh 0:6100f19d230b 235 break;
sdloh 0:6100f19d230b 236 case GFS_500DPS:
sdloh 0:6100f19d230b 237 gRes = 500.0/32768.0;
sdloh 0:6100f19d230b 238 break;
sdloh 0:6100f19d230b 239 case GFS_1000DPS:
sdloh 0:6100f19d230b 240 gRes = 1000.0/32768.0;
sdloh 0:6100f19d230b 241 break;
sdloh 0:6100f19d230b 242 case GFS_2000DPS:
sdloh 0:6100f19d230b 243 gRes = 2000.0/32768.0;
sdloh 0:6100f19d230b 244 break;
sdloh 0:6100f19d230b 245 }
sdloh 0:6100f19d230b 246 }
sdloh 0:6100f19d230b 247
sdloh 0:6100f19d230b 248 void getAres() {
sdloh 0:6100f19d230b 249 switch (Ascale)
sdloh 0:6100f19d230b 250 {
sdloh 0:6100f19d230b 251 // Possible accelerometer scales (and their register bit settings) are:
sdloh 0:6100f19d230b 252 // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11).
sdloh 0:6100f19d230b 253 // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
sdloh 0:6100f19d230b 254 case AFS_2G:
sdloh 0:6100f19d230b 255 aRes = 2.0/32768.0;
sdloh 0:6100f19d230b 256 break;
sdloh 0:6100f19d230b 257 case AFS_4G:
sdloh 0:6100f19d230b 258 aRes = 4.0/32768.0;
sdloh 0:6100f19d230b 259 break;
sdloh 0:6100f19d230b 260 case AFS_8G:
sdloh 0:6100f19d230b 261 aRes = 8.0/32768.0;
sdloh 0:6100f19d230b 262 break;
sdloh 0:6100f19d230b 263 case AFS_16G:
sdloh 0:6100f19d230b 264 aRes = 16.0/32768.0;
sdloh 0:6100f19d230b 265 break;
sdloh 0:6100f19d230b 266 }
sdloh 0:6100f19d230b 267 }
sdloh 0:6100f19d230b 268
sdloh 0:6100f19d230b 269
sdloh 0:6100f19d230b 270 void readAccelData(int16_t * destination)
sdloh 0:6100f19d230b 271 {
sdloh 0:6100f19d230b 272 uint8_t rawData[6]; // x/y/z accel register data stored here
sdloh 0:6100f19d230b 273 readBytes(MPU6050_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array
sdloh 0:6100f19d230b 274 destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
sdloh 0:6100f19d230b 275 destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
sdloh 0:6100f19d230b 276 destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
sdloh 0:6100f19d230b 277 }
sdloh 0:6100f19d230b 278
sdloh 0:6100f19d230b 279 void readGyroData(int16_t * destination)
sdloh 0:6100f19d230b 280 {
sdloh 0:6100f19d230b 281 uint8_t rawData[6]; // x/y/z gyro register data stored here
sdloh 0:6100f19d230b 282 readBytes(MPU6050_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
sdloh 0:6100f19d230b 283 destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
sdloh 0:6100f19d230b 284 destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
sdloh 0:6100f19d230b 285 destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
sdloh 0:6100f19d230b 286 }
sdloh 0:6100f19d230b 287
sdloh 0:6100f19d230b 288 int16_t readTempData()
sdloh 0:6100f19d230b 289 {
sdloh 0:6100f19d230b 290 uint8_t rawData[2]; // x/y/z gyro register data stored here
sdloh 0:6100f19d230b 291 readBytes(MPU6050_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); // Read the two raw data registers sequentially into data array
sdloh 0:6100f19d230b 292 return (int16_t)(((int16_t)rawData[0]) << 8 | rawData[1]) ; // Turn the MSB and LSB into a 16-bit value
sdloh 0:6100f19d230b 293 }
sdloh 0:6100f19d230b 294
sdloh 0:6100f19d230b 295
sdloh 0:6100f19d230b 296
sdloh 0:6100f19d230b 297 // Configure the motion detection control for low power accelerometer mode
sdloh 0:6100f19d230b 298 void LowPowerAccelOnly()
sdloh 0:6100f19d230b 299 {
sdloh 0:6100f19d230b 300
sdloh 0:6100f19d230b 301 // The sensor has a high-pass filter necessary to invoke to allow the sensor motion detection algorithms work properly
sdloh 0:6100f19d230b 302 // Motion detection occurs on free-fall (acceleration below a threshold for some time for all axes), motion (acceleration
sdloh 0:6100f19d230b 303 // above a threshold for some time on at least one axis), and zero-motion toggle (acceleration on each axis less than a
sdloh 0:6100f19d230b 304 // threshold for some time sets this flag, motion above the threshold turns it off). The high-pass filter takes gravity out
sdloh 0:6100f19d230b 305 // consideration for these threshold evaluations; otherwise, the flags would be set all the time!
sdloh 0:6100f19d230b 306
sdloh 0:6100f19d230b 307 uint8_t c = readByte(MPU6050_ADDRESS, PWR_MGMT_1);
sdloh 0:6100f19d230b 308 writeByte(MPU6050_ADDRESS, PWR_MGMT_1, c & ~0x30); // Clear sleep and cycle bits [5:6]
sdloh 0:6100f19d230b 309 writeByte(MPU6050_ADDRESS, PWR_MGMT_1, c | 0x30); // Set sleep and cycle bits [5:6] to zero to make sure accelerometer is running
sdloh 0:6100f19d230b 310
sdloh 0:6100f19d230b 311 c = readByte(MPU6050_ADDRESS, PWR_MGMT_2);
sdloh 0:6100f19d230b 312 writeByte(MPU6050_ADDRESS, PWR_MGMT_2, c & ~0x38); // Clear standby XA, YA, and ZA bits [3:5]
sdloh 0:6100f19d230b 313 writeByte(MPU6050_ADDRESS, PWR_MGMT_2, c | 0x00); // Set XA, YA, and ZA bits [3:5] to zero to make sure accelerometer is running
sdloh 0:6100f19d230b 314
sdloh 0:6100f19d230b 315 c = readByte(MPU6050_ADDRESS, ACCEL_CONFIG);
sdloh 0:6100f19d230b 316 writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c & ~0x07); // Clear high-pass filter bits [2:0]
sdloh 0:6100f19d230b 317 // 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
sdloh 0:6100f19d230b 318 writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c | 0x00); // Set ACCEL_HPF to 0; reset mode disbaling high-pass filter
sdloh 0:6100f19d230b 319
sdloh 0:6100f19d230b 320 c = readByte(MPU6050_ADDRESS, CONFIG);
sdloh 0:6100f19d230b 321 writeByte(MPU6050_ADDRESS, CONFIG, c & ~0x07); // Clear low-pass filter bits [2:0]
sdloh 0:6100f19d230b 322 writeByte(MPU6050_ADDRESS, CONFIG, c | 0x00); // Set DLPD_CFG to 0; 260 Hz bandwidth, 1 kHz rate
sdloh 0:6100f19d230b 323
sdloh 0:6100f19d230b 324 c = readByte(MPU6050_ADDRESS, INT_ENABLE);
sdloh 0:6100f19d230b 325 writeByte(MPU6050_ADDRESS, INT_ENABLE, c & ~0xFF); // Clear all interrupts
sdloh 0:6100f19d230b 326 writeByte(MPU6050_ADDRESS, INT_ENABLE, 0x40); // Enable motion threshold (bits 5) interrupt only
sdloh 0:6100f19d230b 327
sdloh 0:6100f19d230b 328 // Motion detection interrupt requires the absolute value of any axis to lie above the detection threshold
sdloh 0:6100f19d230b 329 // for at least the counter duration
sdloh 0:6100f19d230b 330 writeByte(MPU6050_ADDRESS, MOT_THR, 0x80); // Set motion detection to 0.256 g; LSB = 2 mg
sdloh 0:6100f19d230b 331 writeByte(MPU6050_ADDRESS, MOT_DUR, 0x01); // Set motion detect duration to 1 ms; LSB is 1 ms @ 1 kHz rate
sdloh 0:6100f19d230b 332
sdloh 0:6100f19d230b 333 wait(0.1); // Add delay for accumulation of samples
sdloh 0:6100f19d230b 334
sdloh 0:6100f19d230b 335 c = readByte(MPU6050_ADDRESS, ACCEL_CONFIG);
sdloh 0:6100f19d230b 336 writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c & ~0x07); // Clear high-pass filter bits [2:0]
sdloh 0:6100f19d230b 337 writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c | 0x07); // Set ACCEL_HPF to 7; hold the initial accleration value as a referance
sdloh 0:6100f19d230b 338
sdloh 0:6100f19d230b 339 c = readByte(MPU6050_ADDRESS, PWR_MGMT_2);
sdloh 0:6100f19d230b 340 writeByte(MPU6050_ADDRESS, PWR_MGMT_2, c & ~0xC7); // Clear standby XA, YA, and ZA bits [3:5] and LP_WAKE_CTRL bits [6:7]
sdloh 0:6100f19d230b 341 writeByte(MPU6050_ADDRESS, PWR_MGMT_2, c | 0x47); // Set wakeup frequency to 5 Hz, and disable XG, YG, and ZG gyros (bits [0:2])
sdloh 0:6100f19d230b 342
sdloh 0:6100f19d230b 343 c = readByte(MPU6050_ADDRESS, PWR_MGMT_1);
sdloh 0:6100f19d230b 344 writeByte(MPU6050_ADDRESS, PWR_MGMT_1, c & ~0x20); // Clear sleep and cycle bit 5
sdloh 0:6100f19d230b 345 writeByte(MPU6050_ADDRESS, PWR_MGMT_1, c | 0x20); // Set cycle bit 5 to begin low power accelerometer motion interrupts
sdloh 0:6100f19d230b 346
sdloh 0:6100f19d230b 347 }
sdloh 0:6100f19d230b 348
sdloh 0:6100f19d230b 349
sdloh 0:6100f19d230b 350 void resetMPU6050() {
sdloh 0:6100f19d230b 351 // reset device
sdloh 0:6100f19d230b 352 writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device
sdloh 0:6100f19d230b 353 wait(0.1);
sdloh 0:6100f19d230b 354 }
sdloh 0:6100f19d230b 355
sdloh 0:6100f19d230b 356
sdloh 0:6100f19d230b 357 void initMPU6050()
sdloh 0:6100f19d230b 358 {
sdloh 0:6100f19d230b 359 // Initialize MPU6050 device
sdloh 0:6100f19d230b 360 // wake up device
sdloh 0:6100f19d230b 361 writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors
sdloh 0:6100f19d230b 362 wait(0.1); // Delay 100 ms for PLL to get established on x-axis gyro; should check for PLL ready interrupt
sdloh 0:6100f19d230b 363
sdloh 0:6100f19d230b 364 // get stable time source
sdloh 0:6100f19d230b 365 writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x01); // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001
sdloh 0:6100f19d230b 366
sdloh 0:6100f19d230b 367 // Configure Gyro and Accelerometer
sdloh 0:6100f19d230b 368 // Disable FSYNC and set accelerometer and gyro bandwidth to 44 and 42 Hz, respectively;
sdloh 0:6100f19d230b 369 // DLPF_CFG = bits 2:0 = 010; this sets the sample rate at 1 kHz for both
sdloh 0:6100f19d230b 370 // Maximum delay is 4.9 ms which is just over a 200 Hz maximum rate
sdloh 0:6100f19d230b 371 writeByte(MPU6050_ADDRESS, CONFIG, 0x03);
sdloh 0:6100f19d230b 372
sdloh 0:6100f19d230b 373 // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)
sdloh 0:6100f19d230b 374 writeByte(MPU6050_ADDRESS, SMPLRT_DIV, 0x04); // Use a 200 Hz rate; the same rate set in CONFIG above
sdloh 0:6100f19d230b 375
sdloh 0:6100f19d230b 376 // Set gyroscope full scale range
sdloh 0:6100f19d230b 377 // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3
sdloh 0:6100f19d230b 378 uint8_t c = readByte(MPU6050_ADDRESS, GYRO_CONFIG);
sdloh 0:6100f19d230b 379 writeByte(MPU6050_ADDRESS, GYRO_CONFIG, c & ~0xE0); // Clear self-test bits [7:5]
sdloh 0:6100f19d230b 380 writeByte(MPU6050_ADDRESS, GYRO_CONFIG, c & ~0x18); // Clear AFS bits [4:3]
sdloh 0:6100f19d230b 381 writeByte(MPU6050_ADDRESS, GYRO_CONFIG, c | Gscale << 3); // Set full scale range for the gyro
sdloh 0:6100f19d230b 382
sdloh 0:6100f19d230b 383 // Set accelerometer configuration
sdloh 0:6100f19d230b 384 c = readByte(MPU6050_ADDRESS, ACCEL_CONFIG);
sdloh 0:6100f19d230b 385 writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c & ~0xE0); // Clear self-test bits [7:5]
sdloh 0:6100f19d230b 386 writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c & ~0x18); // Clear AFS bits [4:3]
sdloh 0:6100f19d230b 387 writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c | Ascale << 3); // Set full scale range for the accelerometer
sdloh 0:6100f19d230b 388
sdloh 0:6100f19d230b 389 // Configure Interrupts and Bypass Enable
sdloh 0:6100f19d230b 390 // Set interrupt pin active high, push-pull, and clear on read of INT_STATUS, enable I2C_BYPASS_EN so additional chips
sdloh 0:6100f19d230b 391 // can join the I2C bus and all can be controlled by the Arduino as master
sdloh 0:6100f19d230b 392 writeByte(MPU6050_ADDRESS, INT_PIN_CFG, 0x22);
sdloh 0:6100f19d230b 393 writeByte(MPU6050_ADDRESS, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt
sdloh 0:6100f19d230b 394 }
sdloh 0:6100f19d230b 395
sdloh 0:6100f19d230b 396 // Function which accumulates gyro and accelerometer data after device initialization. It calculates the average
sdloh 0:6100f19d230b 397 // of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers.
sdloh 0:6100f19d230b 398 void calibrateMPU6050(float * dest1, float * dest2)
sdloh 0:6100f19d230b 399 {
sdloh 0:6100f19d230b 400 uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data
sdloh 0:6100f19d230b 401 uint16_t ii, packet_count, fifo_count;
sdloh 0:6100f19d230b 402 int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
sdloh 0:6100f19d230b 403
sdloh 0:6100f19d230b 404 // reset device, reset all registers, clear gyro and accelerometer bias registers
sdloh 0:6100f19d230b 405 writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device
sdloh 0:6100f19d230b 406 wait(0.1);
sdloh 0:6100f19d230b 407
sdloh 0:6100f19d230b 408 // get stable time source
sdloh 0:6100f19d230b 409 // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001
sdloh 0:6100f19d230b 410 writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x01);
sdloh 0:6100f19d230b 411 writeByte(MPU6050_ADDRESS, PWR_MGMT_2, 0x00);
sdloh 0:6100f19d230b 412 wait(0.2);
sdloh 0:6100f19d230b 413
sdloh 0:6100f19d230b 414 // Configure device for bias calculation
sdloh 0:6100f19d230b 415 writeByte(MPU6050_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts
sdloh 0:6100f19d230b 416 writeByte(MPU6050_ADDRESS, FIFO_EN, 0x00); // Disable FIFO
sdloh 0:6100f19d230b 417 writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source
sdloh 0:6100f19d230b 418 writeByte(MPU6050_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master
sdloh 0:6100f19d230b 419 writeByte(MPU6050_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes
sdloh 0:6100f19d230b 420 writeByte(MPU6050_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP
sdloh 0:6100f19d230b 421 wait(0.015);
sdloh 0:6100f19d230b 422
sdloh 0:6100f19d230b 423 // Configure MPU6050 gyro and accelerometer for bias calculation
sdloh 0:6100f19d230b 424 writeByte(MPU6050_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz
sdloh 0:6100f19d230b 425 writeByte(MPU6050_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz
sdloh 0:6100f19d230b 426 writeByte(MPU6050_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity
sdloh 0:6100f19d230b 427 writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity
sdloh 0:6100f19d230b 428
sdloh 0:6100f19d230b 429 uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec
sdloh 0:6100f19d230b 430 uint16_t accelsensitivity = 16384; // = 16384 LSB/g
sdloh 0:6100f19d230b 431
sdloh 0:6100f19d230b 432 // Configure FIFO to capture accelerometer and gyro data for bias calculation
sdloh 0:6100f19d230b 433 writeByte(MPU6050_ADDRESS, USER_CTRL, 0x40); // Enable FIFO
sdloh 0:6100f19d230b 434 writeByte(MPU6050_ADDRESS, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 1024 bytes in MPU-6050)
sdloh 0:6100f19d230b 435 wait(0.08); // accumulate 80 samples in 80 milliseconds = 960 bytes
sdloh 0:6100f19d230b 436
sdloh 0:6100f19d230b 437 // At end of sample accumulation, turn off FIFO sensor read
sdloh 0:6100f19d230b 438 writeByte(MPU6050_ADDRESS, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO
sdloh 0:6100f19d230b 439 readBytes(MPU6050_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count
sdloh 0:6100f19d230b 440 fifo_count = ((uint16_t)data[0] << 8) | data[1];
sdloh 0:6100f19d230b 441 packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging
sdloh 0:6100f19d230b 442
sdloh 0:6100f19d230b 443 for (ii = 0; ii < packet_count; ii++) {
sdloh 0:6100f19d230b 444 int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
sdloh 0:6100f19d230b 445 readBytes(MPU6050_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging
sdloh 0:6100f19d230b 446 accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ; // Form signed 16-bit integer for each sample in FIFO
sdloh 0:6100f19d230b 447 accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ;
sdloh 0:6100f19d230b 448 accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ;
sdloh 0:6100f19d230b 449 gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ;
sdloh 0:6100f19d230b 450 gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ;
sdloh 0:6100f19d230b 451 gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ;
sdloh 0:6100f19d230b 452
sdloh 0:6100f19d230b 453 accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
sdloh 0:6100f19d230b 454 accel_bias[1] += (int32_t) accel_temp[1];
sdloh 0:6100f19d230b 455 accel_bias[2] += (int32_t) accel_temp[2];
sdloh 0:6100f19d230b 456 gyro_bias[0] += (int32_t) gyro_temp[0];
sdloh 0:6100f19d230b 457 gyro_bias[1] += (int32_t) gyro_temp[1];
sdloh 0:6100f19d230b 458 gyro_bias[2] += (int32_t) gyro_temp[2];
sdloh 0:6100f19d230b 459
sdloh 0:6100f19d230b 460 }
sdloh 0:6100f19d230b 461 accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases
sdloh 0:6100f19d230b 462 accel_bias[1] /= (int32_t) packet_count;
sdloh 0:6100f19d230b 463 accel_bias[2] /= (int32_t) packet_count;
sdloh 0:6100f19d230b 464 gyro_bias[0] /= (int32_t) packet_count;
sdloh 0:6100f19d230b 465 gyro_bias[1] /= (int32_t) packet_count;
sdloh 0:6100f19d230b 466 gyro_bias[2] /= (int32_t) packet_count;
sdloh 0:6100f19d230b 467
sdloh 0:6100f19d230b 468 if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;} // Remove gravity from the z-axis accelerometer bias calculation
sdloh 0:6100f19d230b 469 else {accel_bias[2] += (int32_t) accelsensitivity;}
sdloh 0:6100f19d230b 470
sdloh 0:6100f19d230b 471 // Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup
sdloh 0:6100f19d230b 472 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
sdloh 0:6100f19d230b 473 data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases
sdloh 0:6100f19d230b 474 data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF;
sdloh 0:6100f19d230b 475 data[3] = (-gyro_bias[1]/4) & 0xFF;
sdloh 0:6100f19d230b 476 data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF;
sdloh 0:6100f19d230b 477 data[5] = (-gyro_bias[2]/4) & 0xFF;
sdloh 0:6100f19d230b 478
sdloh 0:6100f19d230b 479 // Push gyro biases to hardware registers
sdloh 0:6100f19d230b 480 writeByte(MPU6050_ADDRESS, XG_OFFS_USRH, data[0]);
sdloh 0:6100f19d230b 481 writeByte(MPU6050_ADDRESS, XG_OFFS_USRL, data[1]);
sdloh 0:6100f19d230b 482 writeByte(MPU6050_ADDRESS, YG_OFFS_USRH, data[2]);
sdloh 0:6100f19d230b 483 writeByte(MPU6050_ADDRESS, YG_OFFS_USRL, data[3]);
sdloh 0:6100f19d230b 484 writeByte(MPU6050_ADDRESS, ZG_OFFS_USRH, data[4]);
sdloh 0:6100f19d230b 485 writeByte(MPU6050_ADDRESS, ZG_OFFS_USRL, data[5]);
sdloh 0:6100f19d230b 486
sdloh 0:6100f19d230b 487 dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity; // construct gyro bias in deg/s for later manual subtraction
sdloh 0:6100f19d230b 488 dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity;
sdloh 0:6100f19d230b 489 dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity;
sdloh 0:6100f19d230b 490
sdloh 0:6100f19d230b 491 // Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain
sdloh 0:6100f19d230b 492 // factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold
sdloh 0:6100f19d230b 493 // non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature
sdloh 0:6100f19d230b 494 // compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that
sdloh 0:6100f19d230b 495 // the accelerometer biases calculated above must be divided by 8.
sdloh 0:6100f19d230b 496
sdloh 0:6100f19d230b 497 int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases
sdloh 0:6100f19d230b 498 readBytes(MPU6050_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values
sdloh 0:6100f19d230b 499 accel_bias_reg[0] = (int16_t) ((int16_t)data[0] << 8) | data[1];
sdloh 0:6100f19d230b 500 readBytes(MPU6050_ADDRESS, YA_OFFSET_H, 2, &data[0]);
sdloh 0:6100f19d230b 501 accel_bias_reg[1] = (int16_t) ((int16_t)data[0] << 8) | data[1];
sdloh 0:6100f19d230b 502 readBytes(MPU6050_ADDRESS, ZA_OFFSET_H, 2, &data[0]);
sdloh 0:6100f19d230b 503 accel_bias_reg[2] = (int16_t) ((int16_t)data[0] << 8) | data[1];
sdloh 0:6100f19d230b 504
sdloh 0:6100f19d230b 505 uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers
sdloh 0:6100f19d230b 506 uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis
sdloh 0:6100f19d230b 507
sdloh 0:6100f19d230b 508 for(ii = 0; ii < 3; ii++) {
sdloh 0:6100f19d230b 509 if(accel_bias_reg[ii] & mask) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit
sdloh 0:6100f19d230b 510 }
sdloh 0:6100f19d230b 511
sdloh 0:6100f19d230b 512 // Construct total accelerometer bias, including calculated average accelerometer bias from above
sdloh 0:6100f19d230b 513 accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale)
sdloh 0:6100f19d230b 514 accel_bias_reg[1] -= (accel_bias[1]/8);
sdloh 0:6100f19d230b 515 accel_bias_reg[2] -= (accel_bias[2]/8);
sdloh 0:6100f19d230b 516
sdloh 0:6100f19d230b 517 data[0] = (accel_bias_reg[0] >> 8) & 0xFF;
sdloh 0:6100f19d230b 518 data[1] = (accel_bias_reg[0]) & 0xFF;
sdloh 0:6100f19d230b 519 data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers
sdloh 0:6100f19d230b 520 data[2] = (accel_bias_reg[1] >> 8) & 0xFF;
sdloh 0:6100f19d230b 521 data[3] = (accel_bias_reg[1]) & 0xFF;
sdloh 0:6100f19d230b 522 data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers
sdloh 0:6100f19d230b 523 data[4] = (accel_bias_reg[2] >> 8) & 0xFF;
sdloh 0:6100f19d230b 524 data[5] = (accel_bias_reg[2]) & 0xFF;
sdloh 0:6100f19d230b 525 data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers
sdloh 0:6100f19d230b 526
sdloh 0:6100f19d230b 527 // Push accelerometer biases to hardware registers
sdloh 0:6100f19d230b 528 // writeByte(MPU6050_ADDRESS, XA_OFFSET_H, data[0]);
sdloh 0:6100f19d230b 529 // writeByte(MPU6050_ADDRESS, XA_OFFSET_L_TC, data[1]);
sdloh 0:6100f19d230b 530 // writeByte(MPU6050_ADDRESS, YA_OFFSET_H, data[2]);
sdloh 0:6100f19d230b 531 // writeByte(MPU6050_ADDRESS, YA_OFFSET_L_TC, data[3]);
sdloh 0:6100f19d230b 532 // writeByte(MPU6050_ADDRESS, ZA_OFFSET_H, data[4]);
sdloh 0:6100f19d230b 533 // writeByte(MPU6050_ADDRESS, ZA_OFFSET_L_TC, data[5]);
sdloh 0:6100f19d230b 534
sdloh 0:6100f19d230b 535 // Output scaled accelerometer biases for manual subtraction in the main program
sdloh 0:6100f19d230b 536 dest2[0] = (float)accel_bias[0]/(float)accelsensitivity;
sdloh 0:6100f19d230b 537 dest2[1] = (float)accel_bias[1]/(float)accelsensitivity;
sdloh 0:6100f19d230b 538 dest2[2] = (float)accel_bias[2]/(float)accelsensitivity;
sdloh 0:6100f19d230b 539 }
sdloh 0:6100f19d230b 540
sdloh 0:6100f19d230b 541
sdloh 0:6100f19d230b 542 // Accelerometer and gyroscope self test; check calibration wrt factory settings
sdloh 0:6100f19d230b 543 void MPU6050SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass
sdloh 0:6100f19d230b 544 {
sdloh 0:6100f19d230b 545 uint8_t rawData[4] = {0, 0, 0, 0};
sdloh 0:6100f19d230b 546 uint8_t selfTest[6];
sdloh 0:6100f19d230b 547 float factoryTrim[6];
sdloh 0:6100f19d230b 548
sdloh 0:6100f19d230b 549 // Configure the accelerometer for self-test
sdloh 0:6100f19d230b 550 writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, 0xF0); // Enable self test on all three axes and set accelerometer range to +/- 8 g
sdloh 0:6100f19d230b 551 writeByte(MPU6050_ADDRESS, GYRO_CONFIG, 0xE0); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s
sdloh 0:6100f19d230b 552 wait(0.25); // Delay a while to let the device execute the self-test
sdloh 0:6100f19d230b 553 rawData[0] = readByte(MPU6050_ADDRESS, SELF_TEST_X); // X-axis self-test results
sdloh 0:6100f19d230b 554 rawData[1] = readByte(MPU6050_ADDRESS, SELF_TEST_Y); // Y-axis self-test results
sdloh 0:6100f19d230b 555 rawData[2] = readByte(MPU6050_ADDRESS, SELF_TEST_Z); // Z-axis self-test results
sdloh 0:6100f19d230b 556 rawData[3] = readByte(MPU6050_ADDRESS, SELF_TEST_A); // Mixed-axis self-test results
sdloh 0:6100f19d230b 557 // Extract the acceleration test results first
sdloh 0:6100f19d230b 558 selfTest[0] = (rawData[0] >> 3) | (rawData[3] & 0x30) >> 4 ; // XA_TEST result is a five-bit unsigned integer
sdloh 0:6100f19d230b 559 selfTest[1] = (rawData[1] >> 3) | (rawData[3] & 0x0C) >> 4 ; // YA_TEST result is a five-bit unsigned integer
sdloh 0:6100f19d230b 560 selfTest[2] = (rawData[2] >> 3) | (rawData[3] & 0x03) >> 4 ; // ZA_TEST result is a five-bit unsigned integer
sdloh 0:6100f19d230b 561 // Extract the gyration test results first
sdloh 0:6100f19d230b 562 selfTest[3] = rawData[0] & 0x1F ; // XG_TEST result is a five-bit unsigned integer
sdloh 0:6100f19d230b 563 selfTest[4] = rawData[1] & 0x1F ; // YG_TEST result is a five-bit unsigned integer
sdloh 0:6100f19d230b 564 selfTest[5] = rawData[2] & 0x1F ; // ZG_TEST result is a five-bit unsigned integer
sdloh 0:6100f19d230b 565 // Process results to allow final comparison with factory set values
sdloh 0:6100f19d230b 566 factoryTrim[0] = (4096.0f*0.34f)*(pow( (0.92f/0.34f) , ((selfTest[0] - 1.0f)/30.0f))); // FT[Xa] factory trim calculation
sdloh 0:6100f19d230b 567 factoryTrim[1] = (4096.0f*0.34f)*(pow( (0.92f/0.34f) , ((selfTest[1] - 1.0f)/30.0f))); // FT[Ya] factory trim calculation
sdloh 0:6100f19d230b 568 factoryTrim[2] = (4096.0f*0.34f)*(pow( (0.92f/0.34f) , ((selfTest[2] - 1.0f)/30.0f))); // FT[Za] factory trim calculation
sdloh 0:6100f19d230b 569 factoryTrim[3] = ( 25.0f*131.0f)*(pow( 1.046f , (selfTest[3] - 1.0f) )); // FT[Xg] factory trim calculation
sdloh 0:6100f19d230b 570 factoryTrim[4] = (-25.0f*131.0f)*(pow( 1.046f , (selfTest[4] - 1.0f) )); // FT[Yg] factory trim calculation
sdloh 0:6100f19d230b 571 factoryTrim[5] = ( 25.0f*131.0f)*(pow( 1.046f , (selfTest[5] - 1.0f) )); // FT[Zg] factory trim calculation
sdloh 0:6100f19d230b 572
sdloh 0:6100f19d230b 573 // Output self-test results and factory trim calculation if desired
sdloh 0:6100f19d230b 574 // Serial.println(selfTest[0]); Serial.println(selfTest[1]); Serial.println(selfTest[2]);
sdloh 0:6100f19d230b 575 // Serial.println(selfTest[3]); Serial.println(selfTest[4]); Serial.println(selfTest[5]);
sdloh 0:6100f19d230b 576 // Serial.println(factoryTrim[0]); Serial.println(factoryTrim[1]); Serial.println(factoryTrim[2]);
sdloh 0:6100f19d230b 577 // Serial.println(factoryTrim[3]); Serial.println(factoryTrim[4]); Serial.println(factoryTrim[5]);
sdloh 0:6100f19d230b 578
sdloh 0:6100f19d230b 579 // Report results as a ratio of (STR - FT)/FT; the change from Factory Trim of the Self-Test Response
sdloh 0:6100f19d230b 580 // To get to percent, must multiply by 100 and subtract result from 100
sdloh 0:6100f19d230b 581 for (int i = 0; i < 6; i++) {
sdloh 0:6100f19d230b 582 destination[i] = 100.0f + 100.0f*(selfTest[i] - factoryTrim[i])/factoryTrim[i]; // Report percent differences
sdloh 0:6100f19d230b 583 }
sdloh 0:6100f19d230b 584
sdloh 0:6100f19d230b 585 }
sdloh 0:6100f19d230b 586
sdloh 0:6100f19d230b 587
sdloh 0:6100f19d230b 588 // Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays"
sdloh 0:6100f19d230b 589 // which fuses acceleration and rotation rate to produce a quaternion-based estimate of relative
sdloh 0:6100f19d230b 590 // device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc.
sdloh 0:6100f19d230b 591 // The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms
sdloh 0:6100f19d230b 592 // but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz!
sdloh 0:6100f19d230b 593 void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz)
sdloh 0:6100f19d230b 594 {
sdloh 0:6100f19d230b 595 float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability
sdloh 0:6100f19d230b 596 float norm; // vector norm
sdloh 0:6100f19d230b 597 float f1, f2, f3; // objective funcyion elements
sdloh 0:6100f19d230b 598 float J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33; // objective function Jacobian elements
sdloh 0:6100f19d230b 599 float qDot1, qDot2, qDot3, qDot4;
sdloh 0:6100f19d230b 600 float hatDot1, hatDot2, hatDot3, hatDot4;
sdloh 0:6100f19d230b 601 float gerrx, gerry, gerrz, gbiasx, gbiasy, gbiasz; // gyro bias error
sdloh 0:6100f19d230b 602
sdloh 0:6100f19d230b 603 // Auxiliary variables to avoid repeated arithmetic
sdloh 0:6100f19d230b 604 float _halfq1 = 0.5f * q1;
sdloh 0:6100f19d230b 605 float _halfq2 = 0.5f * q2;
sdloh 0:6100f19d230b 606 float _halfq3 = 0.5f * q3;
sdloh 0:6100f19d230b 607 float _halfq4 = 0.5f * q4;
sdloh 0:6100f19d230b 608 float _2q1 = 2.0f * q1;
sdloh 0:6100f19d230b 609 float _2q2 = 2.0f * q2;
sdloh 0:6100f19d230b 610 float _2q3 = 2.0f * q3;
sdloh 0:6100f19d230b 611 float _2q4 = 2.0f * q4;
sdloh 0:6100f19d230b 612 // float _2q1q3 = 2.0f * q1 * q3;
sdloh 0:6100f19d230b 613 // float _2q3q4 = 2.0f * q3 * q4;
sdloh 0:6100f19d230b 614
sdloh 0:6100f19d230b 615 // Normalise accelerometer measurement
sdloh 0:6100f19d230b 616 norm = sqrt(ax * ax + ay * ay + az * az);
sdloh 0:6100f19d230b 617 if (norm == 0.0f) return; // handle NaN
sdloh 0:6100f19d230b 618 norm = 1.0f/norm;
sdloh 0:6100f19d230b 619 ax *= norm;
sdloh 0:6100f19d230b 620 ay *= norm;
sdloh 0:6100f19d230b 621 az *= norm;
sdloh 0:6100f19d230b 622
sdloh 0:6100f19d230b 623 // Compute the objective function and Jacobian
sdloh 0:6100f19d230b 624 f1 = _2q2 * q4 - _2q1 * q3 - ax;
sdloh 0:6100f19d230b 625 f2 = _2q1 * q2 + _2q3 * q4 - ay;
sdloh 0:6100f19d230b 626 f3 = 1.0f - _2q2 * q2 - _2q3 * q3 - az;
sdloh 0:6100f19d230b 627 J_11or24 = _2q3;
sdloh 0:6100f19d230b 628 J_12or23 = _2q4;
sdloh 0:6100f19d230b 629 J_13or22 = _2q1;
sdloh 0:6100f19d230b 630 J_14or21 = _2q2;
sdloh 0:6100f19d230b 631 J_32 = 2.0f * J_14or21;
sdloh 0:6100f19d230b 632 J_33 = 2.0f * J_11or24;
sdloh 0:6100f19d230b 633
sdloh 0:6100f19d230b 634 // Compute the gradient (matrix multiplication)
sdloh 0:6100f19d230b 635 hatDot1 = J_14or21 * f2 - J_11or24 * f1;
sdloh 0:6100f19d230b 636 hatDot2 = J_12or23 * f1 + J_13or22 * f2 - J_32 * f3;
sdloh 0:6100f19d230b 637 hatDot3 = J_12or23 * f2 - J_33 *f3 - J_13or22 * f1;
sdloh 0:6100f19d230b 638 hatDot4 = J_14or21 * f1 + J_11or24 * f2;
sdloh 0:6100f19d230b 639
sdloh 0:6100f19d230b 640 // Normalize the gradient
sdloh 0:6100f19d230b 641 norm = sqrt(hatDot1 * hatDot1 + hatDot2 * hatDot2 + hatDot3 * hatDot3 + hatDot4 * hatDot4);
sdloh 0:6100f19d230b 642 hatDot1 /= norm;
sdloh 0:6100f19d230b 643 hatDot2 /= norm;
sdloh 0:6100f19d230b 644 hatDot3 /= norm;
sdloh 0:6100f19d230b 645 hatDot4 /= norm;
sdloh 0:6100f19d230b 646
sdloh 0:6100f19d230b 647 // Compute estimated gyroscope biases
sdloh 0:6100f19d230b 648 gerrx = _2q1 * hatDot2 - _2q2 * hatDot1 - _2q3 * hatDot4 + _2q4 * hatDot3;
sdloh 0:6100f19d230b 649 gerry = _2q1 * hatDot3 + _2q2 * hatDot4 - _2q3 * hatDot1 - _2q4 * hatDot2;
sdloh 0:6100f19d230b 650 gerrz = _2q1 * hatDot4 - _2q2 * hatDot3 + _2q3 * hatDot2 - _2q4 * hatDot1;
sdloh 0:6100f19d230b 651
sdloh 0:6100f19d230b 652 // Compute and remove gyroscope biases
sdloh 0:6100f19d230b 653 gbiasx += gerrx * deltat * zeta;
sdloh 0:6100f19d230b 654 gbiasy += gerry * deltat * zeta;
sdloh 0:6100f19d230b 655 gbiasz += gerrz * deltat * zeta;
sdloh 0:6100f19d230b 656 // gx -= gbiasx;
sdloh 0:6100f19d230b 657 // gy -= gbiasy;
sdloh 0:6100f19d230b 658 // gz -= gbiasz;
sdloh 0:6100f19d230b 659
sdloh 0:6100f19d230b 660 // Compute the quaternion derivative
sdloh 0:6100f19d230b 661 qDot1 = -_halfq2 * gx - _halfq3 * gy - _halfq4 * gz;
sdloh 0:6100f19d230b 662 qDot2 = _halfq1 * gx + _halfq3 * gz - _halfq4 * gy;
sdloh 0:6100f19d230b 663 qDot3 = _halfq1 * gy - _halfq2 * gz + _halfq4 * gx;
sdloh 0:6100f19d230b 664 qDot4 = _halfq1 * gz + _halfq2 * gy - _halfq3 * gx;
sdloh 0:6100f19d230b 665
sdloh 0:6100f19d230b 666 // Compute then integrate estimated quaternion derivative
sdloh 0:6100f19d230b 667 q1 += (qDot1 -(beta * hatDot1)) * deltat;
sdloh 0:6100f19d230b 668 q2 += (qDot2 -(beta * hatDot2)) * deltat;
sdloh 0:6100f19d230b 669 q3 += (qDot3 -(beta * hatDot3)) * deltat;
sdloh 0:6100f19d230b 670 q4 += (qDot4 -(beta * hatDot4)) * deltat;
sdloh 0:6100f19d230b 671
sdloh 0:6100f19d230b 672 // Normalize the quaternion
sdloh 0:6100f19d230b 673 norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
sdloh 0:6100f19d230b 674 norm = 1.0f/norm;
sdloh 0:6100f19d230b 675 q[0] = q1 * norm;
sdloh 0:6100f19d230b 676 q[1] = q2 * norm;
sdloh 0:6100f19d230b 677 q[2] = q3 * norm;
sdloh 0:6100f19d230b 678 q[3] = q4 * norm;
sdloh 0:6100f19d230b 679
sdloh 0:6100f19d230b 680 }
sdloh 0:6100f19d230b 681
sdloh 0:6100f19d230b 682
sdloh 0:6100f19d230b 683 };
sdloh 0:6100f19d230b 684 #endif