File content as of revision 5:40ac894e0fa7:

/**************************************************************************************************************************************************************
//  Created by: Ryan Spillman
//
//  Last updated 4/4/2015
//
//  Contains several functions such as calculations for roll, pitch, and yaw (tilt compensated compass)
//  Also contains various filtering and calibration functions
//
//  Requires L3GD20 gyro and LSM303DLHC magnetometer and accelerometer
**************************************************************************************************************************************************************/

#include "mbed.h"
#include "math.h"
#include "L3GD20.h"
#include "LSM303DLHC.h"

#define modSensData_h

L3GD20 gyro(PTE25, PTE24);
LSM303DLHC compass(PTE25, PTE24);

//Variable for low pass filter
#define ALPHA_H             0.2f 
#define ALPHA_A             0.1f
#define MAX_DELTA_HEADING   20

#define M_PI                3.1415926535897932384626433832795f
#define TWO_PI              6.283185307179586476925286766559f
#define RADTODEGREE         57.295779513082320876798154814105f
#define DEGREETORAD         0.01745329251994329576923690768489f
#define GRAVITY             1.0f
#define THRESHOLD           0.02f

//gyro sample period in seconds
#define GYRO_SAMPLE_PERIOD  0.01f
#define CALIBRATION_COUNT   64
#define AVG_COUNT           10

// "accel x,y,z (min/max) = X_MIN/X_MAX  Y_MIN/Y_MAX  Z_MIN/Z_MAX"
#define ACCEL_X_MIN         -1.000000000f
#define ACCEL_X_MAX         1.023437500f
#define ACCEL_Y_MIN         -1.015625000f
#define ACCEL_Y_MAX         1.023437500f
#define ACCEL_Z_MIN         -1.023437500f 
#define ACCEL_Z_MAX         0.960937500f

// "magn x,y,z (min/max) = X_MIN/X_MAX  Y_MIN/Y_MAX  Z_MIN/Z_MAX"
#define MAGN_X_MIN          (-0.370909f)
#define MAGN_X_MAX          (0.569091f)
#define MAGN_Y_MIN          (-0.516364f)
#define MAGN_Y_MAX          (0.392727f)
#define MAGN_Z_MIN          (-0.618367f)
#define MAGN_Z_MAX          (0.408163f)


//Period in seconds of heading PID loop
#define COMPASS_PERIOD              0.060f

#define YAW_VAR_TOLER               5.0f

// Sensor calibration scale and offset values
#define ACCEL_X_OFFSET ((ACCEL_X_MIN + ACCEL_X_MAX) / 2.0f)
#define ACCEL_Y_OFFSET ((ACCEL_Y_MIN + ACCEL_Y_MAX) / 2.0f)
#define ACCEL_Z_OFFSET ((ACCEL_Z_MIN + ACCEL_Z_MAX) / 2.0f)
#define ACCEL_X_SCALE (GRAVITY / (ACCEL_X_MAX - ACCEL_X_OFFSET))
#define ACCEL_Y_SCALE (GRAVITY / (ACCEL_Y_MAX - ACCEL_Y_OFFSET))
#define ACCEL_Z_SCALE (GRAVITY / (ACCEL_Z_MAX - ACCEL_Z_OFFSET))

#define MAGN_X_OFFSET ((MAGN_X_MIN + MAGN_X_MAX) / 2.0f)
#define MAGN_Y_OFFSET ((MAGN_Y_MIN + MAGN_Y_MAX) / 2.0f)
#define MAGN_Z_OFFSET ((MAGN_Z_MIN + MAGN_Z_MAX) / 2.0f)
//not sure what the "normal" value is, thus not using scale for magnetometer
//#define MAGN_X_SCALE (??? / (MAGN_X_MAX - MAGN_X_OFFSET))
//#define MAGN_Y_SCALE (??? / (MAGN_Y_MAX - MAGN_Y_OFFSET))
//#define MAGN_Z_SCALE (??? / (MAGN_Z_MAX - MAGN_Z_OFFSET))


