Subversion Repositories FlightCtrl

Rev

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

Rev Author Line No. Line
2189 - 1
/*
2
    APM_AHRS_DCM.cpp
3
 
4
    AHRS system using DCM matrices
5
 
6
    Based on DCM code by Doug Weibel, Jordi Mu�oz and Jose Julio. DIYDrones.com
7
 
8
    Adapted for the general ArduPilot AHRS interface by Andrew Tridgell
9
 
10
    This library is free software; you can redistribute it and/or
11
    modify it under the terms of the GNU Lesser General Public License
12
    as published by the Free Software Foundation; either version 2.1
13
    of the License, or (at your option) any later version.
14
*/
15
#include "AP_AHRS.h"
16
#include "output.h"
17
#include "profiler.h"
18
#include "analog.h"
19
#include "debug.h"
20
#include <stdio.h>
21
 
22
// this is the speed in cm/s above which we first get a yaw lock with
23
// the GPS
24
#define GPS_SPEED_MIN 300
25
 
26
// this is the speed in cm/s at which we stop using drift correction
27
// from the GPS and wait for the ground speed to get above GPS_SPEED_MIN
28
#define GPS_SPEED_RESET 100
29
 
30
// run a full DCM update round
31
void
32
AP_AHRS_DCM::update(int16_t attitude[3], float delta_t)
33
{
34
    // Get current values for gyros
35
    _gyro_vector  = gyro_attitude;
36
    _accel_vector = accel;
37
 
38
    // Integrate the DCM matrix using gyro inputs
39
    matrix_update(delta_t);
40
 
41
    // Normalize the DCM matrix
42
    normalize();
43
 
44
    // Perform drift correction
45
    //setCurrentProfiledActivity(DRIFT_CORRECTION);
46
    drift_correction(delta_t);
47
 
48
    // paranoid check for bad values in the DCM matrix
49
    //setCurrentProfiledActivity(CHECK_MATRIX);
50
    check_matrix();
51
 
52
    // Calculate pitch, roll, yaw for stabilization and navigation
53
    //setCurrentProfiledActivity(EULER_ANGLES);
54
    euler_angles();
55
 
56
    //setCurrentProfiledActivity(ANGLESOUTPUT);
57
    attitude[0] = roll * INT16DEG_PI_FACTOR;
58
    attitude[1] = pitch* INT16DEG_PI_FACTOR;
59
    attitude[2] = yaw  * INT16DEG_PI_FACTOR;
60
 
61
    // Just for info:
62
    int16_t sensor = degrees(roll) * 10;
63
    debugOut.analog[0] = sensor;
64
 
65
    sensor = degrees(pitch) * 10;
66
    debugOut.analog[1] = sensor;
67
 
68
    sensor = degrees(yaw) * 10;
69
    if (sensor < 0)
70
        sensor += 3600;
71
    debugOut.analog[2] = sensor;
72
}
73
 
74
// update the DCM matrix using only the gyros
75
void
76
AP_AHRS_DCM::matrix_update(float _G_Dt)
77
{
78
    //setCurrentProfiledActivity(MATRIX_UPDATE1);
79
 
80
    // _omega_integ_corr is used for _centripetal correction
81
    // (theoretically better than _omega)
82
    _omega_integ_corr = _gyro_vector + _omega_I;
83
 
84
    // Equation 16, adding proportional and integral correction terms
85
    _omega = _omega_integ_corr + _omega_P + _omega_yaw_P;
86
 
87
    // this is a replacement of the DCM matrix multiply (equation
88
    // 17), with known zero elements removed and the matrix
89
    // operations inlined. This runs much faster than the original
90
    // version of this code, as the compiler was doing a terrible
91
    // job of realising that so many of the factors were in common
92
    // or zero. It also uses much less stack, as we no longer need
93
    // two additional local matrices
94
 
95
    Vector3f r = _omega * _G_Dt;
96
 
97
    //setCurrentProfiledActivity(MATRIX_UPDATE2);
98
 
99
    _dcm_matrix.rotate(r);
100
}
101
 
102
 
