Blame |
Last modification |
View Log
| RSS feed
#include <stdlib.h>
#include <avr/io.h>
#include <stdio.h>
#include "attitude.h"
#include "commands.h"
#include "vector3d.h"
// For scope debugging only!
#include "rc.h"
// where our main data flow comes from.
#include "analog.h"
#include "configuration.h"
#include "output.h"
// Some calculations are performed depending on some stick related things.
#include "controlMixer.h"
#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}
/*
* Gyro integrals. These are the rotation angles of the airframe relative to horizontal.
*/
float attitude
[3];
uint8_t imu_sequence
= 0; //incremented on each call to imu_update
float dcmAcc
[3][3]; //dcm matrix according to accelerometer
float dcmGyro
[3][3]; //dcm matrix according to gyroscopes
float dcmEst
[3][3]; //estimated dcm matrix by fusion of accelerometer and gyro
float getAngleEstimateFromAcc
(uint8_t axis
) {
return GYRO_ACC_FACTOR
* acc_ATT
[axis
];
}
/************************************************************************
* Neutral Readings
************************************************************************/
void attitude_setNeutral
(void) {
// Calibrate hardware.
analog_setNeutral
();
unsigned char i
, j
;
for (i
= 0; i
< 3; i
++)
for (j
= 0; j
< 3; j
++)
dcmGyro
[i
][j
] = (i
== j
) ? 1.0 : 0.0;
}
/*
How to use this module in other projects.
Input variables are:
adcAvg[0..6] ADC readings of 3 axis accelerometer and 3 axis gyroscope (they are calculated in the background by adcutil.h)
interval_us - interval in microseconds since last call to imu_update
Output variables are:
DcmEst[0..2] which are the direction cosine of the X,Y,Z axis
First you must initialize the module with:
imu_init();
Then call periodically every 5-20ms:
imu_update(interval_us);
it is assumed that you also update periodicall the adcAvg[0..5] array
*/
#define ACC_WEIGHT_MAX 0.02 //maximum accelerometer weight in accelerometer-gyro fusion formula
//this value is tuned-up experimentally: if you get too much noise - decrease it
//if you get a delayed response of the filtered values - increase it
//starting with a value of 0.01 .. 0.05 will work for most sensors
#define ACC_ERR_MAX 0.3 //maximum allowable error(external acceleration) where accWeight becomes 0
//-------------------------------------------------------------------
// Globals
//-------------------------------------------------------------------
//bring dcm matrix in order - adjust values to make orthonormal (or at least closer to orthonormal)
//Note: dcm and dcmResult can be the same
void dcm_orthonormalize
(float dcm
[3][3]) {
//err = X . Y , X = X - err/2 * Y , Y = Y - err/2 * X (DCMDraft2 Eqn.19)
float err
= vector3d_dot
((float*) (dcm
[0]), (float*) (dcm
[1]));
float delta
[2][3];
vector3d_scale
(-err
/ 2, (float*) (dcm
[1]), (float*) (delta
[0]));
vector3d_scale
(-err
/ 2, (float*) (dcm
[0]), (float*) (delta
[1]));
vector3d_add
((float*) (dcm
[0]), (float*) (delta
[0]), (float*) (dcm
[0]));
vector3d_add
((float*) (dcm
[1]), (float*) (delta
[0]), (float*) (dcm
[1]));
//Z = X x Y (DCMDraft2 Eqn. 20) ,
vector3d_cross
((float*) (dcm
[0]), (float*) (dcm
[1]), (float*) (dcm
[2]));
//re-nomralization
vector3d_normalize
((float*) (dcm
[0]));
vector3d_normalize
((float*) (dcm
[1]));
vector3d_normalize
((float*) (dcm
[2]));
}
//rotate DCM matrix by a small rotation given by angular rotation vector w
//see http://gentlenav.googlecode.com/files/DCMDraft2.pdf
void dcm_rotate
(float dcm
[3][3], float w
[3]) {
//float W[3][3];
//creates equivalent skew symetric matrix plus identity matrix
//vector3d_skew_plus_identity((float*)w,(float*)W);
//float dcmTmp[3][3];
//matrix_multiply(3,3,3,(float*)W,(float*)dcm,(float*)dcmTmp);
int i
;
float dR
[3];
//update matrix using formula R(t+1)= R(t) + dR(t) = R(t) + w x R(t)
for (i
= 0; i
< 3; i
++) {
vector3d_cross
(w
, dcm
[i
], dR
);
vector3d_add
(dcm
[i
], dR
, dcm
[i
]);
}
//make matrix orthonormal again
dcm_orthonormalize
(dcm
);
}
//-------------------------------------------------------------------
// imu_update
//-------------------------------------------------------------------
#define ACC_WEIGHT 0.01 //accelerometer data weight relative to gyro's weight of 1
#define MAG_WEIGHT 0.0 //magnetometer data weight relative to gyro's weight of 1
void imu_update
(void) {
int i
;
imu_sequence
++;
//interval since last call
//---------------
// I,J,K unity vectors of global coordinate system I-North,J-West,K-zenith
// i,j,k unity vectors of body's coordiante system i-"nose", j-"left wing", k-"top"
//---------------
// [I.i , I.j, I.k]
// DCM = [J.i , J.j, J.k]
// [K.i , K.j, K.k]
//---------------
//Acelerometer
//---------------
//Accelerometer measures gravity vector G in body coordinate system
//Gravity vector is the reverse of K unity vector of global system expressed in local coordinates
//K vector coincides with the z coordinate of body's i,j,k vectors expressed in global coordinates (K.i , K.j, K.k)
float Kacc
[3];
//Acc can estimate global K vector(zenith) measured in body's coordinate systems (the reverse of gravitation vector)
Kacc
[0] = -acc_ATT
[X
];
Kacc
[1] = -acc_ATT
[Y
];
Kacc
[2] = -acc_ATT
[Z
];
vector3d_normalize
(Kacc
);
//calculate correction vector to bring dcmGyro's K vector closer to Acc vector (K vector according to accelerometer)
float wA
[3];
vector3d_cross
(dcmGyro
[2], Kacc
, wA
); // wA = Kgyro x Kacc , rotation needed to bring Kacc to Kgyro
//---------------
//Magnetomer
//---------------
//calculate correction vector to bring dcmGyro's I vector closer to Mag vector (I vector according to magnetometer)
float Imag
[3];
float wM
[3];
//in the absense of magnetometer let's assume North vector (I) is always in XZ plane of the device (y coordinate is 0)
Imag
[0] = sqrt(1 - dcmGyro
[0][2] * dcmGyro
[0][2]);
Imag
[1] = 0;
Imag
[2] = dcmGyro
[0][2];
vector3d_cross
(dcmGyro
[0], Imag
, wM
); // wM = Igyro x Imag, roation needed to bring Imag to Igyro
//---------------
//dcmGyro
//---------------
float w
[3]; //gyro rates (angular velocity of a global vector in local coordinates)
w
[0] = -gyro_ATT
[PITCH
] / 1000.0; //rotation rate about accelerometer's X axis (GY output) in rad/ms
w
[1] = -gyro_ATT
[ROLL
] / 1000.0; //rotation rate about accelerometer's Y axis (GX output) in rad/ms
w
[2] = -gyro_ATT
[YAW
] / 1000.0; //rotation rate about accelerometer's Z axis (GZ output) in rad/ms
for (i
= 0; i
< 3; i
++) {
w
[i
] *= INTEGRATION_TIME_MS
; //scale by elapsed time to get angle in radians
//compute weighted average with the accelerometer correction vector
w
[i
] = (w
[i
] + ACC_WEIGHT
* wA
[i
] + MAG_WEIGHT
* wM
[i
]) / (1.0
+ ACC_WEIGHT
+ MAG_WEIGHT
);
}
dcm_rotate
(dcmGyro
, w
);
//Output for PicQuadController_GYRO_DEBUG1.scc
//only output data ocasionally to allow computer to process data
/*
if(0 == imu_sequence % 4){
printf("%.5f,",(double)interval_ms);
print_float_list(3,(float*)w);
printf(",%.2f,%.2f,%.2f",adcAvg[3+1],adcAvg[3+0],adcAvg[3+2]);
printf("\n ");
}
*/
//Output for: PICQUADCONTROLLER_DEBUG1.pde
//only output data ocasionally to allow computer to process data
/*
if (0 == imu_sequence % 16) {
printf("%.2f,", (double) interval_ms);
print_float_list(3, (float*) Kacc);
printf(", ");
print_float_list(9, (float*) dcmGyro);
printf("\n");
}
*/
}
void attitude_update
(void) {
analog_update
();
startAnalogConversionCycle
();
// Takes 4 ms.
imu_update
();
}