Subversion Repositories Projects

Rev

Rev 964 | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
967 - 1
//////////////////////////////////////
2
// LocoHead - Mathias Kreider, 2011 //
3
//                                                                      //
4
// main.c                                                       //
5
// Main, PPM and Debug functions    //          
6
//////////////////////////////////////
7
 
964 - 8
#include <avr/io.h>   
9
#include <math.h>
10
#include <stdlib.h>
11
#include "vector.h"
12
#include <util/delay.h>
13
#include <inttypes.h>
14
#include <avr/interrupt.h>
15
#include "headtracker.h"
16
 
17
#if(DEBUG)
18
        #include "UART.h"
19
        #include <string.h>
20
#endif
21
 
22
 
23
volatile        uint16_t ppm_ch[PPM_CHANNELS];
24
 
25
enum calibration_mode {CAL_NONE, CAL_X, CAL_Y, CAL_Z};
26
 
27
void ppm_init(void)
28
{
29
 
30
        for(int i = 0; i<PPM_CHANNELS; i++) ppm_ch[i] = DEFAULT_US;
31
 
32
        //TCNT 1 - PPM out - 
33
        DDRD            |= PPM_PIN;
34
        TIMSK           |= (1<<OCIE1B);
35
 
36
        TCNT1            = 0;
37
        OCR1A            = ppm_ch[0];
38
        OCR1B            = PPM_SYNC_PW;
39
        TCCR1A          |= (1<<COM1B1) | (PPM_INV<<COM1B0)      | (1<<WGM11) | (1<<WGM10);
40
        TCCR1B          |= (1<<WGM13)   | (1<<WGM12);
41
        TCCR1B          |= (0<<CS12)    | (1<<CS11)     | (0<<CS10 );
42
 
43
        sei();
44
 
45
}
46
 
47
 
48
ISR(TIMER1_COMPB_vect)
49
{
50
        static uint8_t i = 0;
51
        static uint16_t frame_pw = PPM_FRAME;
52
 
53
        //output PPM channels
54
        ++i;
55
 
56
        if(i >= PPM_CHANNELS+1)
57
        {
58
                i                       = 0;
59
                frame_pw        = PPM_FRAME;
60
        }
61
 
62
        if(i < PPM_CHANNELS)
63
        {
64
 
65
                OCR1A            = ppm_ch[i];
66
                frame_pw        -= OCR1A;
67
        }
68
        else
69
        {
70
                OCR1A           = frame_pw;
71
        }
72
 
73
}
74
 
75
 
76
 
77
int main()
78
{
967 - 79
        _delay_ms(500);  //delay for VCC stabilisation
964 - 80
 
81
        ppm_init();
82
 
83
        #if(DEBUG)
84
                uart_init();
85
        #endif
86
 
87
        //************************************************************************
88
        //** init LSM303DLH                                                                                                             **
89
        //************************************************************************
90
        DDRC = 0;                              // all inputs
91
        PORTC = (1 << PORTC4) | (1 << PORTC5); // enable pull-ups on SDA and SCL
92
 
93
 
94
        TWSR = 0;  // clear bit-rate prescale bits
95
        TWBR = 17;
96
 
97
        //enable accelerometer
98
        i2c_start();
99
        i2c_write_byte(0x30); // write acc
100
        i2c_write_byte(0x20); // CTRL_REG1_A
101
        i2c_write_byte(0x27); // normal power mode, 50 Hz data rate, all axes enabled
102
        i2c_stop();
103
 
104
        //enable magnetometer
105
        i2c_start();
106
        i2c_write_byte(0x3C); // write mag
107
        i2c_write_byte(0x02); // MR_REG_M
108
        i2c_write_byte(0x00); // continuous conversion mode
109
        i2c_stop();
110
 
111
        //raw acc and mag
112
        vector a, m;
113
        //filtered acc and mag
114
        vector a_avg, m_avg;
115
 
116
        //IIR filter will overshoot at startup, read in a couple of values to get it stable
117
        for(int i = 0; i<20; i++)       read_data(&a_avg, &m_avg);
118
 
119
        //get zero reference
120
        float heading_start, pitch_start, roll_start;
121
        heading_start   = get_heading(&a_avg, &m_avg, &p);
122
        pitch_start     = get_perpendicular(&a_avg, &down, &p);
123
        roll_start              = get_perpendicular(&a_avg, &down, &right);
124
 
125
        enum calibration_mode mode = CAL_NONE;
126
        vector cal_m_max = {0, 0, 0};
127
        vector cal_m_min = {0, 0, 0};
128
 
129
        while(1)
130
        {
131
 
132
                if (mode == CAL_NONE) // run mode
133
                {
134
 
135
                        int thr_in, thr_out;           
136
 
137
                        read_data(&a_avg, &m_avg);
138
 
967 - 139
                        //calculate servo pulselength from sensor data
964 - 140
                        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);
141
                        int pitch_us    = get_us(get_perpendicular(&a_avg, &down, &p) - pitch_start,            PIT_MIN, PIT_MAX, PIT_US_MIN, PIT_US_MAX);
142
                        int roll_us     = get_us(get_perpendicular(&a_avg, &down, &right) - roll_start,         ROL_MIN, ROL_MAX, ROL_US_MIN, ROL_US_MAX);;    
143
 
144
                        #if(DEBUG)
145
                                printf("H %d P %d R %d \n", heading_us, pitch_us, roll_us);
146
                        #endif
147
 
148
 
149
                        ppm_ch[HDG_CH-1] = heading_us;  //subtract 1, CHs from 1 - N, array bounds from 0 - N-1
150
                        ppm_ch[PIT_CH-1] = pitch_us;
151
                        ppm_ch[ROL_CH-1] = roll_us;
152
                }
153
 
154
 
155
                else // calibration mode - record max/min measurements
156
                {
157
                        //TODO
158
                        //implement calib_test.c
159
 
160
 
161
                        read_data_raw(&a, &m);
162
                        if (m.x < cal_m_min.x) cal_m_min.x = m.x;
163
                        if (m.x > cal_m_max.x) cal_m_max.x = m.x;
164
                        if (m.y < cal_m_min.y) cal_m_min.y = m.y;
165
                        if (m.y > cal_m_max.y) cal_m_max.y = m.y;
166
                        if (m.z < cal_m_min.z) cal_m_min.z = m.z;
167
                        if (m.z > cal_m_max.z) cal_m_max.z = m.z;
168
 
169
 
170
 
171
                }
172
 
173
                _delay_ms(40);
174
        }
175
}  
176