Subversion Repositories Projects

//////////////////////////////////////

// LocoHead - Mathias Kreider, 2011 //                  

//                                                                      //

// headtracker.c                                        //

// I2C, angular and filter functions//          

//////////////////////////////////////

#include "vector.h"

#include <math.h>

#include <inttypes.h>

#include <avr/io.h>  

#include <stdlib.h>

extern vector m_max;

extern vector m_min;

void i2c_start() {  

        TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN); // send start condition  

        while (!(TWCR & (1 << TWINT)));  

}  

void i2c_write_byte(char byte) {  

        TWDR = byte;              

        TWCR = (1 << TWINT) | (1 << TWEN); // start address transmission  

        while (!(TWCR & (1 << TWINT)));  

}  

char i2c_read_byte() {  

        TWCR = (1 << TWINT) | (1 << TWEA) | (1 << TWEN); // start data reception, transmit ACK  

        while (!(TWCR & (1 << TWINT)));  

        return TWDR;  

}  

char i2c_read_last_byte() {  

        TWCR = (1 << TWINT) | (1 << TWEN); // start data reception

        while (!(TWCR & (1 << TWINT)));  

        return TWDR;  

}  

void i2c_stop() {  

          TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN); // send stop condition  

}  

// Returns a set of acceleration and raw magnetic readings from the cmp01a.

void read_data_raw(vector *a, vector *m)

{

        // read accelerometer values

        i2c_start();

        i2c_write_byte(0x30); // write acc

        i2c_write_byte(0xa8); // OUT_X_L_A, MSB set to enable auto-increment

        i2c_start();              // repeated start

        i2c_write_byte(0x31); // read acc

        unsigned char axl = i2c_read_byte();

        unsigned char axh = i2c_read_byte();

        unsigned char ayl = i2c_read_byte();

        unsigned char ayh = i2c_read_byte();

        unsigned char azl = i2c_read_byte();

        unsigned char azh = i2c_read_last_byte();

        i2c_stop();

        // read magnetometer values

        i2c_start();

        i2c_write_byte(0x3C); // write mag

        i2c_write_byte(0x03); // OUTXH_M

        i2c_start();              // repeated start

        i2c_write_byte(0x3D); // read mag

        unsigned char mxh = i2c_read_byte();

        unsigned char mxl = i2c_read_byte();

        unsigned char myh = i2c_read_byte();

        unsigned char myl = i2c_read_byte();

        unsigned char mzh = i2c_read_byte();

        unsigned char mzl = i2c_read_last_byte();

        i2c_stop();

        a->x = axh << 8 | axl;

        a->y = ayh << 8 | ayl;

        a->z = azh << 8 | azl;

        m->x = mxh << 8 | mxl;

        m->y = myh << 8 | myl;

        m->z = mzh << 8 | mzl;

}

float IIR2(float x, float* z)

{

        //const for butterworth lowpass fc 0.5Hz   

//      const float a[3] = {1.0000,   -1.8521,    0.8623};

//      const float b[3] = {0.0026,    0.0051,    0.0026};

        //const for butterworth lowpass fc 2Hz   

        const float a[3] = {1.0000,   -1.4190,    0.5533};

        const float b[3] = {0.0336,    0.0671,    0.0336};

        float y,r;

        r       =       a[1]*z[0]+a[2]*z[1];

        y       =       b[0]*(x-r)+b[1]*z[0]+b[2]*z[1];

        z[1]=   z[0];

        z[0]=   x-r;

        return y;

}

//cancels out movemt below threshold while using step sum to 

int thr_filter(int  x, int * x_reg, int * y_reg)

{

        int  y;

        int  diff;

        int  sum = 0;

        const int  thr = 4;

        const int  lmt = 5;

        diff = x - *x_reg;

    if(abs(diff) <= thr)

        {

       sum += diff;

       if(abs(sum) >= lmt)

           {

           sum = 0;

           y = x;

                }

        else y = *y_reg;

        }

        else

        {

        y = x;

        sum = 0;

        }

        *x_reg = x;

        *y_reg = y;

        return y;

}

// Returns corrected and low-pass filtered magnetometer and accelerometer values

void read_data(vector *a, vector *m)

{

        //interal state buffers for IIR axis filtering

        static float zm_x[2] = {0.0, 0.0};

        static float zm_y[2] = {0.0, 0.0};

        static float zm_z[2] = {0.0, 0.0};

        static float za_x[2] = {0.0, 0.0};

        static float za_y[2] = {0.0, 0.0};

        static float za_z[2] = {0.0, 0.0};

        read_data_raw(a, m);

        //low pass filter acc

        a->x = IIR2(a->x, za_x);

        a->y = IIR2(a->y, za_y);

        a->z = IIR2(a->z, za_z);

        //compensate scale and offset, low pass filter mag

        m->x = IIR2(((m->x - m_min.x) / (m_max.x - m_min.x) * 2 - 1.0), zm_x);

        m->y = IIR2(((m->y - m_min.y) / (m_max.y - m_min.y) * 2 - 1.0), zm_y);

        m->z = IIR2(((m->z - m_min.z) / (m_max.z - m_min.z) * 2 - 1.0), zm_z);

}

float get_heading(const vector *a, const vector *m, const vector *p)

{

        vector E;

        vector N;

        // cross magnetic vector (magnetic north + inclination) with "down" (acceleration vector) to produce "west"

        // -- right hand rule says

        vector_cross(m, a, &E);

        vector_normalize(&E);

        // cross "down" with "east" to produce "north" (parallel to the ground)

        vector_cross(a, &E, &N);

        vector_normalize(&N);

        // compute heading

        float heading = atan2(vector_dot(&E, p), vector_dot(&N, p)) * 180.0 / M_PI;

        return heading;

}

float get_perpendicular(const vector *a, const vector *d, const vector *q)

{

        float sign = 0.0;

        vector norma = *a;

        if              (q->x == 0.0) {norma.x = 0.0; sign = norma.y;}// cancel out movement on undesired axis

        else if (q->y == 0.0) {norma.y = 0.0; sign = norma.x;} 

        vector_normalize(&norma);

        // compute angle

        float angle = acos(vector_dot(&norma,d)) * 180.0/M_PI;

        if(sign >= 0.0) angle *= -1;

        return angle;

}

int get_us(float angle, float deg_min, float deg_max, int pwm_min,int pwm_max)

{

        //adjust sign change of angular function to new zero offset

        if(angle < -180.0) angle += 360.0;

        if(angle >= 180.0) angle -= 360.0;

        //crop

        if(angle < deg_min) angle  = deg_min;

        else if (angle  > deg_max) angle  = deg_max;

        //scale to pwm

        float   ratio = ((float)(pwm_max - pwm_min)) / (deg_max - deg_min);

        int diff = ((int)((angle-deg_min) * ratio));

        return pwm_min + diff;

}