Subversion Repositories Projects

#include <avr/io.h>  
#include <math.h>
#include <stdlib.h>
#include "vector.h"
#include <util/delay.h>
#include <inttypes.h>
#include <avr/interrupt.h>
#include "headtracker.h"

#if(DEBUG)
        #include "UART.h"
        #include <string.h>
#endif


volatile        uint16_t ppm_ch[PPM_CHANNELS];

enum calibration_mode {CAL_NONE, CAL_X, CAL_Y, CAL_Z};

void ppm_init(void)
{

        for(int i = 0; i<PPM_CHANNELS; i++) ppm_ch[i] = DEFAULT_US;
       
        //TCNT 1 - PPM out -
        DDRD            |= PPM_PIN;
        TIMSK           |= (1<<OCIE1B);

        TCNT1            = 0;
        OCR1A            = ppm_ch[0];
        OCR1B            = PPM_SYNC_PW;
        TCCR1A          |= (1<<COM1B1) | (PPM_INV<<COM1B0)      | (1<<WGM11) | (1<<WGM10);
        TCCR1B          |= (1<<WGM13)   | (1<<WGM12);
        TCCR1B          |= (0<<CS12)    | (1<<CS11)     | (0<<CS10 );

        sei();

}


ISR(TIMER1_COMPB_vect)
{
        static uint8_t i = 0;
        static uint16_t frame_pw = PPM_FRAME;
       
        //output PPM channels
        ++i;
       
        if(i >= PPM_CHANNELS+1)
        {
                i                       = 0;
                frame_pw        = PPM_FRAME;
        }
       
        if(i < PPM_CHANNELS)
        {

                OCR1A            = ppm_ch[i];
                frame_pw        -= OCR1A;
        }
        else
        {
                OCR1A           = frame_pw;
        }
                       
}



int main()
{
        _delay_ms(500);  //delay for VCC stabilization
       
        ppm_init();
       
        #if(DEBUG)
                uart_init();
        #endif

        //************************************************************************
        //** init LSM303DLH                                                                                                             **
        //************************************************************************
        DDRC = 0;                              // all inputs
        PORTC = (1 << PORTC4) | (1 << PORTC5); // enable pull-ups on SDA and SCL


        TWSR = 0;  // clear bit-rate prescale bits
        TWBR = 17;

        //enable accelerometer
        i2c_start();
        i2c_write_byte(0x30); // write acc
        i2c_write_byte(0x20); // CTRL_REG1_A
        i2c_write_byte(0x27); // normal power mode, 50 Hz data rate, all axes enabled
        i2c_stop();

        //enable magnetometer
        i2c_start();
        i2c_write_byte(0x3C); // write mag
        i2c_write_byte(0x02); // MR_REG_M
        i2c_write_byte(0x00); // continuous conversion mode
        i2c_stop();

        //raw acc and mag
        vector a, m;
        //filtered acc and mag
        vector a_avg, m_avg;

        //IIR filter will overshoot at startup, read in a couple of values to get it stable
        for(int i = 0; i<20; i++)       read_data(&a_avg, &m_avg);
       
        //get zero reference
        float heading_start, pitch_start, roll_start;
        heading_start   = get_heading(&a_avg, &m_avg, &p);
        pitch_start     = get_perpendicular(&a_avg, &down, &p);
        roll_start              = get_perpendicular(&a_avg, &down, &right);

        enum calibration_mode mode = CAL_NONE;
        vector cal_m_max = {0, 0, 0};
        vector cal_m_min = {0, 0, 0};

        while(1)
        {
               
                if (mode == CAL_NONE) // run mode
                {
                       
                        int thr_in, thr_out;           
               
                        read_data(&a_avg, &m_avg);

                        //get angle - startoffset
                        int heading_us  = thr_filter(get_us(get_heading(&a_avg, &m_avg, &p) - heading_start, HDG_MIN, HDG_MAX, HDG_US_MIN, HDG_US_MAX),&thr_in, &thr_out);
                        int pitch_us    = get_us(get_perpendicular(&a_avg, &down, &p) - pitch_start,            PIT_MIN, PIT_MAX, PIT_US_MIN, PIT_US_MAX);
                        int roll_us     = get_us(get_perpendicular(&a_avg, &down, &right) - roll_start,         ROL_MIN, ROL_MAX, ROL_US_MIN, ROL_US_MAX);;    
                       
                        #if(DEBUG)
                                printf("H %d P %d R %d \n", heading_us, pitch_us, roll_us);
                        #endif
                       
                       
                        ppm_ch[HDG_CH-1] = heading_us;  //subtract 1, CHs from 1 - N, array bounds from 0 - N-1
                        ppm_ch[PIT_CH-1] = pitch_us;
                        ppm_ch[ROL_CH-1] = roll_us;
                }
       

                else // calibration mode - record max/min measurements
                {
                        //TODO
                        //implement calib_test.c
                       
                       
                        read_data_raw(&a, &m);
                        if (m.x < cal_m_min.x) cal_m_min.x = m.x;
                        if (m.x > cal_m_max.x) cal_m_max.x = m.x;
                        if (m.y < cal_m_min.y) cal_m_min.y = m.y;
                        if (m.y > cal_m_max.y) cal_m_max.y = m.y;
                        if (m.z < cal_m_min.z) cal_m_min.z = m.z;
                        if (m.z > cal_m_max.z) cal_m_max.z = m.z;


               
                }

                _delay_ms(40);
        }
}