Subversion Repositories FlightCtrl

Rev

Go to most recent revision | Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1993 - 1
/*
2
FreeIMU.cpp - A libre and easy to use orientation sensing library for Arduino
3
Copyright (C) 2011 Fabio Varesano <fabio at varesano dot net>
4
 
5
 
6
This program is free software: you can redistribute it and/or modify
7
it under the terms of the version 3 GNU General Public License as
8
published by the Free Software Foundation.
9
 
10
This program is distributed in the hope that it will be useful,
11
but WITHOUT ANY WARRANTY; without even the implied warranty of
12
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13
GNU General Public License for more details.
14
 
15
You should have received a copy of the GNU General Public License
16
along with this program.  If not, see <http://www.gnu.org/licenses/>.
17
 
18
*/
19
 
20
#include <inttypes.h>
21
//#define DEBUG
22
#include "WProgram.h"
23
#include "FreeIMU.h"
24
// #include "WireUtils.h"
25
#include "DebugUtils.h"
26
 
27
//----------------------------------------------------------------------------------------------------
28
// Definitions
29
 
30
#define Kp 2.0f   // proportional gain governs rate of convergence to accelerometer/magnetometer
31
#define Ki 0.005f   // integral gain governs rate of convergence of gyroscope biases
32
//#define halfT 0.02f   // half the sample period
33
 
34
 
35
FreeIMU::FreeIMU() {
36
  #if FREEIMU_VER <= 3
37
    acc = ADXL345();
38
  #else
39
    acc = BMA180();
40
  #endif
41
  gyro = ITG3200();
42
  magn = HMC58X3();
43
 
44
  // initialize quaternion
45
  q0 = 1.0;
46
  q1 = 0.0;
47
  q2 = 0.0;
48
  q3 = 0.0;
49
  exInt = 0.0;
50
  eyInt = 0.0;
51
  ezInt = 0.0;
52
  lastUpdate = 0;
53
  now = 0;
54
}
55
 
56
void FreeIMU::init() {
57
  init(FIMU_ACC_ADDR, FIMU_ITG3200_DEF_ADDR, false);
58
}
59
 
60
void FreeIMU::init(bool fastmode) {
61
  init(FIMU_ACC_ADDR, FIMU_ITG3200_DEF_ADDR, fastmode);
62
}
63
 
64
void FreeIMU::init(int acc_addr, int gyro_addr, bool fastmode) {
65
  delay(5);
66
 
67
  // disable internal pullups of the ATMEGA which Wire enable by default
68
  #if defined(__AVR_ATmega168__) || defined(__AVR_ATmega8__) || defined(__AVR_ATmega328P__)
69
    // deactivate internal pull-ups for twi
70
    // as per note from atmega8 manual pg167
71
    cbi(PORTC, 4);
72
    cbi(PORTC, 5);
73
  #else
74
    // deactivate internal pull-ups for twi
75
    // as per note from atmega128 manual pg204
76
    cbi(PORTD, 0);
77
    cbi(PORTD, 1);
78
  #endif
79
 
80
  if(fastmode) { // switch to 400KHz I2C - eheheh
81
    TWBR = ((16000000L / 400000L) - 16) / 2; // see twi_init in Wire/utility/twi.c
82
    // TODO: make the above usable also for 8MHz arduinos..
83
  }
84
 
85
  #if FREEIMU_VER <= 3
86
    // init ADXL345
87
    acc.init(acc_addr);
88
  #else
89
    // init BMA180
90
    acc.setAddress(acc_addr);
91
    acc.SoftReset();
92
    acc.enableWrite();
93
    acc.SetFilter(acc.F10HZ);
94
    acc.setGSensitivty(acc.G15);
95
    acc.SetSMPSkip();
96
    acc.SetISRMode();
97
    acc.disableWrite();
98
  #endif
99
 
100
  // init ITG3200
101
  gyro.init(gyro_addr);
102
  // calibrate the ITG3200
103
  gyro.zeroCalibrate(64,5);
104
 
105
  // init HMC5843
106
  magn.init(false); // Don't set mode yet, we'll do that later on.
107
  // Calibrate HMC using self test, not recommended to change the gain after calibration.
108
  magn.calibrate(1); // Use gain 1=default, valid 0-7, 7 not recommended.
109
  // Single mode conversion was used in calibration, now set continuous mode
110
  magn.setMode(0);
111
  delay(10);
112
  magn.setDOR(B110);
113
}
114
 
