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