103
// adjust an accelerometer vector for known acceleration forces
104
void
105
AP_AHRS_DCM::accel_adjust(Vector3f &accel)
106
{
107
    float veloc;
108
    // compensate for linear acceleration. This makes a
109
    // surprisingly large difference in the pitch estimate when
110
    // turning, plus on takeoff and landing
111
    //float acceleration = _gps->acceleration();
112
    //accel.x -= acceleration;
113
 
114
    // compensate for centripetal acceleration
115
    veloc = 0; //_gps->ground_speed * 0.01;
116
 
117
    // We are working with a modified version of equation 26 as
118
    // our IMU object reports acceleration in the positive axis
119
    // direction as positive
120
 
121
    // Equation 26 broken up into separate pieces
122
    accel.y -= _omega_integ_corr.z * veloc;
123
    accel.z += _omega_integ_corr.y * veloc;
124
}
125
 
126
/*
127
  reset the DCM matrix and omega. Used on ground start, and on
128
  extreme errors in the matrix
129
 */
130
void
131
AP_AHRS_DCM::reset(bool recover_eulers)
132
{
133
    if (_compass != NULL) {
134
        _compass->null_offsets_disable();
135
    }
136
 
137
    // reset the integration terms
138
    _omega_I.zero();
139
    _omega_P.zero();
140
    _omega_yaw_P.zero();
141
    _omega_integ_corr.zero();
142
    _omega.zero();
143
 
144
    // if the caller wants us to try to recover to the current
145
    // attitude then calculate the dcm matrix from the current
146
    // roll/pitch/yaw values
147
    if (recover_eulers && !isnan(roll) && !isnan(pitch) && !isnan(yaw)) {
148
        _dcm_matrix.from_euler(roll, pitch, yaw);
149
    } else {
150
        // otherwise make it flat
151
        _dcm_matrix.from_euler(0, 0, 0);
152
    }
153
 
154
    if (_compass != NULL) {
155
        _compass->null_offsets_enable();    // This call is needed to restart the nulling
156
        // Otherwise the reset in the DCM matrix can mess up
157
        // the nulling
158
    }
159
}
160
 
161
/*
162
  check the DCM matrix for pathological values
163
 */
164
void
165
AP_AHRS_DCM::check_matrix(void)
166
{
167
    if (_dcm_matrix.is_nan()) {
168
        //Serial.printf("ERROR: DCM matrix NAN\n");
169
        printf("ERROR: DCM matrix NAN\n");
170
        renorm_blowup_count++;
171
        reset(true);
172
        return;
173
    }
174
    // some DCM matrix values can lead to an out of range error in
175
    // the pitch calculation via asin().  These NaN values can
176
    // feed back into the rest of the DCM matrix via the
177
    // error_course value.
178
    if (!(_dcm_matrix.c.x < 1.0 &&
179
          _dcm_matrix.c.x > -1.0)) {
180
        // We have an invalid matrix. Force a normalisation.
181
        renorm_range_count++;
182
        normalize();
183
 
184
        if (_dcm_matrix.is_nan() ||
185
            fabs(_dcm_matrix.c.x) > 10) {
186
            // normalisation didn't fix the problem! We're
187
            // in real trouble. All we can do is reset
188
            //Serial.printf("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n",
189
            //     _dcm_matrix.c.x);
190
            printf("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n", (double)_dcm_matrix.c.x);
191
            renorm_blowup_count++;
192
            reset(true);
193
        }
194
    }
195
}
196
 
197
// renormalise one vector component of the DCM matrix
198
// this will return false if renormalization fails
199
bool
200
AP_AHRS_DCM::renorm(Vector3f const &a, Vector3f &result)
201
{
202
    float   renorm_val;
203
 
204
    // numerical errors will slowly build up over time in DCM,
205
    // causing inaccuracies. We can keep ahead of those errors
206
    // using the renormalization technique from the DCM IMU paper
207
    // (see equations 18 to 21).
208
 
209
    // For APM we don't bother with the taylor expansion
210
    // optimisation from the paper as on our 2560 CPU the cost of
211
    // the sqrt() is 44 microseconds, and the small time saving of
212
    // the taylor expansion is not worth the potential of
213
    // additional error buildup.
214
 
215
    // Note that we can get significant renormalisation values
216
    // when we have a larger delta_t due to a glitch eleswhere in
217
    // APM, such as a I2c timeout or a set of EEPROM writes. While
218
    // we would like to avoid these if possible, if it does happen
219
    // we don't want to compound the error by making DCM less
220
    // accurate.
221
 
222
    renorm_val = 1.0 / a.length();
223
 
224
    // keep the average for reporting
225
    _renorm_val_sum += renorm_val;
226
    _renorm_val_count++;
227
 
228
    if (!(renorm_val < 2.0 && renorm_val > 0.5)) {
229
        // this is larger than it should get - log it as a warning
230
        renorm_range_count++;
231
        if (!(renorm_val < 1.0e6 && renorm_val > 1.0e-6)) {
232
            // we are getting values which are way out of
233
            // range, we will reset the matrix and hope we
234
            // can recover our attitude using drift
235
            // correction before we hit the ground!
236
            //Serial.printf("ERROR: DCM renormalisation error. renorm_val=%f\n",
237
            //     renorm_val);
238
            printf("ERROR: DCM renormalisation error. renorm_val=%f\n", (double)renorm_val);
239
            renorm_blowup_count++;
240
            return false;
241
        }
242
    }
243
 
244
    result = a * renorm_val;
245
    return true;
246
}
247
 
