Subversion Repositories FlightCtrl

#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();

}