WebSVN - Projects - Diff - Rev 964 and 967


//////////////////////////////////////
// LocoHead - Mathias Kreider, 2011 //                  
//                                                                      //
// headtracker.c                                        //
// I2C, angular and filter functions//          
//////////////////////////////////////
 
#include "vector.h"
#include <math.h>
#include <inttypes.h>
#include <avr/io.h>  
#include <stdlib.h>
 
extern vector m_max;
extern vector m_min;
 
 
void i2c_start() {  
        TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN); // send start condition  
        while (!(TWCR & (1 << TWINT)));  
}  
 
void i2c_write_byte(char byte) {  
        TWDR = byte;              
        TWCR = (1 << TWINT) | (1 << TWEN); // start address transmission  
        while (!(TWCR & (1 << TWINT)));  
}  
 
char i2c_read_byte() {  
        TWCR = (1 << TWINT) | (1 << TWEA) | (1 << TWEN); // start data reception, transmit ACK  
        while (!(TWCR & (1 << TWINT)));  
        return TWDR;  
}  
 
char i2c_read_last_byte() {  
        TWCR = (1 << TWINT) | (1 << TWEN); // start data reception
        while (!(TWCR & (1 << TWINT)));  
        return TWDR;  
}  
 
void i2c_stop() {  
          TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN); // send stop condition  
}  
 
 
// Returns a set of acceleration and raw magnetic readings from the cmp01a.
void read_data_raw(vector *a, vector *m)
{
        // read accelerometer values
        i2c_start();
        i2c_write_byte(0x30); // write acc
        i2c_write_byte(0xa8); // OUT_X_L_A, MSB set to enable auto-increment
        i2c_start();              // repeated start
        i2c_write_byte(0x31); // read acc
        unsigned char axl = i2c_read_byte();
        unsigned char axh = i2c_read_byte();
        unsigned char ayl = i2c_read_byte();
        unsigned char ayh = i2c_read_byte();
        unsigned char azl = i2c_read_byte();
        unsigned char azh = i2c_read_last_byte();
        i2c_stop();
 
        // read magnetometer values
        i2c_start();
        i2c_write_byte(0x3C); // write mag
        i2c_write_byte(0x03); // OUTXH_M
        i2c_start();              // repeated start
        i2c_write_byte(0x3D); // read mag
        unsigned char mxh = i2c_read_byte();
        unsigned char mxl = i2c_read_byte();
        unsigned char myh = i2c_read_byte();
        unsigned char myl = i2c_read_byte();
        unsigned char mzh = i2c_read_byte();
        unsigned char mzl = i2c_read_last_byte();
        i2c_stop();
 
        a->x = axh << 8 | axl;
        a->y = ayh << 8 | ayl;
        a->z = azh << 8 | azl;
        m->x = mxh << 8 | mxl;
        m->y = myh << 8 | myl;
        m->z = mzh << 8 | mzl;
}
 
float IIR2(float x, float* z)
{
 
        //const for butterworth lowpass fc 0.5Hz   
//      const float a[3] = {1.0000,   -1.8521,    0.8623};
//      const float b[3] = {0.0026,    0.0051,    0.0026};
 
        //const for butterworth lowpass fc 2Hz   
        const float a[3] = {1.0000,   -1.4190,    0.5533};
        const float b[3] = {0.0336,    0.0671,    0.0336};
 
 
        float y,r;
 
        r       =       a[1]*z[0]+a[2]*z[1];
        y       =       b[0]*(x-r)+b[1]*z[0]+b[2]*z[1];
        z[1]=   z[0];
        z[0]=   x-r;
 
        return y;
 
}
 
 
//cancels out movemt below threshold while using step sum to 
int thr_filter(int  x, int * x_reg, int * y_reg)
{
        int  y;
        int  diff;
        int  sum = 0;
       
        const int  thr = 4;
        const int  lmt = 5;
 
        diff = x - *x_reg;
 
    if(abs(diff) <= thr)
        {
       sum += diff;
       if(abs(sum) >= lmt)
           {
           sum = 0;
           y = x;
                }
        else y = *y_reg;
        }
        else
        {
        y = x;
        sum = 0;
        }
   
 
        *x_reg = x;
        *y_reg = y;
 
        return y;
}
 
 
// Returns corrected and low-pass filtered magnetometer and accelerometer values
void read_data(vector *a, vector *m)
{
        //interal state buffers for IIR axis filtering
        static float zm_x[2] = {0.0, 0.0};
        static float zm_y[2] = {0.0, 0.0};
        static float zm_z[2] = {0.0, 0.0};
        static float za_x[2] = {0.0, 0.0};
        static float za_y[2] = {0.0, 0.0};
        static float za_z[2] = {0.0, 0.0};
       
 
        read_data_raw(a, m);
       
        //low pass filter acc
        a->x = IIR2(a->x, za_x);
        a->y = IIR2(a->y, za_y);
        a->z = IIR2(a->z, za_z);
 
        //compensate scale and offset, low pass filter mag
        m->x = IIR2(((m->x - m_min.x) / (m_max.x - m_min.x) * 2 - 1.0), zm_x);
        m->y = IIR2(((m->y - m_min.y) / (m_max.y - m_min.y) * 2 - 1.0), zm_y);
        m->z = IIR2(((m->z - m_min.z) / (m_max.z - m_min.z) * 2 - 1.0), zm_z);
}
 
 
 