float magnetom[3] = {0,0,0};
float accel[3] = {0,0,0};
float degGyro[3] = {0,0,0};
float prevDegGyro[3]= {0,0,0};
float gyroOffset[3] = {0,0,0};
float roll = 0, pitch = 0, yaw = 0;
float totalAccel = 0;
float normalizedGravity;

float filteredYaw, filteredPitch, filteredRoll;

float lowPassAccel[3] = {0.0000001f,0.0000001f,0.0000001f};
float lowPassMagnetom[3] = {0.0000001f,0.0000001f,0.0000001f};

float prevYaw = 0;
int cycleCount = 0;
int firstPass = 1;
/*
float lowPass(float input, float output)  
{
    if (output == 0.0000001f) return input;
    
    output = (ALPHA * input) + ((1 - ALPHA) * output);
    
    return output;
}
*/
float lowPass(float input, float output, int select) 
{
    //Accelerometer alpha
    if(select == 1)
    {
        if (output == 0.0000001f) return input;
        
        output = output + ALPHA_A * (input - output);
        
        return output;
    }
    //Heading alpha
    else
    {
        if (output == 0.0000001f) return input;
        
        output = output + ALPHA_H * (input - output);
        
        return output;
    }
}

/********************************************************************************************************************************/
//                                          getMagn
/********************************************************************************************************************************/

void getMagn()
{
    compass.ReadMagnOnly(&magnetom[0], &magnetom[1], &magnetom[2]);

    //Compensate magnetometer error
    magnetom[0] -= MAGN_X_OFFSET;
    magnetom[1] -= MAGN_Y_OFFSET;
    magnetom[2] -= MAGN_Z_OFFSET;
}
/********************************************************************************************************************************/
//                                          getAccel
/********************************************************************************************************************************/

