Rogelio Vazquez / Mbed 2 deprecated Robosub_test

first publish

Dependencies:   HMC5883L mbed

Sensors/MPU6050.h@3:394c971eab83, 2017-06-04 (annotated)

Committer:
roger_wee
Date:
Sun Jun 04 06:58:45 2017 +0000
Revision:
3:394c971eab83
Parent:
2:359f1f075c72 
9-dof implementation using madgwick's filter

Who changed what in which revision?

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