248
/*************************************************
249
Direction Cosine Matrix IMU: Theory
250
William Premerlani and Paul Bizard
251
 
252
Numerical errors will gradually reduce the orthogonality conditions expressed by equation 5
253
to approximations rather than identities. In effect, the axes in the two frames of reference no
254
longer describe a rigid body. Fortunately, numerical error accumulates very slowly, so it is a
255
simple matter to stay ahead of it.
256
We call the process of enforcing the orthogonality conditions �renormalization�.
257
*/
258
void
259
AP_AHRS_DCM::normalize(void)
260
{
261
    float error;
262
    Vector3f t0, t1, t2;
263
 
264
    //setCurrentProfiledActivity(MATRIX_NORMALIZE1);
265
 
266
    error = _dcm_matrix.a * _dcm_matrix.b;                      // eq.18
267
    t0 = _dcm_matrix.a - (_dcm_matrix.b * (0.5f * error));      // eq.19
268
    t1 = _dcm_matrix.b - (_dcm_matrix.a * (0.5f * error));      // eq.19
269
    t2 = t0 % t1;
270
 
271
    //setCurrentProfiledActivity(MATRIX_NORMALIZE2);
272
    if (!renorm(t0, _dcm_matrix.a) ||
273
        !renorm(t1, _dcm_matrix.b) ||
274
        !renorm(t2, _dcm_matrix.c)) {
275
        // Our solution is blowing up and we will force back
276
        // to last euler angles
277
        reset(true);
278
    }
279
}
280
 
281
 
