Subversion Repositories FlightCtrl

Rev

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

Rev Author Line No. Line
2189 - 1
#include <stdlib.h>
2
#include <avr/io.h>
3
#include <stdio.h>
4
 
5
#include "attitude.h"
6
#include "commands.h"
7
#include "vector3d.h"
8
// For scope debugging only!
9
#include "rc.h"
10
 
11
// where our main data flow comes from.
12
#include "analog.h"
13
 
14
#include "configuration.h"
15
#include "output.h"
16
 
17
// Some calculations are performed depending on some stick related things.
18
#include "controlMixer.h"
19
 
20
#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}
21
 
22
/*
23
 * Gyro integrals. These are the rotation angles of the airframe relative to horizontal.
24
 */
25
float attitude[3];
26
 
27
uint8_t imu_sequence = 0; //incremented on each call to imu_update
28
 
29
float dcmAcc[3][3]; //dcm matrix according to accelerometer
30
float dcmGyro[3][3]; //dcm matrix according to gyroscopes
31
float dcmEst[3][3]; //estimated dcm matrix by fusion of accelerometer and gyro
32
 
33
float getAngleEstimateFromAcc(uint8_t axis) {
34
        return GYRO_ACC_FACTOR * acc_ATT[axis];
35
}
36
 
37
/************************************************************************
38
 * Neutral Readings                                                    
39
 ************************************************************************/
40
void attitude_setNeutral(void) {
41
        // Calibrate hardware.
42
        analog_setNeutral();
43
        unsigned char i, j;
44
        for (i = 0; i < 3; i++)
45
                for (j = 0; j < 3; j++)
46
                        dcmGyro[i][j] = (i == j) ? 1.0 : 0.0;
47
}
48
 
49
/*
50
 How to use this module in other projects.
51
 
52
 Input variables are:
53
 adcAvg[0..6]  ADC readings of 3 axis accelerometer and 3 axis gyroscope (they are calculated in the background by adcutil.h)
54
 interval_us - interval in microseconds since last call to imu_update
55
 
56
 Output variables are:
57
 DcmEst[0..2] which are the direction cosine of the X,Y,Z axis
58
 
59
 First you must initialize the module with:
60
 imu_init();
61
 
62
 Then call periodically every 5-20ms:
63
 imu_update(interval_us);
64
 it is assumed that you also update periodicall the adcAvg[0..5] array
65
 
66
 */
67
 
68
#define ACC_WEIGHT_MAX 0.02                     //maximum accelerometer weight in accelerometer-gyro fusion formula
69
//this value is tuned-up experimentally: if you get too much noise - decrease it
70
//if you get a delayed response of the filtered values - increase it
71
//starting with a value of  0.01 .. 0.05 will work for most sensors
72
 
73
#define ACC_ERR_MAX  0.3                        //maximum allowable error(external acceleration) where accWeight becomes 0
74
 
75
//-------------------------------------------------------------------
76
// Globals
77
//-------------------------------------------------------------------
78
 
79
//bring dcm matrix in order - adjust values to make orthonormal (or at least closer to orthonormal)
80
//Note: dcm and dcmResult can be the same
81
void dcm_orthonormalize(float dcm[3][3]) {
82
        //err = X . Y ,  X = X - err/2 * Y , Y = Y - err/2 * X  (DCMDraft2 Eqn.19)
83
        float err = vector3d_dot((float*) (dcm[0]), (float*) (dcm[1]));
84
        float delta[2][3];
85
        vector3d_scale(-err / 2, (float*) (dcm[1]), (float*) (delta[0]));
86
        vector3d_scale(-err / 2, (float*) (dcm[0]), (float*) (delta[1]));
87
        vector3d_add((float*) (dcm[0]), (float*) (delta[0]), (float*) (dcm[0]));
88
        vector3d_add((float*) (dcm[1]), (float*) (delta[0]), (float*) (dcm[1]));
89
 
90
        //Z = X x Y  (DCMDraft2 Eqn. 20) ,
91
        vector3d_cross((float*) (dcm[0]), (float*) (dcm[1]), (float*) (dcm[2]));
92
        //re-nomralization
93
        vector3d_normalize((float*) (dcm[0]));
94
        vector3d_normalize((float*) (dcm[1]));
95
        vector3d_normalize((float*) (dcm[2]));
96
}
97
 
98
//rotate DCM matrix by a small rotation given by angular rotation vector w
99
//see http://gentlenav.googlecode.com/files/DCMDraft2.pdf
100
void dcm_rotate(float dcm[3][3], float w[3]) {
101
        //float W[3][3];
102
        //creates equivalent skew symetric matrix plus identity matrix
103
        //vector3d_skew_plus_identity((float*)w,(float*)W);
104
        //float dcmTmp[3][3];
105
        //matrix_multiply(3,3,3,(float*)W,(float*)dcm,(float*)dcmTmp);
106
 
107
        int i;
108
        float dR[3];
109
        //update matrix using formula R(t+1)= R(t) + dR(t) = R(t) + w x R(t)
110
        for (i = 0; i < 3; i++) {
111
                vector3d_cross(w, dcm[i], dR);
112
                vector3d_add(dcm[i], dR, dcm[i]);
113
        }
114
 
115
        //make matrix orthonormal again
116
        dcm_orthonormalize(dcm);
117
}
118
 