void getAccel()
{
    compass.ReadAccOnly(&accel[0], &accel[1], &accel[2]);
    
    //Compensate accelerometer error
    accel[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE;
    accel[1] = (accel[1] - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE;
    accel[2] = (accel[2] - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE;

    
    for(int i = 0; i <= 3; i++)
    {
        lowPassAccel[i] = lowPass(accel[i], lowPassAccel[i], 1);
    }    
}
void gyroIntegral()
{
    float yawDiff = yaw - prevYaw;
    float absYawDiff = sqrt(yawDiff * yawDiff);
    //vC[x] = vP[x] + ((aP[x] + ((inertialAccel[x] - aP[x]) / 2)) * deltaT);
    float gyroInt = prevYaw + ((prevDegGyro[2] + ((degGyro[2] - prevDegGyro[2]) / 2)) * COMPASS_PERIOD);
    float absGyroInt = sqrt(gyroInt * gyroInt);
    prevDegGyro[2] = degGyro[2];
    if(absYawDiff > absGyroInt)
    {
        yaw = prevYaw + gyroInt;
    }
}
/********************************************************************************************************************************/
//                                                      Compass Heading
/********************************************************************************************************************************/

void Compass_Heading()
{

    float mag_x;
    float mag_y;
    //if((roll >= 0.0873f) || (roll <= -0.0873f) || (pitch >= 0.0873f) || (pitch <= -0.0873f))
    //{
        float cos_roll;
        float sin_roll;
        float cos_pitch;
        float sin_pitch;
        
        //saves a few processor cycles by calculating and storing values
        cos_roll = cos(roll);
        sin_roll = sin(roll);
        cos_pitch = cos(pitch);
        sin_pitch = sin(pitch);
        
        // Tilt compensated magnetic field X
        //mag_x = magnetom[0] * cos_pitch + magnetom[1] * sin_roll * sin_pitch + magnetom[2] * cos_roll * sin_pitch;
        mag_x = magnetom[0] * cos_pitch + magnetom[1] * sin_roll * sin_pitch + magnetom[2] * cos_roll * sin_pitch;
        // Tilt compensated magnetic field Y
        //mag_y = magnetom[1] * cos_roll - magnetom[2] * sin_roll;
        mag_y = magnetom[1] * cos_roll - magnetom[2] * sin_roll;
    //}
    /*  
    else
    {
        mag_x = magnetom[0] ;
        mag_y = magnetom[1];
    }
    */
    // Magnetic Heading
    yaw = atan2(-mag_x, mag_y);
    
    
    //make negative angles positive
    if(yaw < 0) yaw = yaw + TWO_PI;
    //yaw = yaw + M_PI;
    yaw = yaw * RADTODEGREE;
}

/********************************************************************************************************************************/
//                                                      getAttitude
/********************************************************************************************************************************/

void getAttitude()
{
  float temp1[3];
  float temp2[3];
  float xAxis[3] = {1.0f, 0.0f, 0.0f};
  
  // GET PITCH
  // Using y-z-plane-component/x-component of gravity vector    
  pitch = -atan2(accel[0], sqrt((accel[1] * accel[1]) + (accel[2] * accel[2])));
  
  //printf("P = %f", pitch);
  
  // GET ROLL
  // Compensate pitch of gravity vector 
  temp1[0] = (accel[1] * xAxis[2]) - (accel[2] * xAxis[1]);
  temp1[1] = (accel[2] * xAxis[0]) - (accel[0] * xAxis[2]);
  temp1[2] = (accel[0] * xAxis[1]) - (accel[1] * xAxis[0]);
  
  temp2[0] = (xAxis[1] * temp1[2]) - (xAxis[2] * temp1[1]);
  temp2[1] = (xAxis[2] * temp1[0]) - (xAxis[0] * temp1[2]);
  temp2[2] = (xAxis[0] * temp1[1]) - (xAxis[1] * temp1[0]);
  
  // Since we compensated for pitch, x-z-plane-component equals z-component:
  roll = atan2(temp2[1], temp2[2]);
}

/********************************************************************************************************************************/
//                                                      Compass Heading
/********************************************************************************************************************************/

void Filtered_Compass_Heading()
{

    float mag_x;
    float mag_y;
    //if((roll >= 0.0873f) || (roll <= -0.0873f) || (pitch >= 0.0873f) || (pitch <= -0.0873f))
    //{
        float cos_roll;
        float sin_roll;
        float cos_pitch;
        float sin_pitch;
        
        //saves a few processor cycles by calculating and storing values
        cos_roll = cos(filteredRoll);
        sin_roll = sin(filteredRoll);
        cos_pitch = cos(filteredPitch);
        sin_pitch = sin(filteredPitch);
        
        // Tilt compensated magnetic field X
        //mag_x = magnetom[0] * cos_pitch + magnetom[1] * sin_roll * sin_pitch + magnetom[2] * cos_roll * sin_pitch;
        mag_x = lowPassMagnetom[0] * cos_pitch + lowPassMagnetom[1] * sin_roll * sin_pitch + lowPassMagnetom[2] * cos_roll * sin_pitch;
        // Tilt compensated magnetic field Y
        //mag_y = magnetom[1] * cos_roll - magnetom[2] * sin_roll;
        mag_y = lowPassMagnetom[1] * cos_roll - lowPassMagnetom[2] * sin_roll;
    //}
    /*  
    else
    {
        mag_x = magnetom[0] ;
        mag_y = magnetom[1];
    }
    */
    // Magnetic Heading
    filteredYaw = atan2(-mag_x, mag_y);
    
    
    //make negative angles positive
    if(filteredYaw < 0) filteredYaw = filteredYaw + TWO_PI;
    //yaw = yaw + M_PI;
    filteredYaw = filteredYaw * RADTODEGREE;
}

/********************************************************************************************************************************/
//                                                      Filtered_Attitude()
/********************************************************************************************************************************/

void Filtered_Attitude()
{
  float temp1[3];
  float temp2[3];
  float xAxis[3] = {1.0f, 0.0f, 0.0f};
  
  // GET PITCH
  // Using y-z-plane-component/x-component of gravity vector    
  pitch = -atan2(lowPassAccel[0], sqrt((lowPassAccel[1] * lowPassAccel[1]) + (lowPassAccel[2] * lowPassAccel[2])));
  
  //printf("P = %f", pitch);
  
  // GET ROLL
  // Compensate pitch of gravity vector 
  temp1[0] = (lowPassAccel[1] * xAxis[2]) - (lowPassAccel[2] * xAxis[1]);
  temp1[1] = (lowPassAccel[2] * xAxis[0]) - (lowPassAccel[0] * xAxis[2]);
  temp1[2] = (lowPassAccel[0] * xAxis[1]) - (lowPassAccel[1] * xAxis[0]);
  
  temp2[0] = (xAxis[1] * temp1[2]) - (xAxis[2] * temp1[1]);
  temp2[1] = (xAxis[2] * temp1[0]) - (xAxis[0] * temp1[2]);
  temp2[2] = (xAxis[0] * temp1[1]) - (xAxis[1] * temp1[0]);
  
  // Since we compensated for pitch, x-z-plane-component equals z-component:
  roll = atan2(temp2[1], temp2[2]);
}


/********************************************************************************************************************************/
//                                                      updateAngles
/********************************************************************************************************************************/

void updateAngles()
//int updateAngles()
{
    //readIMU();
    //getAttitude();
    Filtered_Attitude();
    Compass_Heading();
    //Filtered_Attitude();
    //Filtered_Compass_Heading();
    /*
    totalAccel = sqrt((lowPassAccel[0] * lowPassAccel[0]) + (lowPassAccel[1] * lowPassAccel[1]) + (lowPassAccel[2] * lowPassAccel[2]));
    
    prevYaw = yaw;
    getAttitude();
    Compass_Heading();
    
    if((sqrt((prevYaw - yaw) * (prevYaw - yaw)) >= YAW_VAR_TOLER) && (firstPass != 1))
    {
        yaw = prevYaw;
    }
    else if(firstPass == 1)
    {
        firstPass = 0;
    }
    
    //if((sqrt(accel[0] * accel[0]) <= THRESHOLD) && (sqrt(accel[1] * accel[1]) <= THRESHOLD) && (sqrt(accel[2] * accel[2]) <= THRESHOLD))
    if((totalAccel <= (normalizedGravity + THRESHOLD)) && (totalAccel >= (normalizedGravity - THRESHOLD)))
    {
        //float prevYaw = yaw;
        getAttitude();
        Compass_Heading();
        //float absOfYawDiff = sqrt((prevYaw - yaw) * (prevYaw - yaw));    
        //check for noise
        //if((absOfYawDiff >= 30) && (absOfYawDiff <= (360.0f - 30)))
        //{
        return 1;
        //}
        //return 0;
        
    }
    */
    //return 0;
    //else Compass_Heading();
    //else return 0;
    //getAttitude();
    //Compass_Heading();
    //gyroIntegral();
}

/********************************************************************************************************************************/
//                                                      magAvg
/********************************************************************************************************************************/

void compassAvg()
{
    float accumulator = 0;
    //float prevYaw[AVG_COUNT];
    int sampleCount  = 0;

    
    //We'll take a certain number of samples and then average them to calculate the offset
    while (sampleCount < AVG_COUNT) 
    {
        updateAngles();
        
        //add current sample to previous samples
        accumulator += yaw;
        
        sampleCount++;
        //Make sure the gyro has had enough time to take a new sample.
        wait(COMPASS_PERIOD);
    }
    
        //divide by number of samples to get average offset
    yaw = accumulator / AVG_COUNT;
    

}

/********************************************************************************************************************************/
//                                                      magLowPass
/********************************************************************************************************************************/
void compLowPass()
{
    /*
    prevMagnetom[5][3];
    //read sensors five times
    for(int x = 0; x<= 5; i++)
    {
        for(int i = 0; i<=3; i++)
        {
            prevMagnetom[x, i] = magnetom[i];
        }
        readIMU();
    }
    float diff[5][3];
    for(int x = 1; x<= 5; i++)
    {
        sqrt(prevMagnetom[x][0]
        for(int i = 0; i<=3; i++)
        {
            if(diff[x][i] = sqrt((prevMagnetom[x][i] - prevMagnemtom[x - 1]) * (prevMagnetom[x][i] - prevMagnemtom[x - 1])) >= MAG_VAR_TOLER)
            {
                
            }
        }
    }
    
    float deltaHeading = 0;
    updateAngles();
    deltaHeading = sqrt((yaw - prevYaw[cycleCount]) * (yaw - prevYaw[cycleCount]));
    
    if(deltaHeading >= MAX_DELTA_HEADING)
    {
        yaw = (yaw + prevYaw[cycleCount]) / 2.0f;
    }
    cycleCount = cycleCount + 1;
    prevYaw[cycleCount] = yaw;
    
    if(cycleCount >= 4)
    {
        cycleCount = 0;
        prevYaw = {0,0,0,0,0}
    }
*/
}


/********************************************************************************************************************************/
//                                                      normalizeGravity
/********************************************************************************************************************************/

void normalizeGravity()
{
    float accumulator = 0;
    int sampleCount  = 0;

    
    //We'll take a certain number of samples and then average them to calculate the average
    while (sampleCount < AVG_COUNT) 
    {
        getAccel();
        //add current sample to previous samples
        accumulator += sqrt((accel[0] * accel[0]) + (accel[1] * accel[1]) + (accel[2] * accel[2]));
        sampleCount++;
        //Make sure the IMU has had enough time to take a new sample.
        wait(0.06);
    }
    

    //divide by number of samples to get average offset
    normalizedGravity = accumulator / AVG_COUNT;

}
/********************************************************************************************************************************/
//                                                      gyroAvg
/********************************************************************************************************************************/

void gyroAvg()
{
    float accumulator[3] = {0,0,0};
    int sampleCount  = 0;

    
    //We'll take a certain number of samples and then average them to calculate the offset
    while (sampleCount < AVG_COUNT) 
    {
        //get gyro data
        gyro.read(&degGyro[0], &degGyro[1], &degGyro[2]);
        //add current sample to previous samples
        accumulator[2] += degGyro[2];
        sampleCount++;
        //Make sure the gyro has had enough time to take a new sample.
        wait(GYRO_SAMPLE_PERIOD);
    }
    

    //divide by number of samples to get average offset
    degGyro[2] = accumulator[2] / AVG_COUNT;

}

/********************************************************************************************************************************/
//                                                      gyroCal
/********************************************************************************************************************************/

void gyroCal()
{
    float accumulator[3] = {0,0,0};
    int sampleCount  = 0;

    
    //We'll take a certain number of samples and then average them to calculate the offset
    while (sampleCount < CALIBRATION_COUNT) 
    {
        float gyroData[3];
        //Make sure the gyro has had enough time to take a new sample.
        wait(GYRO_SAMPLE_PERIOD);
        //get gyro data
        gyro.read(&gyroData[0],&gyroData[1],&gyroData[2]);
        for(int i = 0; i < 3; i++)
        {  
            //add current sample to previous samples
            accumulator[i] += gyroData[i];
        }
        sampleCount++;
    }
    
    for(int i = 0; i < 3; i++)
    {
        //divide by number of samples to get average offset
        gyroOffset[i] = accumulator[i] / CALIBRATION_COUNT;
    }
}

void gyroOnly()
{
    gyro.read(&degGyro[0], &degGyro[1], &degGyro[2]);
    //compensate for gyro offset
    for(int i=0;i<=3;i++)
    {
        degGyro[i] -= gyroOffset[i];
    }
}