282
// perform drift correction. This function aims to update _omega_P and
283
// _omega_I with our best estimate of the short term and long term
284
// gyro error. The _omega_P value is what pulls our attitude solution
285
// back towards the reference vector quickly. The _omega_I term is an
286
// attempt to learn the long term drift rate of the gyros.
287
//
288
// This function also updates _omega_yaw_P with a yaw correction term
289
// from our yaw reference vector
290
void
291
AP_AHRS_DCM::drift_correction(float deltat)
292
{
293
    float error_course = 0;
294
    Vector3f accel;
295
    Vector3f error;
296
    float error_norm = 0;
297
    float yaw_deltat = 0;
298
 
299
    accel = _accel_vector;
300
 
301
    // if enabled, use the GPS to correct our accelerometer vector
302
    // for centripetal forces
303
    if(_centripetal &&
304
       _gps != NULL &&
305
       _gps->status() == GPS::GPS_OK) {
306
        accel_adjust(accel);
307
    }
308
 
309
 
310
    //*****Roll and Pitch***************
311
 
312
    // normalise the accelerometer vector to a standard length
313
    // this is important to reduce the impact of noise on the
314
    // drift correction, as very noisy vectors tend to have
315
    // abnormally high lengths. By normalising the length we
316
    // reduce their impact.
317
    float accel_length = accel.length();
318
 
319
    accel *= (_gravity / accel_length);
320
    if (accel.is_inf()) {
321
        // we can't do anything useful with this sample
322
        _omega_P.zero();
323
        return;
324
    }
325
 
326
    // calculate the error, in m/s^2, between the attitude
327
    // implied by the accelerometers and the attitude
328
    // in the current DCM matrix
329
    error =  _dcm_matrix.c % accel;
330
 
331
    // Limit max error to limit the effect of noisy values
332
    // on the algorithm. This limits the error to about 11
333
    // degrees
334
    error_norm = error.length();
335
 
336
    if (error_norm > 2) {
337
        error *= (2 / error_norm);
338
    }
339
 
340
    // we now want to calculate _omega_P and _omega_I. The
341
    // _omega_P value is what drags us quickly to the
342
    // accelerometer reading.
343
    _omega_P = error * _kp_roll_pitch;
344
 
345
    // the _omega_I is the long term accumulated gyro
346
    // error. This determines how much gyro drift we can
347
    // handle.
348
    _omega_I_sum += error * (_ki_roll_pitch * deltat);
349
    _omega_I_sum_time += deltat;
350
 
351
    // these sums support the reporting of the DCM state via MAVLink
352
    _error_rp_sum += error_norm;
353
    _error_rp_count++;
354
 
355
    // yaw drift correction
356
 
357
    // we only do yaw drift correction when we get a new yaw
358
    // reference vector. In between times we rely on the gyros for
359
    // yaw. Avoiding this calculation on every call to
360
    // update_DCM() saves a lot of time
361
    if (_compass && _compass->use_for_yaw()) {
362
        if (_compass->last_update != _compass_last_update) {
363
            yaw_deltat = 1.0e-6*(_compass->last_update - _compass_last_update);
364
            if (_have_initial_yaw && yaw_deltat < 2.0) {
365
                // Equation 23, Calculating YAW error
366
                // We make the gyro YAW drift correction based
367
                // on compass magnetic heading
368
                error_course = (_dcm_matrix.a.x * _compass->heading_y) - (_dcm_matrix.b.x * _compass->heading_x);
369
                _compass_last_update = _compass->last_update;
370
            } else {
371
                // this is our first estimate of the yaw,
372
                // or the compass has come back online after
373
                // no readings for 2 seconds.
374
                //
375
                // construct a DCM matrix based on the current
376
                // roll/pitch and the compass heading.
377
                // First ensure the compass heading has been
378
                // calculated
379
                _compass->calculate(_dcm_matrix);
380
 
381
                // now construct a new DCM matrix
382
                _compass->null_offsets_disable();
383
                _dcm_matrix.from_euler(roll, pitch, _compass->heading);
384
                _compass->null_offsets_enable();
385
                _have_initial_yaw = true;
386
                _compass_last_update = _compass->last_update;
387
                error_course = 0;
388
            }
389
        }
390
    } else if (_gps && _gps->status() == GPS::GPS_OK) {
391
        if (_gps->last_fix_time != _gps_last_update) {
392
            // Use GPS Ground course to correct yaw gyro drift
393
            if (_gps->ground_speed >= GPS_SPEED_MIN) {
394
                yaw_deltat = 1.0e-3*(_gps->last_fix_time - _gps_last_update);
395
                if (_have_initial_yaw && yaw_deltat < 2.0) {
396
                    float course_over_ground_x = cos(ToRad(_gps->ground_course/100.0));
397
                    float course_over_ground_y = sin(ToRad(_gps->ground_course/100.0));
398
                    // Equation 23, Calculating YAW error
399
                    error_course = (_dcm_matrix.a.x * course_over_ground_y) - (_dcm_matrix.b.x * course_over_ground_x);
400
                    _gps_last_update = _gps->last_fix_time;
401
                } else  {
402
                    // when we first start moving, set the
403
                    // DCM matrix to the current
404
                    // roll/pitch values, but with yaw
405
                    // from the GPS
406
                    if (_compass) {
407
                        _compass->null_offsets_disable();
408
                    }
409
                    _dcm_matrix.from_euler(roll, pitch, ToRad(_gps->ground_course));
410
                    if (_compass) {
411
                        _compass->null_offsets_enable();
412
                    }
413
                    _have_initial_yaw =  true;
414
                    error_course = 0;
415
                    _gps_last_update = _gps->last_fix_time;
416
                }
417
            } else if (_gps->ground_speed >= GPS_SPEED_RESET) {
418
                // we are not going fast enough to use GPS for
419
                // course correction, but we won't reset
420
                // _have_initial_yaw yet, instead we just let
421
                // the gyro handle yaw
422
                error_course = 0;
423
            } else {
424
                // we are moving very slowly. Reset
425
                // _have_initial_yaw and adjust our heading
426
                // rapidly next time we get a good GPS ground
427
                // speed
428
                error_course = 0;
429
                _have_initial_yaw = false;
430
            }
431
        }
432
    }
433
 
434
    // see if there is any error in our heading relative to the
435
    // yaw reference. This will be zero most of the time, as we
436
    // only calculate it when we get new data from the yaw
437
    // reference source
438
    if (yaw_deltat == 0 || error_course == 0) {
439
        // we don't have a new reference heading. Slowly
440
        // decay the _omega_yaw_P to ensure that if we have
441
        // lost the yaw reference sensor completely we don't
442
        // keep using a stale offset
443
        _omega_yaw_P *= 0.97;
444
        goto check_sum_time;
445
    }
446
 
447
    // ensure the course error is scaled from -PI to PI
448
    if (error_course > PI) {
449
        error_course -= 2*PI;
450
    } else if (error_course < -PI) {
451
        error_course += 2*PI;
452
    }
453
 
454
    // Equation 24, Applys the yaw correction to the XYZ rotation of the aircraft
455
    // this gives us an error in radians
456
    error = _dcm_matrix.c * error_course;
457
 
458
    // Adding yaw correction to proportional correction vector. We
459
    // allow the yaw reference source to affect all 3 components
460
    // of _omega_yaw_P as we need to be able to correctly hold a
461
    // heading when roll and pitch are non-zero
462
    _omega_yaw_P = error * _kp_yaw;
463
 
464
    // add yaw correction to integrator correction vector, but
465
    // only for the z gyro. We rely on the accelerometers for x
466
    // and y gyro drift correction. Using the compass or GPS for
467
    // x/y drift correction is too inaccurate, and can lead to
468
    // incorrect builups in the x/y drift. We rely on the
469
    // accelerometers to get the x/y components right
470
    _omega_I_sum.z += error.z * (_ki_yaw * yaw_deltat);
471
 
472
    // we keep the sum of yaw error for reporting via MAVLink.
473
    _error_yaw_sum += error_course;
474
    _error_yaw_count++;
475
 
476
check_sum_time:
477
    if (_omega_I_sum_time > 10) {
478
        // every 10 seconds we apply the accumulated
479
        // _omega_I_sum changes to _omega_I. We do this to
480
        // prevent short term feedback between the P and I
481
        // terms of the controller. The _omega_I vector is
482
        // supposed to refect long term gyro drift, but with
483
        // high noise it can start to build up due to short
484
        // term interactions. By summing it over 10 seconds we
485
        // prevent the short term interactions and can apply
486
        // the slope limit more accurately
487
        float drift_limit = _gyro_drift_limit * _omega_I_sum_time;
488
        _omega_I_sum.x = constrain(_omega_I_sum.x, -drift_limit, drift_limit);
489
        _omega_I_sum.y = constrain(_omega_I_sum.y, -drift_limit, drift_limit);
490
        _omega_I_sum.z = constrain(_omega_I_sum.z, -drift_limit, drift_limit);
491
 
492
        _omega_I += _omega_I_sum;
493
 
494
        // zero the sum
495
        _omega_I_sum.zero();
496
        _omega_I_sum_time = 0;
497
    }
498
}
499
 
500
 
501
// calculate the euler angles which will be used for high level
502
// navigation control
503
void
504
AP_AHRS_DCM::euler_angles(void)
505
{
506
    _dcm_matrix.to_euler(&roll, &pitch, &yaw);
507
}
508
 
509
/* reporting of DCM state for MAVLink */
510
 
511
// average error_roll_pitch since last call
512
float AP_AHRS_DCM::get_error_rp(void)
513
{
514
    if (_error_rp_count == 0) {
515
        // this happens when telemetry is setup on two
516
        // serial ports
517
        return _error_rp_last;
518
    }
519
    _error_rp_last = _error_rp_sum / _error_rp_count;
520
    _error_rp_sum = 0;
521
    _error_rp_count = 0;
522
    return _error_rp_last;
523
}
524
 
525
// average error_yaw since last call
526
float AP_AHRS_DCM::get_error_yaw(void)
527
{
528
    if (_error_yaw_count == 0) {
529
        // this happens when telemetry is setup on two
530
        // serial ports
531
        return _error_yaw_last;
532
    }
533
    _error_yaw_last = _error_yaw_sum / _error_yaw_count;
534
    _error_yaw_sum = 0;
535
    _error_yaw_count = 0;
536
    return _error_yaw_last;
537
}