Subversion Repositories Projects

Rev

Go to most recent revision | Details | Last modification | View Log | RSS feed

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