float get_heading(const vector *a, const vector *m, const vector *p)
{
        vector E;
        vector N;
 
        // cross magnetic vector (magnetic north + inclination) with "down" (acceleration vector) to produce "west"
        // -- right hand rule says
 
        vector_cross(m, a, &E);
        vector_normalize(&E);
 
        // cross "down" with "east" to produce "north" (parallel to the ground)
        vector_cross(a, &E, &N);
        vector_normalize(&N);
 
        // compute heading
       
        float heading = atan2(vector_dot(&E, p), vector_dot(&N, p)) * 180.0 / M_PI;
        return heading;
 
}
 
float get_perpendicular(const vector *a, const vector *d, const vector *q)
{
       
 
        float sign = 0.0;
        vector norma = *a;
 
        if              (q->x == 0.0) {norma.x = 0.0; sign = norma.y;}// cancel out movement on undesired axis
        else if (q->y == 0.0) {norma.y = 0.0; sign = norma.x;} 
        vector_normalize(&norma);
       
 
        // compute angle
        float angle = acos(vector_dot(&norma,d)) * 180.0/M_PI;
        if(sign >= 0.0) angle *= -1;
       
        return angle;
 
}
 
int get_us(float angle, float deg_min, float deg_max, int pwm_min,int pwm_max)
{
        //adjust sign change of angular function to new zero offset
        if(angle < -180.0) angle += 360.0;
        if(angle >= 180.0) angle -= 360.0;
 
        //crop
        if(angle < deg_min) angle  = deg_min;
        else if (angle  > deg_max) angle  = deg_max;
 
        //scale to pwm
        float   ratio = ((float)(pwm_max - pwm_min)) / (deg_max - deg_min);
        int diff = ((int)((angle-deg_min) * ratio));
       
        return pwm_min + diff;
}
 

