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 |