115
 
116
void FreeIMU::getRawValues(int * raw_values) {
117
  acc.readAccel(&raw_values[0], &raw_values[1], &raw_values[2]);
118
  gyro.readGyroRaw(&raw_values[3], &raw_values[4], &raw_values[5]);
119
  magn.getValues(&raw_values[6], &raw_values[7], &raw_values[8]);
120
}
121
 
122
 
123
void FreeIMU::getValues(float * values) {  
124
  int accval[3];
125
  acc.readAccel(&accval[0], &accval[1], &accval[2]);
126
  values[0] = ((float) accval[0]);
127
  values[1] = ((float) accval[1]);
128
  values[2] = ((float) accval[2]);
129
 
130
  gyro.readGyro(&values[3]);
131
 
132
  magn.getValues(&values[6]);
133
}
134
 
135
 
136
 
137
//=====================================================================================================
138
// AHRS.c
139
// S.O.H. Madgwick
140
// 25th August 2010
141
//=====================================================================================================
142
// Description:
143
//
144
// Quaternion implementation of the 'DCM filter' [Mayhony et al].  Incorporates the magnetic distortion
145
// compensation algorithms from my filter [Madgwick] which eliminates the need for a reference
146
// direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw
147
// axis only.
148
//
149
// User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'.
150
//
151
// Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated
152
// orientation.  See my report for an overview of the use of quaternions in this application.
153
//
154
// User must call 'AHRSupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz'),
155
// accelerometer ('ax', 'ay', 'ay') and magnetometer ('mx', 'my', 'mz') data.  Gyroscope units are
156
// radians/second, accelerometer and magnetometer units are irrelevant as the vector is normalised.
157
//
158
//=====================================================================================================
159
void FreeIMU::AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
160
  float norm;
161
  float hx, hy, hz, bx, bz;
162
  float vx, vy, vz, wx, wy, wz;
163
  float ex, ey, ez;
164
 
165
  // auxiliary variables to reduce number of repeated operations
166
  float q0q0 = q0*q0;
167
  float q0q1 = q0*q1;
168
  float q0q2 = q0*q2;
169
  float q0q3 = q0*q3;
170
  float q1q1 = q1*q1;
171
  float q1q2 = q1*q2;
172
  float q1q3 = q1*q3;
173
  float q2q2 = q2*q2;  
174
  float q2q3 = q2*q3;
175
  float q3q3 = q3*q3;          
176
 
177
  // normalise the measurements
178
 
179
  now = millis();
180
  halfT = (now - lastUpdate) / 2000.0;
181
  lastUpdate = now;
182
 
183
  norm = sqrt(ax*ax + ay*ay + az*az);      
184
  ax = ax / norm;
185
  ay = ay / norm;
186
  az = az / norm;
187
 
188
  /*
189
  norm = invSqrt(ax*ax + ay*ay + az*az);
190
  ax = ax * norm;
191
  ay = ay * norm;
192
  az = az * norm;
193
  */
194
 
195
  norm = sqrt(mx*mx + my*my + mz*mz);          
196
  mx = mx / norm;
197
  my = my / norm;
198
  mz = mz / norm;
199
 
200
  /*
201
  norm = invSqrt(mx*mx + my*my + mz*mz);
202
  mx = mx * norm;
203
  my = my * norm;
204
  mz = mz * norm;
205
  */
206
 
207
  // compute reference direction of flux
208
  hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
209
  hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
210
  hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);        
211
  bx = sqrt((hx*hx) + (hy*hy));
212
  bz = hz;    
213
 
214
  // estimated direction of gravity and flux (v and w)
215
  vx = 2*(q1q3 - q0q2);
216
  vy = 2*(q0q1 + q2q3);
217
  vz = q0q0 - q1q1 - q2q2 + q3q3;
218
  wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
219
  wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
220
  wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);  
221
 
222
  // error is sum of cross product between reference direction of fields and direction measured by sensors
223
  ex = (ay*vz - az*vy) + (my*wz - mz*wy);
224
  ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
225
  ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
226
 
227
  // integral error scaled integral gain
228
  exInt = exInt + ex*Ki;
