diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp index 492fc9a2c3..3e8b6019f1 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp @@ -1,65 +1,65 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*- -/* - AP_RangeFinder_MaxsonarXL.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. - - Sparkfun URL: http://www.sparkfun.com/products/9491 - datasheet: http://www.sparkfun.com/datasheets/Sensors/Proximity/XL-EZ0-Datasheet.pdf - - Sensor should be connected to one of the analog ports - - Variables: - 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) - - Methods: - read() : read value from analog port and returns the distance (in cm) - -*/ - -// AVR LibC Includes -#include "WConstants.h" -#include "AP_RangeFinder_MaxsonarXL.h" - -// Constructor ////////////////////////////////////////////////////////////// - -AP_RangeFinder_MaxsonarXL::AP_RangeFinder_MaxsonarXL(AP_AnalogSource *source, - ModeFilter *filter) : - RangeFinder(source, filter), _scaler(AP_RANGEFINDER_MAXSONARXL_SCALER) -{ - max_distance = AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE; - min_distance = AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE; -} - -// Public Methods ////////////////////////////////////////////////////////////// -float AP_RangeFinder_MaxsonarXL::calculate_scaler(int sonar_type, float adc_refence_voltage) -{ - float type_scaler = 1.0; - switch(sonar_type) { - case AP_RANGEFINDER_MAXSONARXL: - type_scaler = AP_RANGEFINDER_MAXSONARXL_SCALER; - min_distance = AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE; - max_distance = AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE; - break; - case AP_RANGEFINDER_MAXSONARLV: - type_scaler = AP_RANGEFINDER_MAXSONARLV_SCALER; - min_distance = AP_RANGEFINDER_MAXSONARLV_MIN_DISTANCE; - max_distance = AP_RANGEFINDER_MAXSONARLV_MAX_DISTANCE; - break; - case AP_RANGEFINDER_MAXSONARXLL: - type_scaler = AP_RANGEFINDER_MAXSONARXLL_SCALER; - min_distance = AP_RANGEFINDER_MAXSONARXLL_MIN_DISTANCE; - max_distance = AP_RANGEFINDER_MAXSONARXLL_MAX_DISTANCE; - break; - } - _scaler = type_scaler * adc_refence_voltage / 5.0; - return _scaler; +// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*- +/* + AP_RangeFinder_MaxsonarXL.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. + + Sparkfun URL: http://www.sparkfun.com/products/9491 + datasheet: http://www.sparkfun.com/datasheets/Sensors/Proximity/XL-EZ0-Datasheet.pdf + + Sensor should be connected to one of the analog ports + + Variables: + 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) + + Methods: + read() : read value from analog port and returns the distance (in cm) + +*/ + +// AVR LibC Includes +#include "WConstants.h" +#include "AP_RangeFinder_MaxsonarXL.h" + +// Constructor ////////////////////////////////////////////////////////////// + +AP_RangeFinder_MaxsonarXL::AP_RangeFinder_MaxsonarXL(AP_AnalogSource *source, ModeFilter *filter): + RangeFinder(source, filter), + _scaler(AP_RANGEFINDER_MAXSONARXL_SCALER) + { + max_distance = AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE; + min_distance = AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE; + } + +// Public Methods ////////////////////////////////////////////////////////////// +float AP_RangeFinder_MaxsonarXL::calculate_scaler(int sonar_type, float adc_refence_voltage) +{ + float type_scaler = 1.0; + switch(sonar_type) { + case AP_RANGEFINDER_MAXSONARXL: + type_scaler = AP_RANGEFINDER_MAXSONARXL_SCALER; + min_distance = AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE; + max_distance = AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE; + break; + case AP_RANGEFINDER_MAXSONARLV: + type_scaler = AP_RANGEFINDER_MAXSONARLV_SCALER; + min_distance = AP_RANGEFINDER_MAXSONARLV_MIN_DISTANCE; + max_distance = AP_RANGEFINDER_MAXSONARLV_MAX_DISTANCE; + break; + case AP_RANGEFINDER_MAXSONARXLL: + type_scaler = AP_RANGEFINDER_MAXSONARXLL_SCALER; + min_distance = AP_RANGEFINDER_MAXSONARXLL_MIN_DISTANCE; + max_distance = AP_RANGEFINDER_MAXSONARXLL_MAX_DISTANCE; + break; + } + _scaler = type_scaler * adc_refence_voltage / 5.0; + return _scaler; } \ No newline at end of file diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h index a589e987a5..12780a6dfc 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h @@ -1,33 +1,34 @@ -#ifndef AP_RangeFinder_MaxsonarXL_H -#define AP_RangeFinder_MaxsonarXL_H - -#include "RangeFinder.h" - -// XL-EZ0 (aka XL) -#define AP_RANGEFINDER_MAXSONARXL 0 -#define AP_RANGEFINDER_MAXSONARXL_SCALER 1.0 -#define AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE 20 -#define AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE 765 - -// LV-EZ0 (aka LV) -#define AP_RANGEFINDER_MAXSONARLV 1 -#define AP_RANGEFINDER_MAXSONARLV_SCALER (2.54/2.0) -#define AP_RANGEFINDER_MAXSONARLV_MIN_DISTANCE 15 -#define AP_RANGEFINDER_MAXSONARLV_MAX_DISTANCE 645 - -// XL-EZL0 (aka XLL) -#define AP_RANGEFINDER_MAXSONARXLL 2 -#define AP_RANGEFINDER_MAXSONARXLL_SCALER 2.0 -#define AP_RANGEFINDER_MAXSONARXLL_MIN_DISTANCE 20 -#define AP_RANGEFINDER_MAXSONARXLL_MAX_DISTANCE 1068 - -class AP_RangeFinder_MaxsonarXL : public RangeFinder -{ - public: - AP_RangeFinder_MaxsonarXL(AP_AnalogSource *source, ModeFilter *filter); - int convert_raw_to_distance(int _raw_value) { return _raw_value * _scaler; } // read value from analog port and return distance in cm - float calculate_scaler(int sonar_type, float adc_refence_voltage); - private: - float _scaler; // used to account for different sonar types -}; -#endif +#ifndef AP_RangeFinder_MaxsonarXL_H +#define AP_RangeFinder_MaxsonarXL_H + +#include "RangeFinder.h" + +// XL-EZ0 (aka XL) +#define AP_RANGEFINDER_MAXSONARXL 0 +#define AP_RANGEFINDER_MAXSONARXL_SCALER 1.0 +#define AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE 20 +#define AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE 765 + +// LV-EZ0 (aka LV) +#define AP_RANGEFINDER_MAXSONARLV 1 +#define AP_RANGEFINDER_MAXSONARLV_SCALER (2.54/2.0) +#define AP_RANGEFINDER_MAXSONARLV_MIN_DISTANCE 15 +#define AP_RANGEFINDER_MAXSONARLV_MAX_DISTANCE 645 + +// XL-EZL0 (aka XLL) +#define AP_RANGEFINDER_MAXSONARXLL 2 +#define AP_RANGEFINDER_MAXSONARXLL_SCALER 2.0 +#define AP_RANGEFINDER_MAXSONARXLL_MIN_DISTANCE 20 +#define AP_RANGEFINDER_MAXSONARXLL_MAX_DISTANCE 1068 + +class AP_RangeFinder_MaxsonarXL : public RangeFinder +{ + public: + AP_RangeFinder_MaxsonarXL(AP_AnalogSource *source, ModeFilter *filter); + int convert_raw_to_distance(int _raw_value) { return _raw_value * _scaler; } // read value from analog port and return distance in cm + float calculate_scaler(int sonar_type, float adc_refence_voltage); + + private: + float _scaler; // used to account for different sonar types +}; +#endif diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.cpp index 61f4e7d2ea..bb8a20c896 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.cpp @@ -1,42 +1,41 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*- -/* - AP_RangeFinder_SharpGP2Y.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. - - Sensor should be conected to one of the analog ports - - Sparkfun URL: http://www.sparkfun.com/products/8958 - datasheet: http://www.sparkfun.com/datasheets/Sensors/Infrared/gp2y0a02yk_e.pdf - - Variables: - 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) - - Methods: - read() : read value from analog port - -*/ - -// AVR LibC Includes -#include "WConstants.h" -#include "AP_RangeFinder_SharpGP2Y.h" - -// Constructor ////////////////////////////////////////////////////////////// - -AP_RangeFinder_SharpGP2Y::AP_RangeFinder_SharpGP2Y(AP_AnalogSource *source, - ModeFilter *filter) : +// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*- +/* + AP_RangeFinder_SharpGP2Y.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. + + Sensor should be conected to one of the analog ports + + Sparkfun URL: http://www.sparkfun.com/products/8958 + datasheet: http://www.sparkfun.com/datasheets/Sensors/Infrared/gp2y0a02yk_e.pdf + + Variables: + 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) + + Methods: + read() : read value from analog port + +*/ + +// AVR LibC Includes +#include "WConstants.h" +#include "AP_RangeFinder_SharpGP2Y.h" + +// Constructor ////////////////////////////////////////////////////////////// + +AP_RangeFinder_SharpGP2Y::AP_RangeFinder_SharpGP2Y(AP_AnalogSource *source, ModeFilter *filter) : RangeFinder(source, filter) -{ - max_distance = AP_RANGEFINDER_SHARPEGP2Y_MAX_DISTANCE; - min_distance = AP_RANGEFINDER_SHARPEGP2Y_MIN_DISTANCE; -} - -// Public Methods ////////////////////////////////////////////////////////////// +{ + max_distance = AP_RANGEFINDER_SHARPEGP2Y_MAX_DISTANCE; + min_distance = AP_RANGEFINDER_SHARPEGP2Y_MIN_DISTANCE; +} + +// Public Methods ////////////////////////////////////////////////////////////// diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.h b/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.h index 8798857945..4f5a328979 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.h @@ -1,16 +1,21 @@ -#ifndef AP_RangeFinder_SharpGP2Y_H -#define AP_RangeFinder_SharpGP2Y_H - -#include "RangeFinder.h" - -#define AP_RANGEFINDER_SHARPEGP2Y_MIN_DISTANCE 20 -#define AP_RANGEFINDER_SHARPEGP2Y_MAX_DISTANCE 150 - -class AP_RangeFinder_SharpGP2Y : public RangeFinder -{ - public: +#ifndef AP_RangeFinder_SharpGP2Y_H +#define AP_RangeFinder_SharpGP2Y_H + +#include "RangeFinder.h" + +#define AP_RANGEFINDER_SHARPEGP2Y_MIN_DISTANCE 20 +#define AP_RANGEFINDER_SHARPEGP2Y_MAX_DISTANCE 150 + +class AP_RangeFinder_SharpGP2Y : public RangeFinder +{ + public: AP_RangeFinder_SharpGP2Y(AP_AnalogSource *source, ModeFilter *filter); - int convert_raw_to_distance(int _raw_value) { if( _raw_value == 0 ) return max_distance; else return 14500/_raw_value; } // read value from analog port and return distance in cm - -}; -#endif + int convert_raw_to_distance(int _raw_value) { + if( _raw_value == 0 ) + return max_distance; + else + return 14500/_raw_value; + } // read value from analog port and return distance in cm + +}; +#endif diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index 962148eb1c..582e072d60 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/libraries/AP_RangeFinder/RangeFinder.cpp @@ -1,46 +1,45 @@ -// -*- 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; -} - -// Read Sensor data - only the raw_value is filled in by this parent class -int RangeFinder::read() -{ - int temp_dist; - +// -*- 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; +} + +// Read Sensor data - only the raw_value is filled in by this parent class +int RangeFinder::read() +{ + int temp_dist; + raw_value = _analog_source->read(); - - // convert analog value to distance in cm (using child implementation most likely) - temp_dist = convert_raw_to_distance(raw_value); - - // ensure distance is within min and max - temp_dist = constrain(temp_dist, min_distance, max_distance); - - distance = _mode_filter->get_filtered_with_sample(temp_dist); - return distance; -} - + // convert analog value to distance in cm (using child implementation most likely) + temp_dist = convert_raw_to_distance(raw_value); + + // ensure distance is within min and max + temp_dist = constrain(temp_dist, min_distance, max_distance); + + distance = _mode_filter->get_filtered_with_sample(temp_dist); + return distance; +} + diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h index 64b38e7e89..1870cd5a94 100644 --- a/libraries/AP_RangeFinder/RangeFinder.h +++ b/libraries/AP_RangeFinder/RangeFinder.h @@ -1,44 +1,43 @@ -#ifndef RangeFinder_h -#define RangeFinder_h - -#include -#include +#ifndef RangeFinder_h +#define RangeFinder_h + +#include +#include #include "../AP_AnalogSource/AP_AnalogSource.h" -#include "../ModeFilter/ModeFilter.h" // ArduPilot Mega RC Library - -/* -#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 -*/ - -class RangeFinder -{ - protected: +#include "../ModeFilter/ModeFilter.h" // ArduPilot Mega RC Library + +/* +#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 +*/ + +class RangeFinder +{ + protected: RangeFinder(AP_AnalogSource * source, ModeFilter *filter) : _analog_source(source), - _mode_filter(filter) - {} - public: - - int raw_value; // raw value from the sensor - int distance; // distance in cm - int max_distance; // maximum measurable distance (in cm) - should be set in child's constructor - int min_distance; // minimum measurable distance (in cm) - should be set in child's constructor - int orientation_x, orientation_y, orientation_z; - - virtual void set_orientation(int x, int y, int z); - virtual int convert_raw_to_distance(int _raw_value) { return _raw_value; } // function that each child class should override to convert voltage to distance - virtual int read(); // read value from sensor and return distance in cm - + _mode_filter(filter) {} + public: + + int raw_value; // raw value from the sensor + int distance; // distance in cm + int max_distance; // maximum measurable distance (in cm) - should be set in child's constructor + int min_distance; // minimum measurable distance (in cm) - should be set in child's constructor + int orientation_x, orientation_y, orientation_z; + + virtual void set_orientation(int x, int y, int z); + virtual int convert_raw_to_distance(int _raw_value) { return _raw_value; } // function that each child class should override to convert voltage to distance + virtual int read(); // read value from sensor and return distance in cm + AP_AnalogSource *_analog_source; - ModeFilter *_mode_filter; -}; -#endif + ModeFilter *_mode_filter; +}; +#endif diff --git a/libraries/AP_RangeFinder/keywords.txt b/libraries/AP_RangeFinder/keywords.txt index be2e98d447..b8abff90b0 100644 --- a/libraries/AP_RangeFinder/keywords.txt +++ b/libraries/AP_RangeFinder/keywords.txt @@ -1,14 +1,14 @@ -RangeFinder KEYWORD1 -AP_RangeFinder KEYWORD1 -AP_RangeFinder_SharpGP2Y KEYWORD1 -AP_RangeFinder_MaxsonarXL KEYWORD1 -read KEYWORD2 -set_orientation KEYWORD2 -convert_raw_to_distance KEYWORD2 -raw_value KEYWORD2 -distance KEYWORD2 -max_distance KEYWORD2 -min_distance KEYWORD2 -orientation_x KEYWORD2 -orientation_y KEYWORD2 -orientation_z KEYWORD2 +RangeFinder KEYWORD1 +AP_RangeFinder KEYWORD1 +AP_RangeFinder_SharpGP2Y KEYWORD1 +AP_RangeFinder_MaxsonarXL KEYWORD1 +read KEYWORD2 +set_orientation KEYWORD2 +convert_raw_to_distance KEYWORD2 +raw_value KEYWORD2 +distance KEYWORD2 +max_distance KEYWORD2 +min_distance KEYWORD2 +orientation_x KEYWORD2 +orientation_y KEYWORD2 +orientation_z KEYWORD2 \ No newline at end of file