Screen-Puppet
Dependencies: Matrix MatrixMath PCA9547 PowerControl mbed
Fork of mbed_multiplex by
MPU9250.h@1:f0f34b17c4f0, 2015-09-04 (annotated)
- Committer:
- yenzo
- Date:
- Fri Sep 04 21:44:01 2015 +0000
- Revision:
- 1:f0f34b17c4f0
- Parent:
- 0:80f939ca1f14
With matrix
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yenzo | 0:80f939ca1f14 | 1 | #include "DefineMPU.h" |
yenzo | 1:f0f34b17c4f0 | 2 | #include "CalculMatrix.h" |
yenzo | 0:80f939ca1f14 | 3 | #include "mbed.h" |
yenzo | 0:80f939ca1f14 | 4 | #include "math.h" |
yenzo | 0:80f939ca1f14 | 5 | |
yenzo | 0:80f939ca1f14 | 6 | #define Kp 3.0f * 5.0f // 2 - 5 these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral |
yenzo | 0:80f939ca1f14 | 7 | #define Ki 1.0f |
yenzo | 0:80f939ca1f14 | 8 | |
yenzo | 0:80f939ca1f14 | 9 | class MPU9250 { |
yenzo | 0:80f939ca1f14 | 10 | public : |
yenzo | 0:80f939ca1f14 | 11 | int numero; |
yenzo | 0:80f939ca1f14 | 12 | |
yenzo | 0:80f939ca1f14 | 13 | private: |
yenzo | 0:80f939ca1f14 | 14 | |
yenzo | 0:80f939ca1f14 | 15 | static float Filter; |
yenzo | 0:80f939ca1f14 | 16 | |
yenzo | 0:80f939ca1f14 | 17 | static float magCalibration[3]; // Factory mag calibration and mag bias |
yenzo | 0:80f939ca1f14 | 18 | static float magbias[3]; // Factory mag calibration and mag bias |
yenzo | 0:80f939ca1f14 | 19 | |
yenzo | 0:80f939ca1f14 | 20 | static float gyroBias[3]; // Bias corrections for gyro and accelerometer |
yenzo | 0:80f939ca1f14 | 21 | static float accelBias[3]; // Bias corrections for gyro and accelerometer |
yenzo | 0:80f939ca1f14 | 22 | |
yenzo | 0:80f939ca1f14 | 23 | int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output |
yenzo | 0:80f939ca1f14 | 24 | int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output |
yenzo | 0:80f939ca1f14 | 25 | int16_t magCount[3]; // Stores the 16-bit signed magnetometer sensor output |
yenzo | 0:80f939ca1f14 | 26 | |
yenzo | 0:80f939ca1f14 | 27 | int16_t tempCount; // Stores the real internal chip temperature in degrees Celsius |
yenzo | 0:80f939ca1f14 | 28 | float temperature; |
yenzo | 0:80f939ca1f14 | 29 | float SelfTest[6]; |
yenzo | 0:80f939ca1f14 | 30 | |
yenzo | 0:80f939ca1f14 | 31 | static float GyroMeasError; // gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3 |
yenzo | 0:80f939ca1f14 | 32 | static float beta; // compute beta |
yenzo | 0:80f939ca1f14 | 33 | static float GyroMeasDrift; // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) |
yenzo | 0:80f939ca1f14 | 34 | static float zeta; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value |
yenzo | 0:80f939ca1f14 | 35 | |
yenzo | 0:80f939ca1f14 | 36 | float pitch, yaw, roll; |
yenzo | 0:80f939ca1f14 | 37 | |
yenzo | 0:80f939ca1f14 | 38 | static float q[4]; // vector to hold quaternion |
yenzo | 0:80f939ca1f14 | 39 | static float eInt[3]; // vector to hold integral error for Mahony method |
yenzo | 0:80f939ca1f14 | 40 | |
yenzo | 0:80f939ca1f14 | 41 | float aRes, gRes, mRes; // scale resolutions per LSB for the sensors |
yenzo | 0:80f939ca1f14 | 42 | float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values |
yenzo | 0:80f939ca1f14 | 43 | |
yenzo | 0:80f939ca1f14 | 44 | public: |
yenzo | 0:80f939ca1f14 | 45 | |
yenzo | 0:80f939ca1f14 | 46 | //=================================================================================================================== |
yenzo | 0:80f939ca1f14 | 47 | //====== Set of useful function to access acceleratio, gyroscope, and temperature data |
yenzo | 0:80f939ca1f14 | 48 | //=================================================================================================================== |
yenzo | 0:80f939ca1f14 | 49 | |
yenzo | 0:80f939ca1f14 | 50 | MPU9250() {} |
yenzo | 0:80f939ca1f14 | 51 | |
yenzo | 0:80f939ca1f14 | 52 | MPU9250(int n){ numero = n; } |
yenzo | 0:80f939ca1f14 | 53 | |
yenzo | 0:80f939ca1f14 | 54 | int GetPitch (void){ return (int)pitch; } |
yenzo | 0:80f939ca1f14 | 55 | int GetRoll (void){ return (int)roll; } |
yenzo | 0:80f939ca1f14 | 56 | int GetYaw (void){ return (int)yaw; } |
yenzo | 0:80f939ca1f14 | 57 | |
yenzo | 0:80f939ca1f14 | 58 | void writeByte(uint8_t address, uint8_t subAddress, uint8_t data){ |
yenzo | 0:80f939ca1f14 | 59 | char data_write[2]; |
yenzo | 0:80f939ca1f14 | 60 | data_write[0] = subAddress; |
yenzo | 0:80f939ca1f14 | 61 | data_write[1] = data; |
yenzo | 0:80f939ca1f14 | 62 | i2c.write(address, data_write, 2, 0); |
yenzo | 0:80f939ca1f14 | 63 | } |
yenzo | 0:80f939ca1f14 | 64 | |
yenzo | 0:80f939ca1f14 | 65 | char readByte(uint8_t address, uint8_t subAddress){ |
yenzo | 0:80f939ca1f14 | 66 | char data[1]; |
yenzo | 0:80f939ca1f14 | 67 | char data_write[1]; |
yenzo | 0:80f939ca1f14 | 68 | data_write[0] = subAddress; |
yenzo | 0:80f939ca1f14 | 69 | i2c.write(address, data_write, 1, 1); |
yenzo | 0:80f939ca1f14 | 70 | i2c.read(address, data, 1, 0); |
yenzo | 0:80f939ca1f14 | 71 | return data[0]; |
yenzo | 0:80f939ca1f14 | 72 | } |
yenzo | 0:80f939ca1f14 | 73 | |
yenzo | 0:80f939ca1f14 | 74 | void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest){ |
yenzo | 0:80f939ca1f14 | 75 | char data[14]; |
yenzo | 0:80f939ca1f14 | 76 | char data_write[1]; |
yenzo | 0:80f939ca1f14 | 77 | data_write[0] = subAddress; |
yenzo | 0:80f939ca1f14 | 78 | i2c.write(address, data_write, 1, 1); |
yenzo | 0:80f939ca1f14 | 79 | i2c.read(address, data, count, 0); |
yenzo | 0:80f939ca1f14 | 80 | for(int ii = 0; ii < count; ii++) { |
yenzo | 0:80f939ca1f14 | 81 | dest[ii] = data[ii]; |
yenzo | 0:80f939ca1f14 | 82 | } |
yenzo | 0:80f939ca1f14 | 83 | } |
yenzo | 0:80f939ca1f14 | 84 | |
yenzo | 0:80f939ca1f14 | 85 | void getMres() { |
yenzo | 0:80f939ca1f14 | 86 | switch (Mscale){ |
yenzo | 0:80f939ca1f14 | 87 | case MFS_14BITS: mRes = 10.0*4219.0/8190.0; break; |
yenzo | 0:80f939ca1f14 | 88 | case MFS_16BITS: mRes = 10.0*4219.0/32760.0; break; |
yenzo | 0:80f939ca1f14 | 89 | } |
yenzo | 0:80f939ca1f14 | 90 | } |
yenzo | 0:80f939ca1f14 | 91 | |
yenzo | 0:80f939ca1f14 | 92 | void getGres() { |
yenzo | 0:80f939ca1f14 | 93 | switch (Gscale){ |
yenzo | 0:80f939ca1f14 | 94 | case GFS_250DPS: gRes = 250.0/32768.0; break; |
yenzo | 0:80f939ca1f14 | 95 | case GFS_500DPS: gRes = 500.0/32768.0; break; |
yenzo | 0:80f939ca1f14 | 96 | case GFS_1000DPS: gRes = 1000.0/32768.0; break; |
yenzo | 0:80f939ca1f14 | 97 | case GFS_2000DPS: gRes = 2000.0/32768.0; break; |
yenzo | 0:80f939ca1f14 | 98 | } |
yenzo | 0:80f939ca1f14 | 99 | } |
yenzo | 0:80f939ca1f14 | 100 | |
yenzo | 0:80f939ca1f14 | 101 | void getAres() { |
yenzo | 0:80f939ca1f14 | 102 | switch (Ascale){ |
yenzo | 0:80f939ca1f14 | 103 | case AFS_2G: aRes = 2.0/32768.0; break; |
yenzo | 0:80f939ca1f14 | 104 | case AFS_4G: aRes = 4.0/32768.0; break; |
yenzo | 0:80f939ca1f14 | 105 | case AFS_8G: aRes = 8.0/32768.0; break; |
yenzo | 0:80f939ca1f14 | 106 | case AFS_16G: aRes = 16.0/32768.0; break; |
yenzo | 0:80f939ca1f14 | 107 | } |
yenzo | 0:80f939ca1f14 | 108 | } |
yenzo | 0:80f939ca1f14 | 109 | |
yenzo | 0:80f939ca1f14 | 110 | void readAccelData(int16_t * destination){ |
yenzo | 0:80f939ca1f14 | 111 | uint8_t rawData[6]; // x/y/z accel register data stored here |
yenzo | 0:80f939ca1f14 | 112 | readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array |
yenzo | 0:80f939ca1f14 | 113 | destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value |
yenzo | 0:80f939ca1f14 | 114 | destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ; |
yenzo | 0:80f939ca1f14 | 115 | destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; |
yenzo | 0:80f939ca1f14 | 116 | } |
yenzo | 0:80f939ca1f14 | 117 | |
yenzo | 0:80f939ca1f14 | 118 | void readGyroData(int16_t * destination){ |
yenzo | 0:80f939ca1f14 | 119 | uint8_t rawData[6]; // x/y/z gyro register data stored here |
yenzo | 0:80f939ca1f14 | 120 | readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array |
yenzo | 0:80f939ca1f14 | 121 | destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value |
yenzo | 0:80f939ca1f14 | 122 | destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ; |
yenzo | 0:80f939ca1f14 | 123 | destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; |
yenzo | 0:80f939ca1f14 | 124 | } |
yenzo | 0:80f939ca1f14 | 125 | |
yenzo | 0:80f939ca1f14 | 126 | void readMagData(int16_t * destination){ |
yenzo | 0:80f939ca1f14 | 127 | uint8_t rawData[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition |
yenzo | 0:80f939ca1f14 | 128 | if(readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01) { // wait for magnetometer data ready bit to be set |
yenzo | 0:80f939ca1f14 | 129 | readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]); // Read the six raw data and ST2 registers sequentially into data array |
yenzo | 0:80f939ca1f14 | 130 | uint8_t c = rawData[6]; // End data read by reading ST2 register |
yenzo | 0:80f939ca1f14 | 131 | if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data |
yenzo | 0:80f939ca1f14 | 132 | destination[0] = (int16_t)(((int16_t)rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value |
yenzo | 0:80f939ca1f14 | 133 | destination[1] = (int16_t)(((int16_t)rawData[3] << 8) | rawData[2]) ; // Data stored as little Endian |
yenzo | 0:80f939ca1f14 | 134 | destination[2] = (int16_t)(((int16_t)rawData[5] << 8) | rawData[4]) ; |
yenzo | 0:80f939ca1f14 | 135 | } |
yenzo | 0:80f939ca1f14 | 136 | } |
yenzo | 0:80f939ca1f14 | 137 | } |
yenzo | 0:80f939ca1f14 | 138 | |
yenzo | 0:80f939ca1f14 | 139 | void resetMPU9250() { // reset device |
yenzo | 0:80f939ca1f14 | 140 | writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device |
yenzo | 0:80f939ca1f14 | 141 | wait(0.1); |
yenzo | 0:80f939ca1f14 | 142 | } |
yenzo | 0:80f939ca1f14 | 143 | |
yenzo | 0:80f939ca1f14 | 144 | void initAK8963(float * destination){ // First extract the factory calibration for each magnetometer axis |
yenzo | 0:80f939ca1f14 | 145 | uint8_t rawData[3]; // x/y/z gyro calibration data stored here |
yenzo | 0:80f939ca1f14 | 146 | writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer |
yenzo | 0:80f939ca1f14 | 147 | wait(0.01); |
yenzo | 0:80f939ca1f14 | 148 | writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode |
yenzo | 0:80f939ca1f14 | 149 | wait(0.01); |
yenzo | 0:80f939ca1f14 | 150 | readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]); // Read the x-, y-, and z-axis calibration values |
yenzo | 0:80f939ca1f14 | 151 | destination[0] = (float)(rawData[0] - 128)/256.0f + 1.0f; // Return x-axis sensitivity adjustment values, etc. |
yenzo | 0:80f939ca1f14 | 152 | destination[1] = (float)(rawData[1] - 128)/256.0f + 1.0f; |
yenzo | 0:80f939ca1f14 | 153 | destination[2] = (float)(rawData[2] - 128)/256.0f + 1.0f; |
yenzo | 0:80f939ca1f14 | 154 | writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer |
yenzo | 0:80f939ca1f14 | 155 | wait(0.01); |
yenzo | 0:80f939ca1f14 | 156 | writeByte(AK8963_ADDRESS, AK8963_CNTL, Mscale << 4 | Mmode); // Set magnetometer data resolution and sample ODR |
yenzo | 0:80f939ca1f14 | 157 | wait(0.01); |
yenzo | 0:80f939ca1f14 | 158 | } |
yenzo | 0:80f939ca1f14 | 159 | |
yenzo | 0:80f939ca1f14 | 160 | void initMPU9250(){ |
yenzo | 0:80f939ca1f14 | 161 | writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors |
yenzo | 0:80f939ca1f14 | 162 | wait(0.1); // Delay 100 ms for PLL to get established on x-axis gyro; should check for PLL ready interrupt |
yenzo | 0:80f939ca1f14 | 163 | |
yenzo | 0:80f939ca1f14 | 164 | writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001 |
yenzo | 0:80f939ca1f14 | 165 | writeByte(MPU9250_ADDRESS, CONFIG, 0x03); |
yenzo | 0:80f939ca1f14 | 166 | |
yenzo | 0:80f939ca1f14 | 167 | writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04); // Use a 200 Hz rate; the same rate set in CONFIG above |
yenzo | 0:80f939ca1f14 | 168 | |
yenzo | 0:80f939ca1f14 | 169 | uint8_t c = readByte(MPU9250_ADDRESS, GYRO_CONFIG); |
yenzo | 0:80f939ca1f14 | 170 | writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c & ~0xE0); // Clear self-test bits [7:5] |
yenzo | 0:80f939ca1f14 | 171 | writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c & ~0x18); // Clear AFS bits [4:3] |
yenzo | 0:80f939ca1f14 | 172 | writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c | Gscale << 3); // Set full scale range for the gyro |
yenzo | 0:80f939ca1f14 | 173 | |
yenzo | 0:80f939ca1f14 | 174 | c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG); |
yenzo | 0:80f939ca1f14 | 175 | writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c & ~0xE0); // Clear self-test bits [7:5] |
yenzo | 0:80f939ca1f14 | 176 | writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c & ~0x18); // Clear AFS bits [4:3] |
yenzo | 0:80f939ca1f14 | 177 | writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c | Ascale << 3); // Set full scale range for the accelerometer |
yenzo | 0:80f939ca1f14 | 178 | |
yenzo | 0:80f939ca1f14 | 179 | c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2); |
yenzo | 0:80f939ca1f14 | 180 | writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c & ~0x0F); // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0]) |
yenzo | 0:80f939ca1f14 | 181 | writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c | 0x03); // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz |
yenzo | 0:80f939ca1f14 | 182 | |
yenzo | 0:80f939ca1f14 | 183 | writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22); |
yenzo | 0:80f939ca1f14 | 184 | writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt |
yenzo | 0:80f939ca1f14 | 185 | } |
yenzo | 0:80f939ca1f14 | 186 | |
yenzo | 0:80f939ca1f14 | 187 | void calibrateMPU9250(float * dest1, float * dest2){ |
yenzo | 0:80f939ca1f14 | 188 | uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data |
yenzo | 0:80f939ca1f14 | 189 | uint16_t ii, packet_count, fifo_count; |
yenzo | 0:80f939ca1f14 | 190 | int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0}; |
yenzo | 0:80f939ca1f14 | 191 | |
yenzo | 0:80f939ca1f14 | 192 | writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device |
yenzo | 0:80f939ca1f14 | 193 | wait(0.1); |
yenzo | 0:80f939ca1f14 | 194 | |
yenzo | 0:80f939ca1f14 | 195 | writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); |
yenzo | 0:80f939ca1f14 | 196 | writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00); |
yenzo | 0:80f939ca1f14 | 197 | wait(0.2); |
yenzo | 0:80f939ca1f14 | 198 | |
yenzo | 0:80f939ca1f14 | 199 | writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts |
yenzo | 0:80f939ca1f14 | 200 | writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable FIFO |
yenzo | 0:80f939ca1f14 | 201 | writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source |
yenzo | 0:80f939ca1f14 | 202 | writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master |
yenzo | 0:80f939ca1f14 | 203 | writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes |
yenzo | 0:80f939ca1f14 | 204 | writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP |
yenzo | 0:80f939ca1f14 | 205 | wait(0.015); |
yenzo | 0:80f939ca1f14 | 206 | |
yenzo | 0:80f939ca1f14 | 207 | writeByte(MPU9250_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz |
yenzo | 0:80f939ca1f14 | 208 | writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz |
yenzo | 0:80f939ca1f14 | 209 | writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity |
yenzo | 0:80f939ca1f14 | 210 | writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity |
yenzo | 0:80f939ca1f14 | 211 | |
yenzo | 0:80f939ca1f14 | 212 | uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec |
yenzo | 0:80f939ca1f14 | 213 | uint16_t accelsensitivity = 16384; // = 16384 LSB/g |
yenzo | 0:80f939ca1f14 | 214 | |
yenzo | 0:80f939ca1f14 | 215 | writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40); // Enable FIFO |
yenzo | 0:80f939ca1f14 | 216 | writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9250) |
yenzo | 0:80f939ca1f14 | 217 | wait(0.04); // accumulate 40 samples in 80 milliseconds = 480 bytes |
yenzo | 0:80f939ca1f14 | 218 | |
yenzo | 0:80f939ca1f14 | 219 | writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO |
yenzo | 0:80f939ca1f14 | 220 | readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count |
yenzo | 0:80f939ca1f14 | 221 | fifo_count = ((uint16_t)data[0] << 8) | data[1]; |
yenzo | 0:80f939ca1f14 | 222 | packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging |
yenzo | 0:80f939ca1f14 | 223 | |
yenzo | 0:80f939ca1f14 | 224 | for (ii = 0; ii < packet_count; ii++) { |
yenzo | 0:80f939ca1f14 | 225 | int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0}; |
yenzo | 0:80f939ca1f14 | 226 | readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging |
yenzo | 0:80f939ca1f14 | 227 | accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ; // Form signed 16-bit integer for each sample in FIFO |
yenzo | 0:80f939ca1f14 | 228 | accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ; |
yenzo | 0:80f939ca1f14 | 229 | accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ; |
yenzo | 0:80f939ca1f14 | 230 | gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ; |
yenzo | 0:80f939ca1f14 | 231 | gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ; |
yenzo | 0:80f939ca1f14 | 232 | gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ; |
yenzo | 0:80f939ca1f14 | 233 | |
yenzo | 0:80f939ca1f14 | 234 | accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases |
yenzo | 0:80f939ca1f14 | 235 | accel_bias[1] += (int32_t) accel_temp[1]; |
yenzo | 0:80f939ca1f14 | 236 | accel_bias[2] += (int32_t) accel_temp[2]; |
yenzo | 0:80f939ca1f14 | 237 | gyro_bias[0] += (int32_t) gyro_temp[0]; |
yenzo | 0:80f939ca1f14 | 238 | gyro_bias[1] += (int32_t) gyro_temp[1]; |
yenzo | 0:80f939ca1f14 | 239 | gyro_bias[2] += (int32_t) gyro_temp[2]; |
yenzo | 0:80f939ca1f14 | 240 | } |
yenzo | 0:80f939ca1f14 | 241 | |
yenzo | 0:80f939ca1f14 | 242 | accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases |
yenzo | 0:80f939ca1f14 | 243 | accel_bias[1] /= (int32_t) packet_count; |
yenzo | 0:80f939ca1f14 | 244 | accel_bias[2] /= (int32_t) packet_count; |
yenzo | 0:80f939ca1f14 | 245 | gyro_bias[0] /= (int32_t) packet_count; |
yenzo | 0:80f939ca1f14 | 246 | gyro_bias[1] /= (int32_t) packet_count; |
yenzo | 0:80f939ca1f14 | 247 | gyro_bias[2] /= (int32_t) packet_count; |
yenzo | 0:80f939ca1f14 | 248 | |
yenzo | 0:80f939ca1f14 | 249 | if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;} // Remove gravity from the z-axis accelerometer bias calculation |
yenzo | 0:80f939ca1f14 | 250 | else {accel_bias[2] += (int32_t) accelsensitivity;} |
yenzo | 0:80f939ca1f14 | 251 | |
yenzo | 0:80f939ca1f14 | 252 | 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 |
yenzo | 0:80f939ca1f14 | 253 | data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases |
yenzo | 0:80f939ca1f14 | 254 | data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF; |
yenzo | 0:80f939ca1f14 | 255 | data[3] = (-gyro_bias[1]/4) & 0xFF; |
yenzo | 0:80f939ca1f14 | 256 | data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF; |
yenzo | 0:80f939ca1f14 | 257 | data[5] = (-gyro_bias[2]/4) & 0xFF; |
yenzo | 0:80f939ca1f14 | 258 | |
yenzo | 0:80f939ca1f14 | 259 | dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity; // construct gyro bias in deg/s for later manual subtraction |
yenzo | 0:80f939ca1f14 | 260 | dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity; |
yenzo | 0:80f939ca1f14 | 261 | dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity; |
yenzo | 0:80f939ca1f14 | 262 | |
yenzo | 0:80f939ca1f14 | 263 | int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases |
yenzo | 0:80f939ca1f14 | 264 | readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values |
yenzo | 0:80f939ca1f14 | 265 | accel_bias_reg[0] = (int16_t) ((int16_t)data[0] << 8) | data[1]; |
yenzo | 0:80f939ca1f14 | 266 | readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]); |
yenzo | 0:80f939ca1f14 | 267 | accel_bias_reg[1] = (int16_t) ((int16_t)data[0] << 8) | data[1]; |
yenzo | 0:80f939ca1f14 | 268 | readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]); |
yenzo | 0:80f939ca1f14 | 269 | accel_bias_reg[2] = (int16_t) ((int16_t)data[0] << 8) | data[1]; |
yenzo | 0:80f939ca1f14 | 270 | |
yenzo | 0:80f939ca1f14 | 271 | uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers |
yenzo | 0:80f939ca1f14 | 272 | uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis |
yenzo | 0:80f939ca1f14 | 273 | |
yenzo | 0:80f939ca1f14 | 274 | for(ii = 0; ii < 3; ii++) { |
yenzo | 0:80f939ca1f14 | 275 | if(accel_bias_reg[ii] & mask) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit |
yenzo | 0:80f939ca1f14 | 276 | } |
yenzo | 0:80f939ca1f14 | 277 | |
yenzo | 0:80f939ca1f14 | 278 | accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale) |
yenzo | 0:80f939ca1f14 | 279 | accel_bias_reg[1] -= (accel_bias[1]/8); |
yenzo | 0:80f939ca1f14 | 280 | accel_bias_reg[2] -= (accel_bias[2]/8); |
yenzo | 0:80f939ca1f14 | 281 | |
yenzo | 0:80f939ca1f14 | 282 | data[0] = (accel_bias_reg[0] >> 8) & 0xFF; |
yenzo | 0:80f939ca1f14 | 283 | data[1] = (accel_bias_reg[0]) & 0xFF; |
yenzo | 0:80f939ca1f14 | 284 | data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers |
yenzo | 0:80f939ca1f14 | 285 | data[2] = (accel_bias_reg[1] >> 8) & 0xFF; |
yenzo | 0:80f939ca1f14 | 286 | data[3] = (accel_bias_reg[1]) & 0xFF; |
yenzo | 0:80f939ca1f14 | 287 | data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers |
yenzo | 0:80f939ca1f14 | 288 | data[4] = (accel_bias_reg[2] >> 8) & 0xFF; |
yenzo | 0:80f939ca1f14 | 289 | data[5] = (accel_bias_reg[2]) & 0xFF; |
yenzo | 0:80f939ca1f14 | 290 | data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers |
yenzo | 0:80f939ca1f14 | 291 | |
yenzo | 0:80f939ca1f14 | 292 | dest2[0] = (float)accel_bias[0]/(float)accelsensitivity; |
yenzo | 0:80f939ca1f14 | 293 | dest2[1] = (float)accel_bias[1]/(float)accelsensitivity; |
yenzo | 0:80f939ca1f14 | 294 | dest2[2] = (float)accel_bias[2]/(float)accelsensitivity; |
yenzo | 0:80f939ca1f14 | 295 | } |
yenzo | 0:80f939ca1f14 | 296 | |
yenzo | 0:80f939ca1f14 | 297 | void MadgwickQuaternionUpdate(void){ |
yenzo | 0:80f939ca1f14 | 298 | //void MadgwickQuaternionUpdate(float deltat){ |
yenzo | 0:80f939ca1f14 | 299 | |
yenzo | 0:80f939ca1f14 | 300 | /*float tmp = mx; |
yenzo | 0:80f939ca1f14 | 301 | mx = my; |
yenzo | 0:80f939ca1f14 | 302 | my = tmp; |
yenzo | 0:80f939ca1f14 | 303 | az = -az;*/ |
yenzo | 0:80f939ca1f14 | 304 | |
yenzo | 0:80f939ca1f14 | 305 | gx = gx*PI/180.0f; |
yenzo | 0:80f939ca1f14 | 306 | gy = gy*PI/180.0f; |
yenzo | 0:80f939ca1f14 | 307 | gz = gz*PI/180.0f; |
yenzo | 0:80f939ca1f14 | 308 | |
yenzo | 0:80f939ca1f14 | 309 | float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability |
yenzo | 0:80f939ca1f14 | 310 | float norm; |
yenzo | 0:80f939ca1f14 | 311 | float hx, hy, _2bx, _2bz; |
yenzo | 0:80f939ca1f14 | 312 | float s1, s2, s3, s4; |
yenzo | 0:80f939ca1f14 | 313 | float qDot1, qDot2, qDot3, qDot4; |
yenzo | 0:80f939ca1f14 | 314 | |
yenzo | 0:80f939ca1f14 | 315 | float _2q1mx; |
yenzo | 0:80f939ca1f14 | 316 | float _2q1my; |
yenzo | 0:80f939ca1f14 | 317 | float _2q1mz; |
yenzo | 0:80f939ca1f14 | 318 | float _2q2mx; |
yenzo | 0:80f939ca1f14 | 319 | float _4bx; |
yenzo | 0:80f939ca1f14 | 320 | float _4bz; |
yenzo | 0:80f939ca1f14 | 321 | float _2q1 = 2.0f * q1; |
yenzo | 0:80f939ca1f14 | 322 | float _2q2 = 2.0f * q2; |
yenzo | 0:80f939ca1f14 | 323 | float _2q3 = 2.0f * q3; |
yenzo | 0:80f939ca1f14 | 324 | float _2q4 = 2.0f * q4; |
yenzo | 0:80f939ca1f14 | 325 | float _2q1q3 = 2.0f * q1 * q3; |
yenzo | 0:80f939ca1f14 | 326 | float _2q3q4 = 2.0f * q3 * q4; |
yenzo | 0:80f939ca1f14 | 327 | float q1q1 = q1 * q1; |
yenzo | 0:80f939ca1f14 | 328 | float q1q2 = q1 * q2; |
yenzo | 0:80f939ca1f14 | 329 | float q1q3 = q1 * q3; |
yenzo | 0:80f939ca1f14 | 330 | float q1q4 = q1 * q4; |
yenzo | 0:80f939ca1f14 | 331 | float q2q2 = q2 * q2; |
yenzo | 0:80f939ca1f14 | 332 | float q2q3 = q2 * q3; |
yenzo | 0:80f939ca1f14 | 333 | float q2q4 = q2 * q4; |
yenzo | 0:80f939ca1f14 | 334 | float q3q3 = q3 * q3; |
yenzo | 0:80f939ca1f14 | 335 | float q3q4 = q3 * q4; |
yenzo | 0:80f939ca1f14 | 336 | float q4q4 = q4 * q4; |
yenzo | 0:80f939ca1f14 | 337 | |
yenzo | 0:80f939ca1f14 | 338 | norm = sqrt(ax * ax + ay * ay + az * az); |
yenzo | 0:80f939ca1f14 | 339 | if (norm == 0.0f) return; |
yenzo | 0:80f939ca1f14 | 340 | norm = 1.0f/norm; |
yenzo | 0:80f939ca1f14 | 341 | ax *= norm; |
yenzo | 0:80f939ca1f14 | 342 | ay *= norm; |
yenzo | 0:80f939ca1f14 | 343 | az *= norm; |
yenzo | 0:80f939ca1f14 | 344 | |
yenzo | 0:80f939ca1f14 | 345 | norm = sqrt(mx * mx + my * my + mz * mz); |
yenzo | 0:80f939ca1f14 | 346 | if (norm == 0.0f) return; |
yenzo | 0:80f939ca1f14 | 347 | norm = 1.0f/norm; |
yenzo | 0:80f939ca1f14 | 348 | mx *= norm; |
yenzo | 0:80f939ca1f14 | 349 | my *= norm; |
yenzo | 0:80f939ca1f14 | 350 | mz *= norm; |
yenzo | 0:80f939ca1f14 | 351 | |
yenzo | 0:80f939ca1f14 | 352 | _2q1mx = 2.0f * q1 * mx; |
yenzo | 0:80f939ca1f14 | 353 | _2q1my = 2.0f * q1 * my; |
yenzo | 0:80f939ca1f14 | 354 | _2q1mz = 2.0f * q1 * mz; |
yenzo | 0:80f939ca1f14 | 355 | _2q2mx = 2.0f * q2 * mx; |
yenzo | 0:80f939ca1f14 | 356 | |
yenzo | 0:80f939ca1f14 | 357 | hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4; |
yenzo | 0:80f939ca1f14 | 358 | hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4; |
yenzo | 0:80f939ca1f14 | 359 | _2bx = sqrt(hx * hx + hy * hy); |
yenzo | 0:80f939ca1f14 | 360 | _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4; |
yenzo | 0:80f939ca1f14 | 361 | _4bx = 2.0f * _2bx; |
yenzo | 0:80f939ca1f14 | 362 | _4bz = 2.0f * _2bz; |
yenzo | 0:80f939ca1f14 | 363 | |
yenzo | 0:80f939ca1f14 | 364 | 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); |
yenzo | 0:80f939ca1f14 | 365 | 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); |
yenzo | 0:80f939ca1f14 | 366 | 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); |
yenzo | 0:80f939ca1f14 | 367 | 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); |
yenzo | 0:80f939ca1f14 | 368 | |
yenzo | 0:80f939ca1f14 | 369 | norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); |
yenzo | 0:80f939ca1f14 | 370 | norm = 1.0f/norm; |
yenzo | 0:80f939ca1f14 | 371 | s1 *= norm; |
yenzo | 0:80f939ca1f14 | 372 | s2 *= norm; |
yenzo | 0:80f939ca1f14 | 373 | s3 *= norm; |
yenzo | 0:80f939ca1f14 | 374 | s4 *= norm; |
yenzo | 0:80f939ca1f14 | 375 | |
yenzo | 0:80f939ca1f14 | 376 | qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1; |
yenzo | 0:80f939ca1f14 | 377 | qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2; |
yenzo | 0:80f939ca1f14 | 378 | qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3; |
yenzo | 0:80f939ca1f14 | 379 | qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4; |
yenzo | 0:80f939ca1f14 | 380 | |
yenzo | 0:80f939ca1f14 | 381 | /*q1 += qDot1 * deltat; |
yenzo | 0:80f939ca1f14 | 382 | q2 += qDot2 * deltat; |
yenzo | 0:80f939ca1f14 | 383 | q3 += qDot3 * deltat; |
yenzo | 0:80f939ca1f14 | 384 | q4 += qDot4 * deltat;*/ |
yenzo | 0:80f939ca1f14 | 385 | |
yenzo | 0:80f939ca1f14 | 386 | q1 += qDot1 * Filter; |
yenzo | 0:80f939ca1f14 | 387 | q2 += qDot2 * Filter; |
yenzo | 0:80f939ca1f14 | 388 | q3 += qDot3 * Filter; |
yenzo | 0:80f939ca1f14 | 389 | q4 += qDot4 * Filter; |
yenzo | 0:80f939ca1f14 | 390 | |
yenzo | 0:80f939ca1f14 | 391 | norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); |
yenzo | 0:80f939ca1f14 | 392 | norm = 1.0f/norm; |
yenzo | 0:80f939ca1f14 | 393 | q[0] = q1 * norm; |
yenzo | 0:80f939ca1f14 | 394 | q[1] = q2 * norm; |
yenzo | 0:80f939ca1f14 | 395 | q[2] = q3 * norm; |
yenzo | 0:80f939ca1f14 | 396 | q[3] = q4 * norm; |
yenzo | 0:80f939ca1f14 | 397 | } |
yenzo | 0:80f939ca1f14 | 398 | |
yenzo | 0:80f939ca1f14 | 399 | int Float_to_Int(float numb){ |
yenzo | 0:80f939ca1f14 | 400 | float n = numb -(int)numb; |
yenzo | 0:80f939ca1f14 | 401 | if(n >= 0 && n < 0.5) return (int)numb; |
yenzo | 0:80f939ca1f14 | 402 | if(n >= 0.5 && n <= 1) return (int)numb + 1; |
yenzo | 0:80f939ca1f14 | 403 | if(n <= 0 && n > -0.5) return (int)numb; |
yenzo | 0:80f939ca1f14 | 404 | if(n <= -0.5 && n >= -1) return (int)numb - 1; |
yenzo | 0:80f939ca1f14 | 405 | else return 0; |
yenzo | 0:80f939ca1f14 | 406 | } |
yenzo | 0:80f939ca1f14 | 407 | |
yenzo | 0:80f939ca1f14 | 408 | void CalibIMU(void){ |
yenzo | 0:80f939ca1f14 | 409 | wait(1); |
yenzo | 0:80f939ca1f14 | 410 | resetMPU9250(); |
yenzo | 0:80f939ca1f14 | 411 | calibrateMPU9250(gyroBias, accelBias); |
yenzo | 0:80f939ca1f14 | 412 | wait(2); |
yenzo | 0:80f939ca1f14 | 413 | initMPU9250(); |
yenzo | 0:80f939ca1f14 | 414 | initAK8963(magCalibration); |
yenzo | 0:80f939ca1f14 | 415 | wait(2); |
yenzo | 0:80f939ca1f14 | 416 | } |
yenzo | 0:80f939ca1f14 | 417 | |
yenzo | 0:80f939ca1f14 | 418 | void BiasIMU(void){ |
yenzo | 0:80f939ca1f14 | 419 | getAres(); |
yenzo | 0:80f939ca1f14 | 420 | getGres(); |
yenzo | 0:80f939ca1f14 | 421 | getMres(); |
yenzo | 0:80f939ca1f14 | 422 | magbias[0] = +470.; |
yenzo | 0:80f939ca1f14 | 423 | magbias[1] = +120.; |
yenzo | 0:80f939ca1f14 | 424 | magbias[2] = +125.; |
yenzo | 0:80f939ca1f14 | 425 | } |
yenzo | 0:80f939ca1f14 | 426 | |
yenzo | 0:80f939ca1f14 | 427 | void GetQuaternion(void){ |
yenzo | 0:80f939ca1f14 | 428 | readAccelData(accelCount); |
yenzo | 0:80f939ca1f14 | 429 | ax = (float)accelCount[0]*aRes - accelBias[0]; |
yenzo | 0:80f939ca1f14 | 430 | ay = (float)accelCount[1]*aRes - accelBias[1]; |
yenzo | 0:80f939ca1f14 | 431 | az = (float)accelCount[2]*aRes - accelBias[2]; |
yenzo | 0:80f939ca1f14 | 432 | |
yenzo | 0:80f939ca1f14 | 433 | readGyroData(gyroCount); |
yenzo | 0:80f939ca1f14 | 434 | gx = (float)gyroCount[0]*gRes - gyroBias[0]; |
yenzo | 0:80f939ca1f14 | 435 | gy = (float)gyroCount[1]*gRes - gyroBias[1]; |
yenzo | 0:80f939ca1f14 | 436 | gz = (float)gyroCount[2]*gRes - gyroBias[2]; |
yenzo | 0:80f939ca1f14 | 437 | |
yenzo | 0:80f939ca1f14 | 438 | readMagData(magCount); |
yenzo | 0:80f939ca1f14 | 439 | mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; |
yenzo | 0:80f939ca1f14 | 440 | my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1]; |
yenzo | 0:80f939ca1f14 | 441 | mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2]; |
yenzo | 0:80f939ca1f14 | 442 | } |
yenzo | 0:80f939ca1f14 | 443 | |
yenzo | 0:80f939ca1f14 | 444 | void FinalQuaternion(void){ |
yenzo | 0:80f939ca1f14 | 445 | yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]); |
yenzo | 0:80f939ca1f14 | 446 | pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); |
yenzo | 0:80f939ca1f14 | 447 | roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); |
yenzo | 0:80f939ca1f14 | 448 | pitch *= 180.0f / PI; |
yenzo | 0:80f939ca1f14 | 449 | yaw *= 180.0f / PI; |
yenzo | 0:80f939ca1f14 | 450 | yaw -= 0.9f; |
yenzo | 0:80f939ca1f14 | 451 | roll *= 180.0f / PI; |
yenzo | 0:80f939ca1f14 | 452 | |
yenzo | 0:80f939ca1f14 | 453 | /*if((q[0]*q[1] + q[2]*q[3]) == 0.5){ //north pole |
yenzo | 0:80f939ca1f14 | 454 | pitch = atan2(2*q[1]*q[3]-2*q[0]*q[2], 1 - 2*(q[1] * q[1]) - 2*(q[2] * q[2])); |
yenzo | 0:80f939ca1f14 | 455 | yaw = atan2(2*q[0]*q[3]-2*q[1]*q[2], 1 - 2*(q[0] * q[0]) - 2*(q[2] * q[2])); |
yenzo | 0:80f939ca1f14 | 456 | } |
yenzo | 0:80f939ca1f14 | 457 | else{ |
yenzo | 0:80f939ca1f14 | 458 | pitch = 2.0 * atan2(q[0],q[4]); |
yenzo | 0:80f939ca1f14 | 459 | yaw = 0; |
yenzo | 0:80f939ca1f14 | 460 | } |
yenzo | 0:80f939ca1f14 | 461 | |
yenzo | 0:80f939ca1f14 | 462 | if((q[0]*q[1] + q[2]*q[3]) == -0.5){ //south pole |
yenzo | 0:80f939ca1f14 | 463 | pitch = atan2(2*q[1]*q[3]-2*q[0]*q[2], 1 - 2*(q[1] * q[1]) - 2*(q[2] * q[2])); |
yenzo | 0:80f939ca1f14 | 464 | yaw = atan2(2*q[0]*q[3]-2*q[1]*q[2], 1 - 2*(q[0] * q[0]) - 2*(q[2] * q[2])); |
yenzo | 0:80f939ca1f14 | 465 | } |
yenzo | 0:80f939ca1f14 | 466 | else { |
yenzo | 0:80f939ca1f14 | 467 | pitch = -2.0 * atan2(q[0],q[4]); |
yenzo | 0:80f939ca1f14 | 468 | yaw = 0; |
yenzo | 0:80f939ca1f14 | 469 | } |
yenzo | 0:80f939ca1f14 | 470 | roll = asin(2*q[0]*q[1] + 2*q[2]*q[3]);*/ |
yenzo | 0:80f939ca1f14 | 471 | |
yenzo | 0:80f939ca1f14 | 472 | |
yenzo | 0:80f939ca1f14 | 473 | |
yenzo | 0:80f939ca1f14 | 474 | //if(roll < 180 & roll > 0) roll -= 180; |
yenzo | 0:80f939ca1f14 | 475 | //else if(roll > -180 & roll < 0) roll += 180; |
yenzo | 0:80f939ca1f14 | 476 | |
yenzo | 0:80f939ca1f14 | 477 | pitch = Float_to_Int(pitch); |
yenzo | 0:80f939ca1f14 | 478 | roll = Float_to_Int(roll); |
yenzo | 0:80f939ca1f14 | 479 | yaw = Float_to_Int(yaw); |
yenzo | 0:80f939ca1f14 | 480 | } |
yenzo | 0:80f939ca1f14 | 481 | }; |
yenzo | 0:80f939ca1f14 | 482 | |
yenzo | 0:80f939ca1f14 | 483 | //Definition des variables dans la Class MPU9250, car la definition ne peut pas être faite à l'intérieur |
yenzo | 0:80f939ca1f14 | 484 | float MPU9250::gyroBias[3] = {0, 0, 0}; |
yenzo | 0:80f939ca1f14 | 485 | float MPU9250::accelBias[3] = {0, 0, 0}; // Bias corrections for gyro and accelerometer |
yenzo | 0:80f939ca1f14 | 486 | float MPU9250::magCalibration[3] = {0, 0, 0}; |
yenzo | 0:80f939ca1f14 | 487 | float MPU9250::magbias[3] = {0, 0, 0}; |
yenzo | 0:80f939ca1f14 | 488 | float MPU9250::GyroMeasError = PI * (60.0f / 180.0f); // gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3 |
yenzo | 0:80f939ca1f14 | 489 | float MPU9250::beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta |
yenzo | 0:80f939ca1f14 | 490 | float MPU9250::GyroMeasDrift = PI * (1.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) |
yenzo | 0:80f939ca1f14 | 491 | float MPU9250::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 |
yenzo | 0:80f939ca1f14 | 492 | float MPU9250::q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion |
yenzo | 0:80f939ca1f14 | 493 | float MPU9250::eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony method |
yenzo | 0:80f939ca1f14 | 494 | float MPU9250::Filter = 0.05; |