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 8bit 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 nonerror 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(t3)/6 + x(t2)/2 + x(t1)/2 + x(t)/6  y(t2)/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 logarithmicscale 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