119
//-------------------------------------------------------------------
120
// imu_update
121
//-------------------------------------------------------------------
122
#define ACC_WEIGHT 0.01         //accelerometer data weight relative to gyro's weight of 1
123
#define MAG_WEIGHT 0.0          //magnetometer data weight relative to gyro's weight of 1
124
 
125
void imu_update(void) {
126
        int i;
127
        imu_sequence++;
128
 
129
        //interval since last call
130
 
131
        //---------------
132
        // I,J,K unity vectors of global coordinate system I-North,J-West,K-zenith
133
        // i,j,k unity vectors of body's coordiante system  i-"nose", j-"left wing", k-"top"
134
        //---------------
135
        //                      [I.i , I.j, I.k]
136
        // DCM =        [J.i , J.j, J.k]
137
        //                      [K.i , K.j, K.k]
138
 
139
 
140
        //---------------
141
        //Acelerometer
142
        //---------------
143
        //Accelerometer measures gravity vector G in body coordinate system
144
        //Gravity vector is the reverse of K unity vector of global system expressed in local coordinates
145
        //K vector coincides with the z coordinate of body's i,j,k vectors expressed in global coordinates (K.i , K.j, K.k)
146
        float Kacc[3];
147
        //Acc can estimate global K vector(zenith) measured in body's coordinate systems (the reverse of gravitation vector)
148
        Kacc[0] = -acc_ATT[X];
149
        Kacc[1] = -acc_ATT[Y];
150
        Kacc[2] = -acc_ATT[Z];
151
        vector3d_normalize(Kacc);
152
        //calculate correction vector to bring dcmGyro's K vector closer to Acc vector (K vector according to accelerometer)
153
        float wA[3];
154
        vector3d_cross(dcmGyro[2], Kacc, wA); // wA = Kgyro x  Kacc , rotation needed to bring Kacc to Kgyro
155
 
156
        //---------------
157
        //Magnetomer
158
        //---------------
159
        //calculate correction vector to bring dcmGyro's I vector closer to Mag vector (I vector according to magnetometer)
160
        float Imag[3];
161
        float wM[3];
162
        //in the absense of magnetometer let's assume North vector (I) is always in XZ plane of the device (y coordinate is 0)
163
        Imag[0] = sqrt(1 - dcmGyro[0][2] * dcmGyro[0][2]);
164
        Imag[1] = 0;
165
        Imag[2] = dcmGyro[0][2];
166
 
167
        vector3d_cross(dcmGyro[0], Imag, wM); // wM = Igyro x Imag, roation needed to bring Imag to Igyro
168
 
169
        //---------------
170
        //dcmGyro
171
        //---------------
172
        float w[3]; //gyro rates (angular velocity of a global vector in local coordinates)
173
        w[0] = -gyro_ATT[PITCH] / 1000.0; //rotation rate about accelerometer's X axis (GY output) in rad/ms
174
        w[1] = -gyro_ATT[ROLL]  / 1000.0; //rotation rate about accelerometer's Y axis (GX output) in rad/ms
175
        w[2] = -gyro_ATT[YAW]   / 1000.0; //rotation rate about accelerometer's Z axis (GZ output) in rad/ms
176
 
177
        for (i = 0; i < 3; i++) {
178
                w[i] *= INTEGRATION_TIME_MS; //scale by elapsed time to get angle in radians
179
                //compute weighted average with the accelerometer correction vector
180
                w[i] = (w[i] + ACC_WEIGHT * wA[i] + MAG_WEIGHT * wM[i]) / (1.0
181
                                + ACC_WEIGHT + MAG_WEIGHT);
182
        }
183
 
184
        dcm_rotate(dcmGyro, w);
185
 
186
        //Output for PicQuadController_GYRO_DEBUG1.scc
187
        //only output data ocasionally to allow computer to process data
188
        /*
189
         if(0 == imu_sequence % 4){
190
         printf("%.5f,",(double)interval_ms);
191
         print_float_list(3,(float*)w);
192
         printf(",%.2f,%.2f,%.2f",adcAvg[3+1],adcAvg[3+0],adcAvg[3+2]);
193
 
194
         printf("\n ");
195
         }
196
         */
197
 
198
        //Output for: PICQUADCONTROLLER_DEBUG1.pde
199
        //only output data ocasionally to allow computer to process data
200
        /*
201
        if (0 == imu_sequence % 16) {
202
                printf("%.2f,", (double) interval_ms);
203
                print_float_list(3, (float*) Kacc);
204
                printf(", ");
205
                print_float_list(9, (float*) dcmGyro);
206
                printf("\n");
207
        }
208
        */
209
}
210
 
211
void attitude_update(void) {
212
        analog_update();
213
        startAnalogConversionCycle();
214
 
215
        // Takes 4 ms.
216
        imu_update();
217
}