229
  eyInt = eyInt + ey*Ki;
230
  ezInt = ezInt + ez*Ki;
231
 
232
  // adjusted gyroscope measurements
233
  gx = gx + Kp*ex + exInt;
234
  gy = gy + Kp*ey + eyInt;
235
  gz = gz + Kp*ez + ezInt;
236
 
237
  // integrate quaternion rate and normalise
238
  iq0 = (-q1*gx - q2*gy - q3*gz)*halfT;
239
  iq1 = (q0*gx + q2*gz - q3*gy)*halfT;
240
  iq2 = (q0*gy - q1*gz + q3*gx)*halfT;
241
  iq3 = (q0*gz + q1*gy - q2*gx)*halfT;  
242
 
243
  q0 += iq0;
244
  q1 += iq1;
245
  q2 += iq2;
246
  q3 += iq3;
247
 
248
  // normalise quaternion
249
 
250
  norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
251
  q0 = q0 / norm;
252
  q1 = q1 / norm;
253
  q2 = q2 / norm;
254
  q3 = q3 / norm;
255
 
256
/*
257
  norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
258
  q0 = q0 * norm;
259
  q1 = q1 * norm;
260
  q2 = q2 * norm;
261
  q3 = q3 * norm;
262
  */
263
}
264
 
265
void FreeIMU::getQ(float * q) {
266
  float val[9];
267
  getValues(val);
268
 
269
  DEBUG_PRINT(val[3] * M_PI/180);
270
  DEBUG_PRINT(val[4] * M_PI/180);
271
  DEBUG_PRINT(val[5] * M_PI/180);
272
  DEBUG_PRINT(val[0]);
273
  DEBUG_PRINT(val[1]);
274
  DEBUG_PRINT(val[2]);
275
  DEBUG_PRINT(val[6]);
276
  DEBUG_PRINT(val[7]);
277
  DEBUG_PRINT(val[8]);
278
 
279
  // gyro values are expressed in deg/sec, the * M_PI/180 will convert it to radians/sec
280
  AHRSupdate(val[3] * M_PI/180, val[4] * M_PI/180, val[5] * M_PI/180, val[0], val[1], val[2], val[6], val[7], val[8]);
281
  q[0] = q0;
282
  q[1] = q1;
283
  q[2] = q2;
284
  q[3] = q3;
285
}
286
 
287
// Returns the Euler angles in radians defined with the Aerospace sequence.
288
// See Sebastian O.H. Madwick report 
289
// "An efficient orientation filter for inertial and intertial/magnetic sensor arrays" Chapter 2 Quaternion representation
290
void FreeIMU::getEuler(float * angles) {
291
  float q[4]; // quaternion
292
  getQ(q);
293
  angles[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1) * 180/M_PI; // psi
294
  angles[1] = -asin(2 * q[1] * q[3] + 2 * q[0] * q[2]) * 180/M_PI; // theta
295
  angles[2] = atan2(2 * q[2] * q[3] - 2 * q[0] * q[1], 2 * q[0] * q[0] + 2 * q[3] * q[3] - 1) * 180/M_PI; // phi
296
}
297
 
298
 
299
 
300
void FreeIMU::getYawPitchRoll(float * ypr) {
301
  float q[4]; // quaternion
302
  float gx, gy, gz; // estimated gravity direction
303
  getQ(q);
304
 
305
  gx = 2 * (q[1]*q[3] - q[0]*q[2]);
306
  gy = 2 * (q[0]*q[1] + q[2]*q[3]);
307
  gz = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
308
 
309
  ypr[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1) * 180/M_PI;
310
  ypr[1] = atan(gx / sqrt(gy*gy + gz*gz))  * 180/M_PI;
311
  ypr[2] = atan(gy / sqrt(gx*gx + gz*gz))  * 180/M_PI;
312
}
313
 
314
 
315
float invSqrt(float number) {
316
  volatile long i;
317
  volatile float x, y;
318
  volatile const float f = 1.5F;
319
 
320
  x = number * 0.5F;
321
  y = number;
322
  i = * ( long * ) &y;
323
  i = 0x5f375a86 - ( i >> 1 );
324
  y = * ( float * ) &i;
325
  y = y * ( f - ( x * y * y ) );
326
  return y;
327
}