Subversion Repositories Projects

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

// LocoHead - Mathias Kreider, 2011 //

//                                                                      //

// main.c                                                       //

// Main, PPM and Debug functions    //          

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

#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 stabilisation

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

                        //calculate servo pulselength from sensor data

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

        }

}