Subversion Repositories Projects

Rev

Rev 964 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

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