WebSVN - Projects - Diff - Rev 964 and 967


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

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

Subversion Repositories Projects

(root)/LoCoHead/main.c - Rev 964 → 967