A Team / DCM_AHRS

DCM Code ported from Arduino for FRDM-KL25Z

Dependents:   minimu_data_capture minimu_data_capture

Fork of DCM_AHRS by Kris Reynolds

minimu9.cpp@0:dc35364e2291, 2012-04-12 (annotated)

Committer:
krmreynolds
Date:
Thu Apr 12 13:47:23 2012 +0000
Revision:
0:dc35364e2291
Child:
1:3272ece36ce1
Added GNU Agreement

Who changed what in which revision?

UserRevisionLine numberNew contents of line
krmreynolds 0:dc35364e2291 1 /* mbed L3G4200D Library version 0.1
krmreynolds 0:dc35364e2291 2 * Copyright (c) 2012 Prediluted
krmreynolds 0:dc35364e2291 3 *
krmreynolds 0:dc35364e2291 4 * Permission is hereby granted, free of charge, to any person obtaining a copy
krmreynolds 0:dc35364e2291 5 * of this software and associated documentation files (the "Software"), to deal
krmreynolds 0:dc35364e2291 6 * in the Software without restriction, including without limitation the rights
krmreynolds 0:dc35364e2291 7 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
krmreynolds 0:dc35364e2291 8 * copies of the Software, and to permit persons to whom the Software is
krmreynolds 0:dc35364e2291 9 * furnished to do so, subject to the following conditions:
krmreynolds 0:dc35364e2291 10 *
krmreynolds 0:dc35364e2291 11 * The above copyright notice and this permission notice shall be included in
krmreynolds 0:dc35364e2291 12 * all copies or substantial portions of the Software.
krmreynolds 0:dc35364e2291 13 *
krmreynolds 0:dc35364e2291 14 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
krmreynolds 0:dc35364e2291 15 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
krmreynolds 0:dc35364e2291 16 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
krmreynolds 0:dc35364e2291 17 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
krmreynolds 0:dc35364e2291 18 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
krmreynolds 0:dc35364e2291 19 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
krmreynolds 0:dc35364e2291 20 * THE SOFTWARE.
krmreynolds 0:dc35364e2291 21 */
krmreynolds 0:dc35364e2291 22
krmreynolds 0:dc35364e2291 23 /*
krmreynolds 0:dc35364e2291 24 * Communication for the minimu9, ported from the original Polulu minIMU-9 Arduino code
krmreynolds 0:dc35364e2291 25 */
krmreynolds 0:dc35364e2291 26 #include <stdlib.h>
krmreynolds 0:dc35364e2291 27 #include <algorithm>
krmreynolds 0:dc35364e2291 28 #include <iostream>
krmreynolds 0:dc35364e2291 29 #include <vector>
krmreynolds 0:dc35364e2291 30 #include <math.h>
krmreynolds 0:dc35364e2291 31 #include "minimu9.h"
krmreynolds 0:dc35364e2291 32 #include "L3G4200D.h"
krmreynolds 0:dc35364e2291 33 #include "LSM303.h"
krmreynolds 0:dc35364e2291 34 #include "Matrix.h"
krmreynolds 0:dc35364e2291 35
krmreynolds 0:dc35364e2291 36
krmreynolds 0:dc35364e2291 37 /*
krmreynolds 0:dc35364e2291 38 * void setup() replaced with the constructor
krmreynolds 0:dc35364e2291 39 */
krmreynolds 0:dc35364e2291 40 minimu9::minimu9() {
krmreynolds 0:dc35364e2291 41 // Calibration constants defaults, will need to calibrate then use
krmreynolds 0:dc35364e2291 42 // set_calibration_constants() to have the proper values for your board
krmreynolds 0:dc35364e2291 43 M_X_MIN = -570;
krmreynolds 0:dc35364e2291 44 M_Y_MIN = -746;
krmreynolds 0:dc35364e2291 45 M_Z_MIN = -416;
krmreynolds 0:dc35364e2291 46 M_X_MAX = 477;
krmreynolds 0:dc35364e2291 47 M_Y_MAX = 324;
krmreynolds 0:dc35364e2291 48 M_Z_MAX = 652;
krmreynolds 0:dc35364e2291 49
krmreynolds 0:dc35364e2291 50 /*For debugging purposes*/
krmreynolds 0:dc35364e2291 51 //OUTPUTMODE=1 will print the corrected data,
krmreynolds 0:dc35364e2291 52 //OUTPUTMODE=0 will print uncorrected data of the gyros (with drift)
krmreynolds 0:dc35364e2291 53 OUTPUTMODE = 1;
krmreynolds 0:dc35364e2291 54
krmreynolds 0:dc35364e2291 55 //#define PRINT_DCM 0 //Will print the whole direction cosine matrix
krmreynolds 0:dc35364e2291 56 PRINT_ANALOGS = 0; //Will print the analog raw data
krmreynolds 0:dc35364e2291 57 PRINT_EULER = 1; //Will print the Euler angles Roll, Pitch and Yaw
krmreynolds 0:dc35364e2291 58 PRINT_DCM = 0;
krmreynolds 0:dc35364e2291 59
krmreynolds 0:dc35364e2291 60 PRINT_SERIAL = 0;
krmreynolds 0:dc35364e2291 61 //PRINT_SERIAL = 1;
krmreynolds 0:dc35364e2291 62
krmreynolds 0:dc35364e2291 63 t.start();
krmreynolds 0:dc35364e2291 64 #if PRINT_SERIAL == 1
krmreynolds 0:dc35364e2291 65 Serial serial(USBTX, USBRX);
krmreynolds 0:dc35364e2291 66 #endif
krmreynolds 0:dc35364e2291 67 gyro = new L3G4200D( p9, p10 );
krmreynolds 0:dc35364e2291 68 compass = new LSM303( p9, p10 );
krmreynolds 0:dc35364e2291 69 I2C i2c(p9, p10);
krmreynolds 0:dc35364e2291 70
krmreynolds 0:dc35364e2291 71 wait(1.5);
krmreynolds 0:dc35364e2291 72 /*
krmreynolds 0:dc35364e2291 73 * Give variables initial values
krmreynolds 0:dc35364e2291 74 */
krmreynolds 0:dc35364e2291 75 G_Dt=0.02;
krmreynolds 0:dc35364e2291 76 timer=0;
krmreynolds 0:dc35364e2291 77 timer24=0;
krmreynolds 0:dc35364e2291 78 counter=0;
krmreynolds 0:dc35364e2291 79 gyro_sat=0;
krmreynolds 0:dc35364e2291 80
krmreynolds 0:dc35364e2291 81 memcpy(SENSOR_SIGN, (int [9]) {
krmreynolds 0:dc35364e2291 82 1,1,1,-1,-1,-1,1,1,1
krmreynolds 0:dc35364e2291 83 }, 9*sizeof(int));
krmreynolds 0:dc35364e2291 84 memcpy(AN_OFFSET, (int [6]) {
krmreynolds 0:dc35364e2291 85 0,0,0,0,0,0
krmreynolds 0:dc35364e2291 86 }, 6*sizeof(int));
krmreynolds 0:dc35364e2291 87 memcpy(Accel_Vector, (float [3]) {
krmreynolds 0:dc35364e2291 88 0,0,0
krmreynolds 0:dc35364e2291 89 }, 3*sizeof(float));
krmreynolds 0:dc35364e2291 90 memcpy(Gyro_Vector, (float [3]) {
krmreynolds 0:dc35364e2291 91 0,0,0
krmreynolds 0:dc35364e2291 92 }, 3*sizeof(float));
krmreynolds 0:dc35364e2291 93 memcpy(Omega_Vector, (float [3]) {
krmreynolds 0:dc35364e2291 94 0,0,0
krmreynolds 0:dc35364e2291 95 }, 3*sizeof(float));
krmreynolds 0:dc35364e2291 96 memcpy(Omega_P, (float [3]) {
krmreynolds 0:dc35364e2291 97 0,0,0
krmreynolds 0:dc35364e2291 98 }, 3*sizeof(float));
krmreynolds 0:dc35364e2291 99 memcpy(Omega_I, (float [3]) {
krmreynolds 0:dc35364e2291 100 0,0,0
krmreynolds 0:dc35364e2291 101 }, 3*sizeof(float));
krmreynolds 0:dc35364e2291 102 memcpy(Omega, (float [3]) {
krmreynolds 0:dc35364e2291 103 0,0,0
krmreynolds 0:dc35364e2291 104 }, 3*sizeof(float));
krmreynolds 0:dc35364e2291 105 memcpy(errorRollPitch, (float [3]) {
krmreynolds 0:dc35364e2291 106 0,0,0
krmreynolds 0:dc35364e2291 107 }, 3*sizeof(float));
krmreynolds 0:dc35364e2291 108 memcpy(errorYaw, (float [3]) {
krmreynolds 0:dc35364e2291 109 0,0,0
krmreynolds 0:dc35364e2291 110 }, 3*sizeof(float));
krmreynolds 0:dc35364e2291 111 memcpy( DCM_Matrix, (float[3][3]) {
krmreynolds 0:dc35364e2291 112 {
krmreynolds 0:dc35364e2291 113 1,0,0
krmreynolds 0:dc35364e2291 114 }, { 0,1,0 }, { 0,0,1 }
krmreynolds 0:dc35364e2291 115 }, 9*sizeof(float) );
krmreynolds 0:dc35364e2291 116 memcpy( Update_Matrix, (float[3][3]) {
krmreynolds 0:dc35364e2291 117 {
krmreynolds 0:dc35364e2291 118 0,1,2
krmreynolds 0:dc35364e2291 119 }, { 3,4,5 }, { 6,7,8 }
krmreynolds 0:dc35364e2291 120 }, 9*sizeof(float) );
krmreynolds 0:dc35364e2291 121 memcpy( Temporary_Matrix, (float[3][3]) {
krmreynolds 0:dc35364e2291 122 {
krmreynolds 0:dc35364e2291 123 0,0,0
krmreynolds 0:dc35364e2291 124 }, { 0,0,0 }, { 0,0,0 }
krmreynolds 0:dc35364e2291 125 }, 9*sizeof(float) );
krmreynolds 0:dc35364e2291 126
krmreynolds 0:dc35364e2291 127 /*
krmreynolds 0:dc35364e2291 128 * Initialize the accel, compass, and gyro
krmreynolds 0:dc35364e2291 129 */
krmreynolds 0:dc35364e2291 130 // Accel
krmreynolds 0:dc35364e2291 131 compass->accRegisterWrite(LSM303::ACC_CTRL_REG1, 0x24); // normal power mode, all axes enabled, 50 Hz
krmreynolds 0:dc35364e2291 132 compass->accRegisterWrite(LSM303::ACC_CTRL_REG4, 0x30 ); // 8g full scale
krmreynolds 0:dc35364e2291 133 // Compass
krmreynolds 0:dc35364e2291 134 compass->magRegisterWrite(LSM303::MAG_MR_REG, 0x00); // continuous conversion mode
krmreynolds 0:dc35364e2291 135 // Gyro
krmreynolds 0:dc35364e2291 136 gyro->registerWrite(L3G4200D::CTRL_REG1, 0x0F); // normal power mode, all axes enabled, 100 Hz
krmreynolds 0:dc35364e2291 137 gyro->registerWrite(L3G4200D::CTRL_REG4, 0x20); // 2000 dps full scale
krmreynolds 0:dc35364e2291 138
krmreynolds 0:dc35364e2291 139 #if PRINT_SERIAL == 1
krmreynolds 0:dc35364e2291 140 serial.printf( "initiatied\n" );
krmreynolds 0:dc35364e2291 141 }
krmreynolds 0:dc35364e2291 142 #endif
krmreynolds 0:dc35364e2291 143
krmreynolds 0:dc35364e2291 144 for (int i=0; i<32; i++) { // We take some readings...
krmreynolds 0:dc35364e2291 145 Read_Gyro();
krmreynolds 0:dc35364e2291 146 Read_Accel();
krmreynolds 0:dc35364e2291 147 for (int y=0; y<6; y++) // Cumulate values
krmreynolds 0:dc35364e2291 148 AN_OFFSET[y] += AN[y];
krmreynolds 0:dc35364e2291 149 wait(0.2);
krmreynolds 0:dc35364e2291 150 }
krmreynolds 0:dc35364e2291 151
krmreynolds 0:dc35364e2291 152 for (int y=0; y<6; y++)
krmreynolds 0:dc35364e2291 153 AN_OFFSET[y] = AN_OFFSET[y]/32;
krmreynolds 0:dc35364e2291 154
krmreynolds 0:dc35364e2291 155 AN_OFFSET[5]-=GRAVITY*SENSOR_SIGN[5];
krmreynolds 0:dc35364e2291 156
krmreynolds 0:dc35364e2291 157 #if PRINT_SERIAL == 1
krmreynolds 0:dc35364e2291 158 serial.printf("Offset:\n");
krmreynolds 0:dc35364e2291 159 for (int y=0; y<6; y++) {
krmreynolds 0:dc35364e2291 160 serial.printf( "%d", AN_OFFSET[y] );
krmreynolds 0:dc35364e2291 161 }
krmreynolds 0:dc35364e2291 162 serial.printf( "\n" );
krmreynolds 0:dc35364e2291 163 #endif
krmreynolds 0:dc35364e2291 164 wait(2);
krmreynolds 0:dc35364e2291 165 timer=t.read_ms();
krmreynolds 0:dc35364e2291 166 wait(0.2);
krmreynolds 0:dc35364e2291 167 counter=0;
krmreynolds 0:dc35364e2291 168
krmreynolds 0:dc35364e2291 169 #if PRINT_SERIAL == 1
krmreynolds 0:dc35364e2291 170 while ( 1 ) {
krmreynolds 0:dc35364e2291 171 get_data();
krmreynolds 0:dc35364e2291 172 }
krmreynolds 0:dc35364e2291 173 #endif
krmreynolds 0:dc35364e2291 174 }
krmreynolds 0:dc35364e2291 175
krmreynolds 0:dc35364e2291 176 void minimu9::get_data() { //Main Loop
krmreynolds 0:dc35364e2291 177 if ((t.read_ms()-timer)>=20) { // Main loop runs at 50Hz
krmreynolds 0:dc35364e2291 178 counter++;
krmreynolds 0:dc35364e2291 179 timer_old = timer;
krmreynolds 0:dc35364e2291 180 timer=t.read_ms();
krmreynolds 0:dc35364e2291 181 if (timer>timer_old)
krmreynolds 0:dc35364e2291 182 G_Dt = (timer-timer_old)/1000.0; // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
krmreynolds 0:dc35364e2291 183 else
krmreynolds 0:dc35364e2291 184 G_Dt = 0;
krmreynolds 0:dc35364e2291 185
krmreynolds 0:dc35364e2291 186 // *** DCM algorithm
krmreynolds 0:dc35364e2291 187 // Data adquisition
krmreynolds 0:dc35364e2291 188 Read_Gyro(); // This read gyro data
krmreynolds 0:dc35364e2291 189 Read_Accel(); // Read I2C accelerometer
krmreynolds 0:dc35364e2291 190
krmreynolds 0:dc35364e2291 191 if (counter > 5) { // Read compass data at 10Hz... (5 loop runs)
krmreynolds 0:dc35364e2291 192 counter=0;
krmreynolds 0:dc35364e2291 193 Read_Compass(); // Read I2C magnetometer
krmreynolds 0:dc35364e2291 194 Compass_Heading(); // Calculate magnetic heading
krmreynolds 0:dc35364e2291 195 }
krmreynolds 0:dc35364e2291 196
krmreynolds 0:dc35364e2291 197 // Calculations...
krmreynolds 0:dc35364e2291 198 Matrix_update();
krmreynolds 0:dc35364e2291 199 Normalize();
krmreynolds 0:dc35364e2291 200 Drift_correction();
krmreynolds 0:dc35364e2291 201 Euler_angles();
krmreynolds 0:dc35364e2291 202 // ***
krmreynolds 0:dc35364e2291 203
krmreynolds 0:dc35364e2291 204 if ( 1 == PRINT_SERIAL ) {
krmreynolds 0:dc35364e2291 205 print_data();
krmreynolds 0:dc35364e2291 206 }
krmreynolds 0:dc35364e2291 207 }
krmreynolds 0:dc35364e2291 208
krmreynolds 0:dc35364e2291 209 }
krmreynolds 0:dc35364e2291 210
krmreynolds 0:dc35364e2291 211 long convert_to_dec(float x) {
krmreynolds 0:dc35364e2291 212 return x*10000000;
krmreynolds 0:dc35364e2291 213 }
krmreynolds 0:dc35364e2291 214
krmreynolds 0:dc35364e2291 215 void minimu9::Read_Gyro() {
krmreynolds 0:dc35364e2291 216 gyro->read();
krmreynolds 0:dc35364e2291 217 std::vector<short> g = gyro->read();
krmreynolds 0:dc35364e2291 218 AN[0] = g[0];
krmreynolds 0:dc35364e2291 219 AN[1] = g[1];
krmreynolds 0:dc35364e2291 220 AN[2] = g[2];
krmreynolds 0:dc35364e2291 221
krmreynolds 0:dc35364e2291 222 gyro_x = SENSOR_SIGN[0] * (AN[0] - AN_OFFSET[0]);
krmreynolds 0:dc35364e2291 223 gyro_y = SENSOR_SIGN[1] * (AN[1] - AN_OFFSET[1]);
krmreynolds 0:dc35364e2291 224 gyro_z = SENSOR_SIGN[2] * (AN[2] - AN_OFFSET[2]);
krmreynolds 0:dc35364e2291 225 }
krmreynolds 0:dc35364e2291 226
krmreynolds 0:dc35364e2291 227
krmreynolds 0:dc35364e2291 228 // Reads x,y and z accelerometer registers
krmreynolds 0:dc35364e2291 229 void minimu9::Read_Accel() {
krmreynolds 0:dc35364e2291 230 std::vector<short> a = compass->accRead();
krmreynolds 0:dc35364e2291 231
krmreynolds 0:dc35364e2291 232 AN[3] = a[0];
krmreynolds 0:dc35364e2291 233 AN[4] = a[1];
krmreynolds 0:dc35364e2291 234 AN[5] = a[2];
krmreynolds 0:dc35364e2291 235
krmreynolds 0:dc35364e2291 236 accel_x = SENSOR_SIGN[3] * (AN[3] - AN_OFFSET[3]);
krmreynolds 0:dc35364e2291 237 accel_y = SENSOR_SIGN[4] * (AN[4] - AN_OFFSET[4]);
krmreynolds 0:dc35364e2291 238 accel_z = SENSOR_SIGN[5] * (AN[5] - AN_OFFSET[5]);
krmreynolds 0:dc35364e2291 239 }
krmreynolds 0:dc35364e2291 240
krmreynolds 0:dc35364e2291 241 void minimu9::Compass_Heading() {
krmreynolds 0:dc35364e2291 242 float MAG_X;
krmreynolds 0:dc35364e2291 243 float MAG_Y;
krmreynolds 0:dc35364e2291 244 float cos_roll;
krmreynolds 0:dc35364e2291 245 float sin_roll;
krmreynolds 0:dc35364e2291 246 float cos_pitch;
krmreynolds 0:dc35364e2291 247 float sin_pitch;
krmreynolds 0:dc35364e2291 248
krmreynolds 0:dc35364e2291 249 cos_roll = cos(roll);
krmreynolds 0:dc35364e2291 250 sin_roll = sin(roll);
krmreynolds 0:dc35364e2291 251 cos_pitch = cos(pitch);
krmreynolds 0:dc35364e2291 252 sin_pitch = sin(pitch);
krmreynolds 0:dc35364e2291 253
krmreynolds 0:dc35364e2291 254 // adjust for LSM303 compass axis offsets/sensitivity differences by scaling to +/-0.5 range
krmreynolds 0:dc35364e2291 255 c_magnetom_x = (float)(magnetom_x - SENSOR_SIGN[6]*M_X_MIN) / (M_X_MAX - M_X_MIN) - SENSOR_SIGN[6]*0.0;
krmreynolds 0:dc35364e2291 256 c_magnetom_y = (float)(magnetom_y - SENSOR_SIGN[7]*M_Y_MIN) / (M_Y_MAX - M_Y_MIN) - SENSOR_SIGN[7]*-0.5;
krmreynolds 0:dc35364e2291 257 c_magnetom_z = (float)(magnetom_z - SENSOR_SIGN[8]*M_Z_MIN) / (M_Z_MAX - M_Z_MIN) - SENSOR_SIGN[8]*-0.5;
krmreynolds 0:dc35364e2291 258
krmreynolds 0:dc35364e2291 259 // Tilt compensated Magnetic filed X:
krmreynolds 0:dc35364e2291 260 MAG_X = c_magnetom_x*cos_pitch+c_magnetom_y*sin_roll*sin_pitch+c_magnetom_z*cos_roll*sin_pitch;
krmreynolds 0:dc35364e2291 261 // Tilt compensated Magnetic filed Y:
krmreynolds 0:dc35364e2291 262 MAG_Y = c_magnetom_y*cos_roll-c_magnetom_z*sin_roll;
krmreynolds 0:dc35364e2291 263 // Magnetic Heading
krmreynolds 0:dc35364e2291 264 MAG_Heading = atan2(-MAG_Y,MAG_X);
krmreynolds 0:dc35364e2291 265 }
krmreynolds 0:dc35364e2291 266
krmreynolds 0:dc35364e2291 267 void minimu9::Read_Compass() {
krmreynolds 0:dc35364e2291 268 std::vector<short> m = compass->magRead();
krmreynolds 0:dc35364e2291 269
krmreynolds 0:dc35364e2291 270 magnetom_x = SENSOR_SIGN[6] * m[0];
krmreynolds 0:dc35364e2291 271 magnetom_y = SENSOR_SIGN[7] * m[1];
krmreynolds 0:dc35364e2291 272 magnetom_z = SENSOR_SIGN[8] * m[2];
krmreynolds 0:dc35364e2291 273 }
krmreynolds 0:dc35364e2291 274
krmreynolds 0:dc35364e2291 275 void minimu9::Normalize(void) {
krmreynolds 0:dc35364e2291 276 float error=0;
krmreynolds 0:dc35364e2291 277 float temporary[3][3];
krmreynolds 0:dc35364e2291 278 float renorm=0;
krmreynolds 0:dc35364e2291 279
krmreynolds 0:dc35364e2291 280 error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19
krmreynolds 0:dc35364e2291 281
krmreynolds 0:dc35364e2291 282 Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
krmreynolds 0:dc35364e2291 283 Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
krmreynolds 0:dc35364e2291 284
krmreynolds 0:dc35364e2291 285 Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19
krmreynolds 0:dc35364e2291 286 Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19
krmreynolds 0:dc35364e2291 287
krmreynolds 0:dc35364e2291 288 Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
krmreynolds 0:dc35364e2291 289
krmreynolds 0:dc35364e2291 290 renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21
krmreynolds 0:dc35364e2291 291 Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
krmreynolds 0:dc35364e2291 292
krmreynolds 0:dc35364e2291 293 renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21
krmreynolds 0:dc35364e2291 294 Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
krmreynolds 0:dc35364e2291 295
krmreynolds 0:dc35364e2291 296 renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21
krmreynolds 0:dc35364e2291 297 Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
krmreynolds 0:dc35364e2291 298 }
krmreynolds 0:dc35364e2291 299
krmreynolds 0:dc35364e2291 300 /**************************************************/
krmreynolds 0:dc35364e2291 301 void minimu9::Drift_correction(void) {
krmreynolds 0:dc35364e2291 302 float mag_heading_x;
krmreynolds 0:dc35364e2291 303 float mag_heading_y;
krmreynolds 0:dc35364e2291 304 float errorCourse;
krmreynolds 0:dc35364e2291 305 //Compensation the Roll, Pitch and Yaw drift.
krmreynolds 0:dc35364e2291 306 static float Scaled_Omega_P[3];
krmreynolds 0:dc35364e2291 307 static float Scaled_Omega_I[3];
krmreynolds 0:dc35364e2291 308 float Accel_magnitude;
krmreynolds 0:dc35364e2291 309 float Accel_weight;
krmreynolds 0:dc35364e2291 310
krmreynolds 0:dc35364e2291 311
krmreynolds 0:dc35364e2291 312 //*****Roll and Pitch***************
krmreynolds 0:dc35364e2291 313
krmreynolds 0:dc35364e2291 314 // Calculate the magnitude of the accelerometer vector
krmreynolds 0:dc35364e2291 315 Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
krmreynolds 0:dc35364e2291 316 Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
krmreynolds 0:dc35364e2291 317 // Dynamic weighting of accelerometer info (reliability filter)
krmreynolds 0:dc35364e2291 318 // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
krmreynolds 0:dc35364e2291 319 Accel_weight = 1 - 2 * abs( 1 - Accel_magnitude );
krmreynolds 0:dc35364e2291 320 if ( Accel_weight < 0 ) {
krmreynolds 0:dc35364e2291 321 Accel_weight = 0;
krmreynolds 0:dc35364e2291 322 } else if ( Accel_weight > 1 ) {
krmreynolds 0:dc35364e2291 323 Accel_weight = 1;
krmreynolds 0:dc35364e2291 324 }
krmreynolds 0:dc35364e2291 325
krmreynolds 0:dc35364e2291 326 Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
krmreynolds 0:dc35364e2291 327 Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
krmreynolds 0:dc35364e2291 328
krmreynolds 0:dc35364e2291 329 Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
krmreynolds 0:dc35364e2291 330 Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
krmreynolds 0:dc35364e2291 331
krmreynolds 0:dc35364e2291 332 //*****YAW***************
krmreynolds 0:dc35364e2291 333 // We make the gyro YAW drift correction based on compass magnetic heading
krmreynolds 0:dc35364e2291 334
krmreynolds 0:dc35364e2291 335 mag_heading_x = cos(MAG_Heading);
krmreynolds 0:dc35364e2291 336 mag_heading_y = sin(MAG_Heading);
krmreynolds 0:dc35364e2291 337 errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x); //Calculating YAW error
krmreynolds 0:dc35364e2291 338 Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
krmreynolds 0:dc35364e2291 339
krmreynolds 0:dc35364e2291 340 Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01 proportional of YAW.
krmreynolds 0:dc35364e2291 341 Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
krmreynolds 0:dc35364e2291 342
krmreynolds 0:dc35364e2291 343 Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001 Integrator
krmreynolds 0:dc35364e2291 344 Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
krmreynolds 0:dc35364e2291 345 }
krmreynolds 0:dc35364e2291 346
krmreynolds 0:dc35364e2291 347 void minimu9::Matrix_update(void) {
krmreynolds 0:dc35364e2291 348 Gyro_Vector[0]=Gyro_Scaled_X(gyro_x); //gyro x roll
krmreynolds 0:dc35364e2291 349 Gyro_Vector[1]=Gyro_Scaled_Y(gyro_y); //gyro y pitch
krmreynolds 0:dc35364e2291 350 Gyro_Vector[2]=Gyro_Scaled_Z(gyro_z); //gyro Z yaw
krmreynolds 0:dc35364e2291 351
krmreynolds 0:dc35364e2291 352 Accel_Vector[0]=accel_x;
krmreynolds 0:dc35364e2291 353 Accel_Vector[1]=accel_y;
krmreynolds 0:dc35364e2291 354 Accel_Vector[2]=accel_z;
krmreynolds 0:dc35364e2291 355
krmreynolds 0:dc35364e2291 356 Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term
krmreynolds 0:dc35364e2291 357 Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
krmreynolds 0:dc35364e2291 358
krmreynolds 0:dc35364e2291 359 #if OUTPUTMODE==1
krmreynolds 0:dc35364e2291 360 Update_Matrix[0][0]=0;
krmreynolds 0:dc35364e2291 361 Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
krmreynolds 0:dc35364e2291 362 Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
krmreynolds 0:dc35364e2291 363 Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
krmreynolds 0:dc35364e2291 364 Update_Matrix[1][1]=0;
krmreynolds 0:dc35364e2291 365 Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
krmreynolds 0:dc35364e2291 366 Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
krmreynolds 0:dc35364e2291 367 Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
krmreynolds 0:dc35364e2291 368 Update_Matrix[2][2]=0;
krmreynolds 0:dc35364e2291 369 #else // Uncorrected data (no drift correction)
krmreynolds 0:dc35364e2291 370 Update_Matrix[0][0]=0;
krmreynolds 0:dc35364e2291 371 Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
krmreynolds 0:dc35364e2291 372 Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
krmreynolds 0:dc35364e2291 373 Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
krmreynolds 0:dc35364e2291 374 Update_Matrix[1][1]=0;
krmreynolds 0:dc35364e2291 375 Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
krmreynolds 0:dc35364e2291 376 Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
krmreynolds 0:dc35364e2291 377 Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
krmreynolds 0:dc35364e2291 378 Update_Matrix[2][2]=0;
krmreynolds 0:dc35364e2291 379 #endif
krmreynolds 0:dc35364e2291 380
krmreynolds 0:dc35364e2291 381 Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
krmreynolds 0:dc35364e2291 382
krmreynolds 0:dc35364e2291 383 for (int x=0; x<3; x++) { //Matrix Addition (update)
krmreynolds 0:dc35364e2291 384 for (int y=0; y<3; y++) {
krmreynolds 0:dc35364e2291 385 DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
krmreynolds 0:dc35364e2291 386 }
krmreynolds 0:dc35364e2291 387 }
krmreynolds 0:dc35364e2291 388 }
krmreynolds 0:dc35364e2291 389
krmreynolds 0:dc35364e2291 390 void minimu9::Euler_angles(void) {
krmreynolds 0:dc35364e2291 391 pitch = -asin(DCM_Matrix[2][0]);
krmreynolds 0:dc35364e2291 392 roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
krmreynolds 0:dc35364e2291 393 yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
krmreynolds 0:dc35364e2291 394 }
krmreynolds 0:dc35364e2291 395
krmreynolds 0:dc35364e2291 396 void minimu9::print_data(void) {
krmreynolds 0:dc35364e2291 397 Serial serial(USBTX, USBRX);
krmreynolds 0:dc35364e2291 398
krmreynolds 0:dc35364e2291 399 serial.printf("!");
krmreynolds 0:dc35364e2291 400
krmreynolds 0:dc35364e2291 401 #if PRINT_EULER == 1
krmreynolds 0:dc35364e2291 402 serial.printf("ANG:");
krmreynolds 0:dc35364e2291 403 serial.printf("%f, ", ToDeg(roll));
krmreynolds 0:dc35364e2291 404 serial.printf("%f, ", ToDeg(pitch));
krmreynolds 0:dc35364e2291 405 serial.printf("%f, ", ToDeg(yaw));
krmreynolds 0:dc35364e2291 406 #endif
krmreynolds 0:dc35364e2291 407 #if PRINT_ANALOGS==1
krmreynolds 0:dc35364e2291 408 serial.printf(",AN:");
krmreynolds 0:dc35364e2291 409 serial.printf("%d, ", AN[0]);
krmreynolds 0:dc35364e2291 410 serial.printf("%d, ", AN[1]);
krmreynolds 0:dc35364e2291 411 serial.printf("%d, ", AN[2]);
krmreynolds 0:dc35364e2291 412 serial.printf("%d, ", AN[3]);
krmreynolds 0:dc35364e2291 413 serial.printf("%d, ", AN[4]);
krmreynolds 0:dc35364e2291 414 serial.printf("%d, ", AN[5]);
krmreynolds 0:dc35364e2291 415 serial.printf("%f, ", c_magnetom_x);
krmreynolds 0:dc35364e2291 416 serial.printf("%f, ", c_magnetom_y);
krmreynolds 0:dc35364e2291 417 serial.printf("%f, ", c_magnetom_z);
krmreynolds 0:dc35364e2291 418 #endif
krmreynolds 0:dc35364e2291 419
krmreynolds 0:dc35364e2291 420 #if PRINT_DCM == 1
krmreynolds 0:dc35364e2291 421 serial.printf (",DCM:");
krmreynolds 0:dc35364e2291 422 serial.printf("%d, ", convert_to_dec(DCM_Matrix[0][0]));
krmreynolds 0:dc35364e2291 423 serial.printf("%d, ", convert_to_dec(DCM_Matrix[0][1]));
krmreynolds 0:dc35364e2291 424 serial.printf("%d, ", convert_to_dec(DCM_Matrix[0][2]));
krmreynolds 0:dc35364e2291 425 serial.printf("%d, ", convert_to_dec(DCM_Matrix[1][0]));
krmreynolds 0:dc35364e2291 426 serial.printf("%d, ", convert_to_dec(DCM_Matrix[1][1]));
krmreynolds 0:dc35364e2291 427 serial.printf("%d, ", convert_to_dec(DCM_Matrix[1][2]));
krmreynolds 0:dc35364e2291 428 serial.printf("%d, ", convert_to_dec(DCM_Matrix[2][0]));
krmreynolds 0:dc35364e2291 429 serial.printf("%d, ", convert_to_dec(DCM_Matrix[2][1]));
krmreynolds 0:dc35364e2291 430 serial.printf("%d, ", convert_to_dec(DCM_Matrix[2][2]));
krmreynolds 0:dc35364e2291 431 #endif
krmreynolds 0:dc35364e2291 432 serial.printf("\n");
krmreynolds 0:dc35364e2291 433 }
krmreynolds 0:dc35364e2291 434
krmreynolds 0:dc35364e2291 435 float minimu9::get_pitch( void ) {
krmreynolds 0:dc35364e2291 436 return ToDeg( pitch );
krmreynolds 0:dc35364e2291 437 }
krmreynolds 0:dc35364e2291 438 float minimu9::get_roll( void ) {
krmreynolds 0:dc35364e2291 439 return ToDeg( roll );
krmreynolds 0:dc35364e2291 440 }
krmreynolds 0:dc35364e2291 441 float minimu9::get_yaw( void ) {
krmreynolds 0:dc35364e2291 442 return ToDeg( yaw );
krmreynolds 0:dc35364e2291 443 }
krmreynolds 0:dc35364e2291 444
krmreynolds 0:dc35364e2291 445 int min ( int a, int b ) {
krmreynolds 0:dc35364e2291 446 if ( b < a )
krmreynolds 0:dc35364e2291 447 return b;
krmreynolds 0:dc35364e2291 448 return a;
krmreynolds 0:dc35364e2291 449 }
krmreynolds 0:dc35364e2291 450 int max ( int a, int b ) {
krmreynolds 0:dc35364e2291 451 if ( b > a )
krmreynolds 0:dc35364e2291 452 return b;
krmreynolds 0:dc35364e2291 453 return a;
krmreynolds 0:dc35364e2291 454 }
krmreynolds 0:dc35364e2291 455
krmreynolds 0:dc35364e2291 456 void minimu9::Calibrate_compass( void ) {
krmreynolds 0:dc35364e2291 457 Serial serial(USBTX, USBRX);
krmreynolds 0:dc35364e2291 458 int running_min[3] = {2047, 2047, 2047}, running_max[3] = {-2048, -2048, -2048};
krmreynolds 0:dc35364e2291 459 while ( 1 ) {
krmreynolds 0:dc35364e2291 460 std::vector<short> m = compass->magRead();
krmreynolds 0:dc35364e2291 461
krmreynolds 0:dc35364e2291 462 for ( int i = 0; i < 3; i++ ) {
krmreynolds 0:dc35364e2291 463 running_min[i] = min(running_min[i], m[i]);
krmreynolds 0:dc35364e2291 464 running_max[i] = max(running_max[i], m[i]);
krmreynolds 0:dc35364e2291 465 }
krmreynolds 0:dc35364e2291 466
krmreynolds 0:dc35364e2291 467 serial.printf("M min ");
krmreynolds 0:dc35364e2291 468 serial.printf("X: %d", (int)running_min[0] );
krmreynolds 0:dc35364e2291 469 serial.printf(" Y: %d", (int)running_min[1]);
krmreynolds 0:dc35364e2291 470 serial.printf(" Z: %d", (int)running_min[2]);
krmreynolds 0:dc35364e2291 471
krmreynolds 0:dc35364e2291 472 serial.printf(" M max ");
krmreynolds 0:dc35364e2291 473 serial.printf("X: %d", (int)running_max[0] );
krmreynolds 0:dc35364e2291 474 serial.printf(" Y: %d", (int)running_max[1]);
krmreynolds 0:dc35364e2291 475 serial.printf(" Z: %d",(int)running_max[2]);
krmreynolds 0:dc35364e2291 476 serial.printf("\n");
krmreynolds 0:dc35364e2291 477
krmreynolds 0:dc35364e2291 478 }
krmreynolds 0:dc35364e2291 479 }
krmreynolds 0:dc35364e2291 480
krmreynolds 0:dc35364e2291 481 void minimu9::set_calibration_values( int x_min, int y_min, int z_min, int x_max, int y_max, int z_max ) {
krmreynolds 0:dc35364e2291 482 M_X_MIN = x_min;
krmreynolds 0:dc35364e2291 483 M_Y_MIN = y_min;
krmreynolds 0:dc35364e2291 484 M_Z_MIN = z_min;
krmreynolds 0:dc35364e2291 485 M_X_MAX = x_max;
krmreynolds 0:dc35364e2291 486 M_Y_MAX = y_max;
krmreynolds 0:dc35364e2291 487 M_Z_MAX = z_max;
krmreynolds 0:dc35364e2291 488 }
krmreynolds 0:dc35364e2291 489
krmreynolds 0:dc35364e2291 490 void minimu9::set_print_settings( int mode, int analogs, int euler, int dcm, int serial ) {
krmreynolds 0:dc35364e2291 491 /*For debugging purposes*/
krmreynolds 0:dc35364e2291 492 //OUTPUTMODE=1 will print the corrected data,
krmreynolds 0:dc35364e2291 493 //OUTPUTMODE=0 will print uncorrected data of the gyros (with drift)
krmreynolds 0:dc35364e2291 494 OUTPUTMODE = mode;
krmreynolds 0:dc35364e2291 495
krmreynolds 0:dc35364e2291 496 //#define PRINT_DCM 0 //Will print the whole direction cosine matrix
krmreynolds 0:dc35364e2291 497 PRINT_ANALOGS = analogs; //Will print the analog raw data
krmreynolds 0:dc35364e2291 498 PRINT_EULER = euler; //Will print the Euler angles Roll, Pitch and Yaw
krmreynolds 0:dc35364e2291 499 PRINT_DCM = dcm;
krmreynolds 0:dc35364e2291 500
krmreynolds 0:dc35364e2291 501 PRINT_SERIAL = serial;
krmreynolds 0:dc35364e2291 502 //PRINT_SERIAL = 1;
krmreynolds 0:dc35364e2291 503 }