aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_RangeFinder - bug fix for cases when analog value is zero.

Browse Source 
- addition of filter call which averages the past 4 values

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1224 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
rmackay9@yahoo.com 2010-12-21 12:34:24 +00:00
parent 24bb547eae
commit c2db64116a
6 changed files with 94 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp
View File

@ -28,12 +28,17 @@
// AVR LibC Includes
#include "WConstants.h"
#include "AP_RangeFinder_MaxsonarXL.h"
// Public Methods //////////////////////////////////////////////////////////////
void AP_RangeFinder_MaxsonarXL::init(int analogPort)
{
// local variables
int i;
// set the given analog port to an input
pinMode(analogPort, INPUT);
// initialise everything
_analogPort = analogPort;
max_distance = AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE;
@ -41,6 +46,10 @@ void AP_RangeFinder_MaxsonarXL::init(int analogPort)
// make first call to read to get initial distance
read();
// initialise history
for( i=0; i<AP_RANGEFINDER_NUM_AVERAGES; i++ )
_history[i] = distance;
}
// Read Sensor data
@ -52,17 +61,6 @@ int AP_RangeFinder_MaxsonarXL::read()
// for this sensor, the sensor value is the distance in cm! nice and easy!
distance = constrain(raw_value,min_distance,max_distance);
// implement filter
//switch( _filterType ) {
// case AP_RANGEFINDER_FILTER_LIMITED_CHANGE:
// distance = constrain(
// break;
// case AP_RANGEFINDER_FILTER_NONE:
// default:
// distance = tempDistance;
// break;
//}
// return distance
return distance;
return filter(distance);
}

29
libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.cpp
View File

@ -28,12 +28,17 @@
// AVR LibC Includes
#include "WConstants.h"
#include "AP_RangeFinder_SharpGP2Y.h"
// Public Methods //////////////////////////////////////////////////////////////
void AP_RangeFinder_SharpGP2Y::init(int analogPort)
{
// local variables
int i;
// set the given analog port to an input
pinMode(analogPort, INPUT);
// initialise everything
_analogPort = analogPort;
max_distance = AP_RANGEFINDER_SHARPEGP2Y_MAX_DISTANCE;
@ -41,6 +46,10 @@ void AP_RangeFinder_SharpGP2Y::init(int analogPort)
// make first call to read to get initial distance
read();
// initialise history
for( i=0; i<AP_RANGEFINDER_NUM_AVERAGES; i++ )
_history[i] = distance;
}
// Read Sensor data
@ -48,19 +57,11 @@ int AP_RangeFinder_SharpGP2Y::read()
{
// read raw sensor value and convert to distance
raw_value = analogRead(_analogPort);
distance = constrain(14500/raw_value,min_distance,max_distance);
// implement filter
//switch( _filterType ) {
// case AP_RANGEFINDER_FILTER_LIMITED_CHANGE:
// distance = constrain(
// break;
// case AP_RANGEFINDER_FILTER_NONE:
// default:
// distance = tempDistance;
// break;
//}
if( raw_value == 0 )
distance = max_distance;
else
distance = constrain(14500/raw_value,min_distance,max_distance);
// return distance
return distance;
return filter(distance);
}

37
libraries/AP_RangeFinder/RangeFinder.cpp Normal file
View File

@ -0,0 +1,37 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*-
/*
AP_RangeFinder.cpp - Arduino Library for Sharpe GP2Y0A02YK0F
infrared proximity sensor
Code by Jose Julio and Randy Mackay. DIYDrones.com
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This has the basic functions that all RangeFinders need implemented
*/
// AVR LibC Includes
#include "WConstants.h"
#include "RangeFinder.h"
// Public Methods //////////////////////////////////////////////////////////////
void RangeFinder::set_orientation(int x, int y, int z)
{
orientation_x = x;
orientation_y = y;
orientation_z = z;
}
// Protected Methods //////////////////////////////////////////////////////////
int RangeFinder::filter(int latestValue)
{
int i;
int total = 0;
_history_ptr = (_history_ptr + 1) % AP_RANGEFINDER_NUM_AVERAGES;
_history[_history_ptr] = latestValue;
for(i=0; i<AP_RANGEFINDER_NUM_AVERAGES; i++ )
total += _history[i];
return total / AP_RANGEFINDER_NUM_AVERAGES;
}

