Revision 1826
Added Butterworth filtering code.
trunk/code/projects/libdragonfly/rangefinder.c | ||
---|---|---|
34 | 34 |
**/ |
35 | 35 |
|
36 | 36 |
/* |
37 |
Authors: James Kong and Greg Tress
|
|
37 |
Authors: James Kong, Greg Tress, Emily Hart
|
|
38 | 38 |
|
39 |
Last Modified: 4/30/06 by James |
|
39 |
Last Modified: 5/12/10 by Emily |
|
40 |
- Added Butterworth filtering functions. As of right now, the filter should |
|
41 |
only be used if you really know what you are doing. These functions were |
|
42 |
designed to be used with a task scheduler, which is not currently complete. |
|
43 |
This implementation uses the RTC and runs massive computations in the RTC |
|
44 |
interrupt. |
|
45 |
|
|
46 |
Modified: 4/30/06 by James |
|
40 | 47 |
-Started log_distance conversion function !!!NOT COMPLETE!!! |
41 | 48 |
-Cleaning up comments |
42 | 49 |
|
... | ... | |
45 | 52 |
Using Sharp GP2D02 IR Rangefinder |
46 | 53 |
|
47 | 54 |
Vin is the input to the rangefinder, designated RANGE_CTRL. |
48 |
Vout is the output from the rangefinder, designated RANGE_IN# where # is the rangefinder you are reading from |
|
55 |
Vout is the output from the rangefinder, designated RANGE_IN# where # is the |
|
56 |
rangefinder you are reading from |
|
49 | 57 |
|
50 | 58 |
Expected Initial Conditions: |
51 | 59 |
Vin is high and Vout should read high. |
... | ... | |
58 | 66 |
|
59 | 67 |
*/ |
60 | 68 |
#include <avr/pgmspace.h> |
69 |
#include <avr/interrupt.h> |
|
61 | 70 |
|
62 | 71 |
#include "rangefinder.h" |
63 | 72 |
#include "analog.h" |
64 | 73 |
#include "dio.h" |
74 |
#include "time.h" |
|
65 | 75 |
|
66 | 76 |
/* |
67 | 77 |
read_distance returns the 8-bit reading from the rangefinder |
... | ... | |
96 | 106 |
81,80,79,78 |
97 | 107 |
}; |
98 | 108 |
|
109 |
/* 1 if the filter is enabled, else 0 */ |
|
110 |
static int use_filter; |
|
111 |
// VALUE - MIN_IR_LINEAR is stored so only 8 bits are needed |
|
112 |
/* X values for the Butterworth filter, for each rangefinder */ |
|
113 |
static uint8_t butter_x[5][4]; |
|
114 |
/* Y values for the Butterworth filter, for each rangefinder */ |
|
115 |
static uint8_t butter_y[5][3]; |
|
116 |
/* How many consecutive -1s have been seen by each rangefinder */ |
|
117 |
static uint8_t neg_one_count[5]; |
|
118 |
|
|
99 | 119 |
/** |
100 | 120 |
* @defgroup rangefinder Rangefinder |
101 | 121 |
* @brief Functions for using the IR rangefinders |
... | ... | |
107 | 127 |
|
108 | 128 |
/** |
109 | 129 |
* Initializes the rangefinders. This must be called before |
110 |
* range_read_distance. |
|
130 |
* range_read_distance. This function does not initialize the filter.
|
|
111 | 131 |
* |
112 | 132 |
* @see range_read_distance |
113 | 133 |
**/ |
114 | 134 |
void range_init(void) |
115 | 135 |
{ |
116 | 136 |
digital_output(_PIN_B4,0); |
137 |
use_filter = 0; |
|
117 | 138 |
} |
118 | 139 |
|
119 | 140 |
/** |
141 |
* Initializes the rangefinders with an option to enable the Butterworth |
|
142 |
* filtering. |
|
143 |
* |
|
144 |
* As of 5/12/2010, the filter should only be used if you really know what you |
|
145 |
* are doing. It was designed to be used with a task scheduler, which is not |
|
146 |
* currently complete. This implementation uses the RTC and runs massive |
|
147 |
* computations in the RTC interrupt. |
|
148 |
* |
|
149 |
* @param filter 1 to enable the filter, 0 to leave it turned off |
|
150 |
**/ |
|
151 |
void range_init_filter(int filter) |
|
152 |
{ |
|
153 |
range_init(); |
|
154 |
if(filter){ |
|
155 |
use_filter = 1; |
|
156 |
butter_init(); |
|
157 |
} |
|
158 |
} |
|
159 |
|
|
160 |
/** |
|
120 | 161 |
* Reads the distance measured by one of the rangefinders. |
121 | 162 |
* This distance is in arbitrary units. |
122 | 163 |
* |
... | ... | |
151 | 192 |
} |
152 | 193 |
} |
153 | 194 |
|
195 |
/** |
|
196 |
* Initializes the butterworth filter. |
|
197 |
**/ |
|
198 |
void butter_init(void) |
|
199 |
{ |
|
200 |
int i; |
|
201 |
// init -1 count to 3 for each rangefinder |
|
202 |
// this will cause them to restart the filter |
|
203 |
for(i=0; i<5; i++) |
|
204 |
{ |
|
205 |
neg_one_count[i] = 3; |
|
206 |
} |
|
207 |
|
|
208 |
// set up the rtc to run butter_task periodically |
|
209 |
rtc_init(SIXTEENTH_SECOND, butter_task); |
|
210 |
} |
|
211 |
|
|
212 |
/* |
|
213 |
* Reads each rangefinder and sends its value through the Butterworth filter. |
|
214 |
* This task should be run frequently so that range_read_filtered_distance |
|
215 |
* returns fresh values |
|
216 |
*/ |
|
217 |
void butter_task(void) |
|
218 |
{ |
|
219 |
butter_filter(IR1, range_read_distance(IR1)); |
|
220 |
butter_filter(IR2, range_read_distance(IR2)); |
|
221 |
butter_filter(IR3, range_read_distance(IR3)); |
|
222 |
butter_filter(IR4, range_read_distance(IR4)); |
|
223 |
butter_filter(IR5, range_read_distance(IR5)); |
|
224 |
} |
|
225 |
|
|
226 |
/* |
|
227 |
* Butterworth helper function that takes a rangefinder ID and turns it into |
|
228 |
* an array index between 0 and 4. |
|
229 |
*/ |
|
230 |
int get_range_index(int range_id){ |
|
231 |
switch(range_id){ |
|
232 |
case IR1: |
|
233 |
return 0; |
|
234 |
case IR2: |
|
235 |
return 1; |
|
236 |
case IR3: |
|
237 |
return 2; |
|
238 |
case IR4: |
|
239 |
return 3; |
|
240 |
case IR5: |
|
241 |
return 4; |
|
242 |
default: // should never happen |
|
243 |
return 0xff; |
|
244 |
} |
|
245 |
} |
|
246 |
|
|
247 |
/** |
|
248 |
* Puts the given value from the given rangefinder through the Butterworth |
|
249 |
* filter. This function should be called every time a new value is read from a |
|
250 |
* rangefinder. |
|
251 |
* |
|
252 |
* The Butterworth filter has a cutoff frequency of 5Hz and an order of 3 |
|
253 |
* |
|
254 |
* @param range_id the rangefinder to use. This should be one of the constants |
|
255 |
* IR1 - IR5. |
|
256 |
* @param the value read from that rangefinder |
|
257 |
**/ |
|
258 |
void butter_filter(int range_id, int val) |
|
259 |
{ |
|
260 |
int range_index = get_range_index(range_id); |
|
261 |
// we have a non-error value |
|
262 |
if(val > -1 && val <= MAX_IR_LINEAR) |
|
263 |
{ |
|
264 |
// we just passed two or fewer -1's: act as though none seen |
|
265 |
if(neg_one_count[range_index] < 3) |
|
266 |
{ |
|
267 |
// shift the values of the arrays to the left |
|
268 |
int i; |
|
269 |
for(i=0; i<2; i++) |
|
270 |
{ |
|
271 |
butter_x[range_index][i] = butter_x[range_index][i+1]; |
|
272 |
butter_y[range_index][i] = butter_y[range_index][i+1]; |
|
273 |
} |
|
274 |
butter_x[range_index][2] = butter_x[range_index][3]; |
|
275 |
// add the new value to the X array |
|
276 |
butter_x[range_index][3] = val - MIN_IR_LINEAR; |
|
277 |
} |
|
278 |
// we just passed three or more -1 values: reset filter with new value |
|
279 |
else |
|
280 |
{ |
|
281 |
int i; |
|
282 |
// fill x and y values with the new value |
|
283 |
for(i=0; i<3; i++) |
|
284 |
{ |
|
285 |
butter_x[range_index][i] = val - MIN_IR_LINEAR; |
|
286 |
butter_y[range_index][i] = val - MIN_IR_LINEAR; |
|
287 |
} |
|
288 |
butter_x[range_index][3] = val - MIN_IR_LINEAR; |
|
289 |
} |
|
290 |
|
|
291 |
// reset the -1 count value |
|
292 |
neg_one_count[range_index] = 0; |
|
293 |
|
|
294 |
/* |
|
295 |
* butterworth filter the last values |
|
296 |
* |
|
297 |
* butterworth filter equation is |
|
298 |
* y(t) = x(t-3)/6 + x(t-2)/2 + x(t-1)/2 + x(t)/6 - y(t-2)/3 |
|
299 |
* |
|
300 |
* values are multiplied by 16 before divisions, and divided by 16 at the |
|
301 |
* very end to mitigate rounding errors |
|
302 |
* |
|
303 |
* 8 is added before dividing by 16 so that numbers are rounded instead of |
|
304 |
* truncated |
|
305 |
*/ |
|
306 |
int16_t temp1 = (int16_t)butter_x[range_index][3] + |
|
307 |
(int16_t)butter_x[range_index][0] + (2*MIN_IR_LINEAR); |
|
308 |
int16_t temp2 = (int16_t)butter_x[range_index][2] + |
|
309 |
(int16_t)butter_x[range_index][1] + (2*MIN_IR_LINEAR); |
|
310 |
int16_t temp3 = (int16_t)butter_y[range_index][0] + MIN_IR_LINEAR; |
|
311 |
int16_t filtered_big = ((((temp1*8)-(temp3*16))/3)+(temp2*8)+8)/16; |
|
312 |
filtered_big -= MIN_IR_LINEAR; |
|
313 |
uint8_t filtered = filtered_big > 0xff ? 0xff : (uint8_t)filtered_big; |
|
314 |
|
|
315 |
butter_y[range_index][2] = filtered; |
|
316 |
} |
|
317 |
// -1 seen - don't want to store it |
|
318 |
else |
|
319 |
{ |
|
320 |
// increment -1 count, preventing overflow |
|
321 |
neg_one_count[range_index] = neg_one_count[range_index] == 0xff ? 0xff : |
|
322 |
neg_one_count[range_index]+1; |
|
323 |
} |
|
324 |
} |
|
325 |
|
|
326 |
/** |
|
327 |
* Returns the most recent filtered reading of the rangefinder. The raw |
|
328 |
* rangefinder values have been run through a Butterworth filter. |
|
329 |
* |
|
330 |
* If the filter was not initialized in rangefinder_init, will return the |
|
331 |
* unfiltered value from the rangefinder. |
|
332 |
* |
|
333 |
* @param range_id the rangefinder to use. This should be one of the constants |
|
334 |
* IR1 - IR5. |
|
335 |
**/ |
|
336 |
int range_read_filtered_distance(int range_id){ |
|
337 |
if(!use_filter){ |
|
338 |
return range_read_distance(range_id); |
|
339 |
} |
|
340 |
|
|
341 |
int range_index = get_range_index(range_id); |
|
342 |
|
|
343 |
// haven't seen too many -1s recently - return filtered value |
|
344 |
if(neg_one_count[range_index] < 3) |
|
345 |
{ |
|
346 |
return butter_y[range_index][2] + MIN_IR_LINEAR; |
|
347 |
} |
|
348 |
// have seen several -1s - return -1 |
|
349 |
return -1; |
|
350 |
} |
|
351 |
|
|
154 | 352 |
/** @} **/ //end defgroup |
trunk/code/projects/libdragonfly/rangefinder.h | ||
---|---|---|
56 | 56 |
#define MIN_IR_ADC8 27 |
57 | 57 |
/** @brief largest meaningful rangefinder reading (logarithmic scale) **/ |
58 | 58 |
#define MAX_IR_ADC8 98 |
59 |
/** @brief smallest meaningful rangefinder reading (linear scale) **/ |
|
60 |
#define MIN_IR_LINEAR 78 |
|
61 |
/** @brief largest meaningful rangefinder reading (linear scale) **/ |
|
62 |
#define MAX_IR_LINEAR 327 |
|
59 | 63 |
|
60 | 64 |
/** @brief Initialize the rangefinders **/ |
61 | 65 |
void range_init(void); |
66 |
/** @brief Initialize the rangefinders with option to use filtering **/ |
|
67 |
void range_init_filter(int filter); |
|
62 | 68 |
/** @brief Read the distance from a rangefinder **/ |
63 | 69 |
int range_read_distance(int range_id); |
64 | 70 |
/** @brief Convert logarithmic-scale distance readings to a linear scale **/ |
65 | 71 |
int linearize_distance(int value); |
72 |
/** @brief Task that reads rangefinders and sends the values to the filter **/ |
|
73 |
void butter_task(void); |
|
74 |
/** @brief Initialize the butterworth filter **/ |
|
75 |
void butter_init(void); |
|
76 |
/** @brief Puts a raw rangefinder value through the Butterworth filter **/ |
|
77 |
void butter_filter(int range_id, int16_t val); |
|
78 |
/** @brief Returns the most recent filtered reading of the rangefinder **/ |
|
79 |
int range_read_filtered_distance(int range_id); |
|
66 | 80 |
|
67 | 81 |
/** @} **/ //end addtogroup |
68 | 82 |
|
Also available in: Unified diff