Rev 964	Rev 967
-		1	//////////////////////////////////////
-		2	// LocoHead - Mathias Kreider, 2011 //
-		3	// //
-		4	// headtracker.c //
-		5	// I2C, angular and filter functions//
-		6	//////////////////////////////////////
-		7
1	#include "vector.h"	8	#include "vector.h"
2	#include <math.h>	9	#include <math.h>
3	#include <inttypes.h>	10	#include <inttypes.h>
4	#include <avr/io.h>	11	#include <avr/io.h>
5	#include <stdlib.h>	12	#include <stdlib.h>
6		13
7	extern vector m_max;	14	extern vector m_max;
8	extern vector m_min;	15	extern vector m_min;
9		16
10		17
11	void i2c_start() {	18	void i2c_start() {
12	TWCR = (1 << TWINT) \| (1 << TWSTA) \| (1 << TWEN); // send start condition	19	TWCR = (1 << TWINT) \| (1 << TWSTA) \| (1 << TWEN); // send start condition
13	while (!(TWCR & (1 << TWINT)));	20	while (!(TWCR & (1 << TWINT)));
14	}	21	}
15		22
16	void i2c_write_byte(char byte) {	23	void i2c_write_byte(char byte) {
17	TWDR = byte;	24	TWDR = byte;
18	TWCR = (1 << TWINT) \| (1 << TWEN); // start address transmission	25	TWCR = (1 << TWINT) \| (1 << TWEN); // start address transmission
19	while (!(TWCR & (1 << TWINT)));	26	while (!(TWCR & (1 << TWINT)));
20	}	27	}
21		28
22	char i2c_read_byte() {	29	char i2c_read_byte() {
23	TWCR = (1 << TWINT) \| (1 << TWEA) \| (1 << TWEN); // start data reception, transmit ACK	30	TWCR = (1 << TWINT) \| (1 << TWEA) \| (1 << TWEN); // start data reception, transmit ACK
24	while (!(TWCR & (1 << TWINT)));	31	while (!(TWCR & (1 << TWINT)));
25	return TWDR;	32	return TWDR;
26	}	33	}
27		34
28	char i2c_read_last_byte() {	35	char i2c_read_last_byte() {
29	TWCR = (1 << TWINT) \| (1 << TWEN); // start data reception	36	TWCR = (1 << TWINT) \| (1 << TWEN); // start data reception
30	while (!(TWCR & (1 << TWINT)));	37	while (!(TWCR & (1 << TWINT)));
31	return TWDR;	38	return TWDR;
32	}	39	}
33		40
34	void i2c_stop() {	41	void i2c_stop() {
35	TWCR = (1 << TWINT) \| (1 << TWSTO) \| (1 << TWEN); // send stop condition	42	TWCR = (1 << TWINT) \| (1 << TWSTO) \| (1 << TWEN); // send stop condition
36	}	43	}
37		44
38		45
39	// Returns a set of acceleration and raw magnetic readings from the cmp01a.	46	// Returns a set of acceleration and raw magnetic readings from the cmp01a.
40	void read_data_raw(vector a, vector m)	47	void read_data_raw(vector a, vector m)
41	{	48	{
42	// read accelerometer values	49	// read accelerometer values
43	i2c_start();	50	i2c_start();
44	i2c_write_byte(0x30); // write acc	51	i2c_write_byte(0x30); // write acc
45	i2c_write_byte(0xa8); // OUT_X_L_A, MSB set to enable auto-increment	52	i2c_write_byte(0xa8); // OUT_X_L_A, MSB set to enable auto-increment
46	i2c_start(); // repeated start	53	i2c_start(); // repeated start
47	i2c_write_byte(0x31); // read acc	54	i2c_write_byte(0x31); // read acc
48	unsigned char axl = i2c_read_byte();	55	unsigned char axl = i2c_read_byte();
49	unsigned char axh = i2c_read_byte();	56	unsigned char axh = i2c_read_byte();
50	unsigned char ayl = i2c_read_byte();	57	unsigned char ayl = i2c_read_byte();
51	unsigned char ayh = i2c_read_byte();	58	unsigned char ayh = i2c_read_byte();
52	unsigned char azl = i2c_read_byte();	59	unsigned char azl = i2c_read_byte();
53	unsigned char azh = i2c_read_last_byte();	60	unsigned char azh = i2c_read_last_byte();
54	i2c_stop();	61	i2c_stop();
55		62
56	// read magnetometer values	63	// read magnetometer values
57	i2c_start();	64	i2c_start();
58	i2c_write_byte(0x3C); // write mag	65	i2c_write_byte(0x3C); // write mag
59	i2c_write_byte(0x03); // OUTXH_M	66	i2c_write_byte(0x03); // OUTXH_M
60	i2c_start(); // repeated start	67	i2c_start(); // repeated start
61	i2c_write_byte(0x3D); // read mag	68	i2c_write_byte(0x3D); // read mag
62	unsigned char mxh = i2c_read_byte();	69	unsigned char mxh = i2c_read_byte();
63	unsigned char mxl = i2c_read_byte();	70	unsigned char mxl = i2c_read_byte();
64	unsigned char myh = i2c_read_byte();	71	unsigned char myh = i2c_read_byte();
65	unsigned char myl = i2c_read_byte();	72	unsigned char myl = i2c_read_byte();
66	unsigned char mzh = i2c_read_byte();	73	unsigned char mzh = i2c_read_byte();
67	unsigned char mzl = i2c_read_last_byte();	74	unsigned char mzl = i2c_read_last_byte();
68	i2c_stop();	75	i2c_stop();
69		76
70	a->x = axh << 8 \| axl;	77	a->x = axh << 8 \| axl;
71	a->y = ayh << 8 \| ayl;	78	a->y = ayh << 8 \| ayl;
72	a->z = azh << 8 \| azl;	79	a->z = azh << 8 \| azl;
73	m->x = mxh << 8 \| mxl;	80	m->x = mxh << 8 \| mxl;
74	m->y = myh << 8 \| myl;	81	m->y = myh << 8 \| myl;
75	m->z = mzh << 8 \| mzl;	82	m->z = mzh << 8 \| mzl;
76	}	83	}
77		84
78	float IIR2(float x, float* z)	85	float IIR2(float x, float* z)
79	{	86	{
80		87
81	//const for butterworth lowpass fc 0.5Hz	88	//const for butterworth lowpass fc 0.5Hz
82	// const float a[3] = {1.0000, -1.8521, 0.8623};	89	// const float a[3] = {1.0000, -1.8521, 0.8623};
83	// const float b[3] = {0.0026, 0.0051, 0.0026};	90	// const float b[3] = {0.0026, 0.0051, 0.0026};
84		91
85	//const for butterworth lowpass fc 2Hz	92	//const for butterworth lowpass fc 2Hz
86	const float a[3] = {1.0000, -1.4190, 0.5533};	93	const float a[3] = {1.0000, -1.4190, 0.5533};
87	const float b[3] = {0.0336, 0.0671, 0.0336};	94	const float b[3] = {0.0336, 0.0671, 0.0336};
88		95
89		96
90	float y,r;	97	float y,r;
91		98
92	r = a[1]z[0]+a[2]z[1];	99	r = a[1]z[0]+a[2]z[1];
93	y = b[0](x-r)+b[1]z[0]+b[2]*z[1];	100	y = b[0](x-r)+b[1]z[0]+b[2]*z[1];
94	z[1]= z[0];	101	z[1]= z[0];
95	z[0]= x-r;	102	z[0]= x-r;
96		103
97	return y;	104	return y;
98		105
99	}	106	}
100		107
101		108
102	//cancels out movemt below threshold while using step sum to	109	//cancels out movemt below threshold while using step sum to
103	int thr_filter(int x, int * x_reg, int * y_reg)	110	int thr_filter(int x, int * x_reg, int * y_reg)
104	{	111	{
105	int y;	112	int y;
106	int diff;	113	int diff;
107	int sum = 0;	114	int sum = 0;
108		115
109	const int thr = 4;	116	const int thr = 4;
110	const int lmt = 5;	117	const int lmt = 5;
111		118
112	diff = x - *x_reg;	119	diff = x - *x_reg;
113		120
114	if(abs(diff) <= thr)	121	if(abs(diff) <= thr)
115	{	122	{
116	sum += diff;	123	sum += diff;
117	if(abs(sum) >= lmt)	124	if(abs(sum) >= lmt)
118	{	125	{
119	sum = 0;	126	sum = 0;
120	y = x;	127	y = x;
121	}	128	}
122	else y = *y_reg;	129	else y = *y_reg;
123	}	130	}
124	else	131	else
125	{	132	{
126	y = x;	133	y = x;
127	sum = 0;	134	sum = 0;
128	}	135	}
129		136
130		137
131	*x_reg = x;	138	*x_reg = x;
132	*y_reg = y;	139	*y_reg = y;
133		140
134	return y;	141	return y;
135	}	142	}
136		143
137		144
138	// Returns corrected and low-pass filtered magnetometer and accelerometer values	145	// Returns corrected and low-pass filtered magnetometer and accelerometer values
139	void read_data(vector a, vector m)	146	void read_data(vector a, vector m)
140	{	147	{
141	//interal state buffers for IIR axis filtering	148	//interal state buffers for IIR axis filtering
142	static float zm_x[2] = {0.0, 0.0};	149	static float zm_x[2] = {0.0, 0.0};
143	static float zm_y[2] = {0.0, 0.0};	150	static float zm_y[2] = {0.0, 0.0};
144	static float zm_z[2] = {0.0, 0.0};	151	static float zm_z[2] = {0.0, 0.0};
145	static float za_x[2] = {0.0, 0.0};	152	static float za_x[2] = {0.0, 0.0};
146	static float za_y[2] = {0.0, 0.0};	153	static float za_y[2] = {0.0, 0.0};
147	static float za_z[2] = {0.0, 0.0};	154	static float za_z[2] = {0.0, 0.0};
148		155
149		156
150	read_data_raw(a, m);	157	read_data_raw(a, m);
151		158
152	//low pass filter acc	159	//low pass filter acc
153	a->x = IIR2(a->x, za_x);	160	a->x = IIR2(a->x, za_x);
154	a->y = IIR2(a->y, za_y);	161	a->y = IIR2(a->y, za_y);
155	a->z = IIR2(a->z, za_z);	162	a->z = IIR2(a->z, za_z);
156		163
157	//compensate scale and offset, low pass filter mag	164	//compensate scale and offset, low pass filter mag
158	m->x = IIR2(((m->x - m_min.x) / (m_max.x - m_min.x) * 2 - 1.0), zm_x);	165	m->x = IIR2(((m->x - m_min.x) / (m_max.x - m_min.x) * 2 - 1.0), zm_x);
159	m->y = IIR2(((m->y - m_min.y) / (m_max.y - m_min.y) * 2 - 1.0), zm_y);	166	m->y = IIR2(((m->y - m_min.y) / (m_max.y - m_min.y) * 2 - 1.0), zm_y);
160	m->z = IIR2(((m->z - m_min.z) / (m_max.z - m_min.z) * 2 - 1.0), zm_z);	167	m->z = IIR2(((m->z - m_min.z) / (m_max.z - m_min.z) * 2 - 1.0), zm_z);
161	}	168	}
162		169
163		170
164		171
165	float get_heading(const vector a, const vector m, const vector *p)	172	float get_heading(const vector a, const vector m, const vector *p)
166	{	173	{
167	vector E;	174	vector E;
168	vector N;	175	vector N;
169		176
170	// cross magnetic vector (magnetic north + inclination) with "down" (acceleration vector) to produce "west"	177	// cross magnetic vector (magnetic north + inclination) with "down" (acceleration vector) to produce "west"
171	// -- right hand rule says	178	// -- right hand rule says
172		179
173	vector_cross(m, a, &E);	180	vector_cross(m, a, &E);
174	vector_normalize(&E);	181	vector_normalize(&E);
175		182
176	// cross "down" with "east" to produce "north" (parallel to the ground)	183	// cross "down" with "east" to produce "north" (parallel to the ground)
177	vector_cross(a, &E, &N);	184	vector_cross(a, &E, &N);
178	vector_normalize(&N);	185	vector_normalize(&N);
179		186
180	// compute heading	187	// compute heading
181		188
182	float heading = atan2(vector_dot(&E, p), vector_dot(&N, p)) * 180.0 / M_PI;	189	float heading = atan2(vector_dot(&E, p), vector_dot(&N, p)) * 180.0 / M_PI;
183	return heading;	190	return heading;
184		191
185	}	192	}
186		193
187	float get_perpendicular(const vector a, const vector d, const vector *q)	194	float get_perpendicular(const vector a, const vector d, const vector *q)
188	{	195	{
189		196
190		197
191	float sign = 0.0;	198	float sign = 0.0;
192	vector norma = *a;	199	vector norma = *a;
193		200
194	if (q->x == 0.0) {norma.x = 0.0; sign = norma.y;}// cancel out movement on undesired axis	201	if (q->x == 0.0) {norma.x = 0.0; sign = norma.y;}// cancel out movement on undesired axis
195	else if (q->y == 0.0) {norma.y = 0.0; sign = norma.x;}	202	else if (q->y == 0.0) {norma.y = 0.0; sign = norma.x;}
196	vector_normalize(&norma);	203	vector_normalize(&norma);
197		204
198		205
199	// compute angle	206	// compute angle
200	float angle = acos(vector_dot(&norma,d)) * 180.0/M_PI;	207	float angle = acos(vector_dot(&norma,d)) * 180.0/M_PI;
201	if(sign >= 0.0) angle *= -1;	208	if(sign >= 0.0) angle *= -1;
202		209
203	return angle;	210	return angle;
204		211
205	}	212	}
206		213
207	int get_us(float angle, float deg_min, float deg_max, int pwm_min,int pwm_max)	214	int get_us(float angle, float deg_min, float deg_max, int pwm_min,int pwm_max)
208	{	215	{
209	//adjust sign change of angular function to new zero offset	216	//adjust sign change of angular function to new zero offset
210	if(angle < -180.0) angle += 360.0;	217	if(angle < -180.0) angle += 360.0;
211	if(angle >= 180.0) angle -= 360.0;	218	if(angle >= 180.0) angle -= 360.0;
212		219
213	//crop	220	//crop
214	if(angle < deg_min) angle = deg_min;	221	if(angle < deg_min) angle = deg_min;
215	else if (angle > deg_max) angle = deg_max;	222	else if (angle > deg_max) angle = deg_max;
216		223
217	//scale to pwm	224	//scale to pwm
218	float ratio = ((float)(pwm_max - pwm_min)) / (deg_max - deg_min);	225	float ratio = ((float)(pwm_max - pwm_min)) / (deg_max - deg_min);
219	int diff = ((int)((angle-deg_min) * ratio));	226	int diff = ((int)((angle-deg_min) * ratio));
220		227
221	return pwm_min + diff;	228	return pwm_min + diff;
222	}	229	}
223		230

Subversion Repositories Projects

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