24
libraries/AP_RangeFinder/RangeFinder.h
View File

@ -3,22 +3,36 @@
#include <inttypes.h>
//#define AP_RANGEFINDER_FILTER_NONE 0
//#define AP_RANGEFINDER_FILTER_LIMITED_CHANGE 1
#define AP_RANGEFINDER_ORIENTATION_FRONT 0, 10, 0
#define AP_RANGEFINDER_ORIENTATION_RIGHT -10, 0, 0
#define AP_RANGEFINDER_ORIENTATION_BACK 0,-10, 0
#define AP_RANGEFINDER_ORIENTATION_LEFT 10, 0, 0
#define AP_RANGEFINDER_ORIENTATION_UP 0, 0,-10
#define AP_RANGEFINDER_ORIENTATION_DOWN 0, 0, 10
#define AP_RANGEFINDER_ORIENTATION_FRONT_RIGHT -5, -5, 0
#define AP_RANGEFINDER_ORIENTATION_BACK_RIGHT -5, -5, 0
#define AP_RANGEFINDER_ORIENTATION_BACK_LEFT 5, -5, 0
#define AP_RANGEFINDER_ORIENTATION_FRONT_LEFT 5, 5, 0
#define AP_RANGEFINDER_NUM_AVERAGES 4
class RangeFinder
{
protected:
public:
int _analogPort; // the port to which the sensor is connected
int _filterType;
int _history_ptr;
int _history[AP_RANGEFINDER_NUM_AVERAGES];
public:
int raw_value; // raw value from the sensor
int distance; // distance in cm
int max_distance; // maximum measurable distance (in cm)
int min_distance; // minimum measurable distance (in cm)
int orientation_x, orientation_y, orientation_z;
int filter(int latestValue); // returns the average of the last AP_RANGEFINDER_NUM_AVERAGES values
virtual void init(int analogPort);
//virtual void set_filter(int filterType) { _filterType = filterType; };
virtual void set_orientation(int x, int y, int z);
virtual int read(); // read value from analog port and return distance in cm
};
#endif

13
libraries/AP_RangeFinder/examples/AP_RangeFinder_test/AP_RangeFinder_test.pde
View File

@ -3,13 +3,13 @@
Code by DIYDrones.com
*/
#include <AP_RangeFinder.h> // Sonar library
#include <AP_RangeFinder.h> // Range finder library
#define RF_PIN 5 // the far back-right pin on the oilpan (near the CLI switch)
#define RF_PIN A5 // the far back-right pin on the oilpan (near the CLI switch)
// create the range finder object
//AP_RangeFinder_SharpGP2Y aRF;
AP_RangeFinder_MaxsonarXL aRF;
AP_RangeFinder_SharpGP2Y aRF;
//AP_RangeFinder_MaxsonarXL aRF;
void setup()
{
@ -20,11 +20,12 @@ void setup()
void loop()
{
int i = 0;
Serial.print("dist:");
Serial.print(aRF.read());
Serial.print("\traw:");
Serial.print(aRF.raw_value);
Serial.print(aRF.raw_value);
Serial.println();
delay(100);
delay(50);
}

5
libraries/AP_RangeFinder/keywords.txt
View File

@ -4,7 +4,12 @@ AP_RangeFinder_SharpGP2Y KEYWORD1
AP_RangeFinder_MaxsonarXL KEYWORD1
init KEYWORD2
read KEYWORD2
filter KEYWORD2
set_orientation KEYWORD2
raw_value KEYWORD2
distance KEYWORD2
max_distance KEYWORD2
min_distance KEYWORD2
orientation_x KEYWORD2
orientation_y KEYWORD2
orientation_z KEYWORD2