diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp index ca6f4de470..5353b6da54 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp @@ -22,9 +22,10 @@ */ // AVR LibC Includes -#include -#include // Arduino I2C lib #include "AP_RangeFinder_MaxsonarI2CXL.h" +#include + +extern const AP_HAL::HAL& hal; // Constructor ////////////////////////////////////////////////////////////// @@ -43,7 +44,9 @@ AP_RangeFinder_MaxsonarI2CXL::AP_RangeFinder_MaxsonarI2CXL( FilterInt16 *filter bool AP_RangeFinder_MaxsonarI2CXL::take_reading() { // take range reading and read back results - if (I2c.write(_addr, (uint8_t)AP_RANGE_FINDER_MAXSONARI2CXL_COMMAND_TAKE_RANGE_READING) != 0) { + uint8_t tosend[1] = + { AP_RANGE_FINDER_MAXSONARI2CXL_COMMAND_TAKE_RANGE_READING }; + if (hal.i2c->write(_addr, 1, tosend) != 0) { healthy = false; return false; }else{ @@ -59,7 +62,7 @@ int AP_RangeFinder_MaxsonarI2CXL::read() int16_t ret_value = 0; // take range reading and read back results - if (I2c.read(_addr, 2, buff) != 0) { + if (hal.i2c->read(_addr, 2, buff) != 0) { healthy = false; }else{ // combine results into distance @@ -68,4 +71,4 @@ int AP_RangeFinder_MaxsonarI2CXL::read() } return ret_value; -} \ No newline at end of file +} diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp index c562813fdf..4618dcac44 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp @@ -25,17 +25,11 @@ * */ -// AVR LibC Includes -#if defined(ARDUINO) && ARDUINO >= 100 - #include "Arduino.h" -#else - #include "WConstants.h" -#endif #include "AP_RangeFinder_MaxsonarXL.h" // Constructor ////////////////////////////////////////////////////////////// -AP_RangeFinder_MaxsonarXL::AP_RangeFinder_MaxsonarXL(AP_AnalogSource *source, FilterInt16 *filter) : +AP_RangeFinder_MaxsonarXL::AP_RangeFinder_MaxsonarXL(AP_HAL::AnalogSource *source, FilterInt16 *filter) : RangeFinder(source, filter), _scaler(AP_RANGEFINDER_MAXSONARXL_SCALER) { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h index eced5050f0..76ec077885 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h @@ -1,5 +1,5 @@ -#ifndef AP_RangeFinder_MaxsonarXL_H -#define AP_RangeFinder_MaxsonarXL_H +#ifndef __AP_RangeFinder_MaxsonarXL_H__ +#define __AP_RangeFinder_MaxsonarXL_H__ #include "RangeFinder.h" @@ -30,7 +30,7 @@ class AP_RangeFinder_MaxsonarXL : public RangeFinder { public: - AP_RangeFinder_MaxsonarXL(AP_AnalogSource *source, FilterInt16 *filter); + AP_RangeFinder_MaxsonarXL(AP_HAL::AnalogSource *source, FilterInt16 *filter); int convert_raw_to_distance(int _raw_value) { return _raw_value * _scaler; } // read value from analog port and return distance in cm diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.cpp index d91f711068..fe94564ffa 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.cpp @@ -25,17 +25,11 @@ * */ -// AVR LibC Includes -#if defined(ARDUINO) && ARDUINO >= 100 - #include "Arduino.h" -#else - #include "WConstants.h" -#endif #include "AP_RangeFinder_SharpGP2Y.h" // Constructor ////////////////////////////////////////////////////////////// -AP_RangeFinder_SharpGP2Y::AP_RangeFinder_SharpGP2Y(AP_AnalogSource *source, FilterInt16 *filter) : +AP_RangeFinder_SharpGP2Y::AP_RangeFinder_SharpGP2Y(AP_HAL::AnalogSource *source, FilterInt16 *filter) : RangeFinder(source, filter) { max_distance = AP_RANGEFINDER_SHARPEGP2Y_MAX_DISTANCE; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.h b/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.h index 2e424cd82b..10841c74df 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.h @@ -1,5 +1,5 @@ -#ifndef AP_RangeFinder_SharpGP2Y_H -#define AP_RangeFinder_SharpGP2Y_H +#ifndef __AP_RangeFinder_SharpGP2Y_H__ +#define __AP_RangeFinder_SharpGP2Y_H__ #include "RangeFinder.h" @@ -9,12 +9,13 @@ class AP_RangeFinder_SharpGP2Y : public RangeFinder { public: - AP_RangeFinder_SharpGP2Y(AP_AnalogSource *source, FilterInt16 *filter); - int convert_raw_to_distance(int _raw_value) { - if( _raw_value == 0 ) + AP_RangeFinder_SharpGP2Y(AP_HAL::AnalogSource *source, FilterInt16 *filter); + int convert_raw_to_distance(int _raw_value) { + if( _raw_value == 0 ) { return max_distance; - else + } else { return 14500/_raw_value; + } } // read value from analog port and return distance in cm }; diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index 28ffaf6337..a83b4983dd 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/libraries/AP_RangeFinder/RangeFinder.cpp @@ -11,17 +11,8 @@ * * This has the basic functions that all RangeFinders need implemented */ - -// AVR LibC Includes -#if defined(ARDUINO) && ARDUINO >= 100 - #include "Arduino.h" -#else - #include "WConstants.h" -#endif #include "RangeFinder.h" - - // Public Methods ////////////////////////////////////////////////////////////// void RangeFinder::set_orientation(int x, int y, int z) @@ -36,7 +27,7 @@ int RangeFinder::read() { int temp_dist; - raw_value = _analog_source->read(); + raw_value = _analog_source->read_average(); // convert analog value to distance in cm (using child implementation most likely) temp_dist = convert_raw_to_distance(raw_value); diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h index 96ab12f1bc..2708fc6403 100644 --- a/libraries/AP_RangeFinder/RangeFinder.h +++ b/libraries/AP_RangeFinder/RangeFinder.h @@ -1,8 +1,8 @@ -#ifndef RangeFinder_h -#define RangeFinder_h +#ifndef __RANGEFINDER_H__ +#define __RANGEFINDER_H__ #include -#include +#include #include // Filter library /* @@ -21,25 +21,38 @@ class RangeFinder { protected: - RangeFinder(AP_AnalogSource * source, FilterInt16 *filter) : + RangeFinder(AP_HAL::AnalogSource * source, FilterInt16 *filter) : _analog_source(source), _mode_filter(filter) { } public: + // raw_value: read the sensor + int raw_value; + // distance: in cm + int distance; + // maximum measurable distance: in cm + int max_distance; + // minimum measurable distance: in cm + int min_distance; - 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; + int orientation_x, orientation_y, orientation_z; + void set_orientation(int x, int y, int z); - virtual void set_orientation(int x, int y, int z); - virtual int convert_raw_to_distance(int _raw_value) { + /** + * convert_raw_to_distance: + * function that each child class should override to convert voltage + * to distance (in cm) + */ + 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 + } + /** + * read: + * read value from sensor and return distance in cm + */ + virtual int read(); - AP_AnalogSource * _analog_source; + AP_HAL::AnalogSource* _analog_source; FilterInt16 * _mode_filter; }; -#endif +#endif // __RANGEFINDER_H__ diff --git a/libraries/AP_RangeFinder/examples/MaxsonarXL_test/Arduino.h b/libraries/AP_RangeFinder/examples/MaxsonarXL_test/Arduino.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_RangeFinder/examples/AP_RangeFinder_test/Makefile b/libraries/AP_RangeFinder/examples/MaxsonarXL_test/Makefile similarity index 100% rename from libraries/AP_RangeFinder/examples/AP_RangeFinder_test/Makefile rename to libraries/AP_RangeFinder/examples/MaxsonarXL_test/Makefile diff --git a/libraries/AP_RangeFinder/examples/MaxsonarXL_test/MaxsonarXL_test.pde b/libraries/AP_RangeFinder/examples/MaxsonarXL_test/MaxsonarXL_test.pde new file mode 100644 index 0000000000..be0c841a30 --- /dev/null +++ b/libraries/AP_RangeFinder/examples/MaxsonarXL_test/MaxsonarXL_test.pde @@ -0,0 +1,92 @@ +/* + * AP_RangeFinder_test + * Code by DIYDrones.com + */ + +// includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define APM_HARDWARE_APM1 1 +#define APM_HARDWARE_APM2 2 + +// uncomment appropriate line for your APM hardware type +//#define CONFIG_APM_HARDWARE APM_HARDWARE_APM1 +#define CONFIG_APM_HARDWARE APM_HARDWARE_APM2 // also applies to APM2.5 + +// uncomment appropriate line corresponding to your sonar +#define SONAR_TYPE AP_RANGEFINDER_MAXSONARXL // 0 - XL (default) +//#define SONAR_TYPE AP_RANGEFINDER_MAXSONARLV // 1 - LV (cheaper) +//#define SONAR_TYPE AP_RANGEFINDER_MAXSONARXLL // 2 - XLL (XL with 10m range) +//#define SONAR_TYPE AP_RANGEFINDER_MAXSONARHRLV // 3 - HRLV-MaxSonar-EZ0 (5m range) +//#define SONAR_TYPE AP_RANGEFINDER_MAXSONARI2CXL // 4 - XLI2C (XL with I2C interface and 7m range) + +// For APM1 we use built in ADC for sonar reads from an analog pin +#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM1 && SONAR_TYPE <= AP_RANGEFINDER_MAXSONARHRLV +# define USE_ADC_ADS7844 // use APM1's built in ADC and connect sonar to pitot tube +#endif + +// define Pitot tube's ADC Channel +#define AP_RANGEFINDER_PITOT_TYPE_ADC_CHANNEL 7 + +//////////////////////////////////////////////////////////////////////////////// +// hal.console-> ports +//////////////////////////////////////////////////////////////////////////////// + + +#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 // also applies to APM2.5 +const AP_HAL::HAL& hal = AP_HAL_AVR_APM2; +#else +const AP_HAL::HAL& hal = AP_HAL_AVR_APM1; +#endif + +// declare global instances +ModeFilterInt16_Size5 mode_filter(2); + +#ifdef USE_ADC_ADS7844 +AP_ADC_ADS7844 adc; +AP_ADC_AnalogSource adc_analog_source(&adc, + AP_RANGEFINDER_PITOT_TYPE_ADC_CHANNEL, 0.25);// use Pitot tube +#endif + +AP_RangeFinder_MaxsonarXL *rf; + +void setup() +{ + hal.console->println("Range Finder Test v1.1"); + hal.console->print("Sonar Type: "); + hal.console->println(SONAR_TYPE); + +#ifdef USE_ADC_ADS7844 + adc.Init(); // APM ADC initialization + AP_HAL::AnalogSource *analog_source = &adc_analog_source; + float scaling = 3.3; +#else + AP_HAL::AnalogSource *analog_source = hal.analogin->channel(3); + float scaling = 5; +#endif + rf = new AP_RangeFinder_MaxsonarXL(analog_source, &mode_filter); + rf->calculate_scaler(SONAR_TYPE, scaling); // setup scaling for sonar +} + +void loop() +{ + hal.console->print("dist:"); + hal.console->print(rf->read()); + hal.console->print("\traw:"); + hal.console->print(rf->raw_value); + hal.console->println(); + + hal.scheduler->delay(100); + +} + +AP_HAL_MAIN(); diff --git a/libraries/AP_RangeFinder/examples/MaxsonarXL_test/nocore.inoflag b/libraries/AP_RangeFinder/examples/MaxsonarXL_test/nocore.inoflag new file mode 100644 index 0000000000..e69de29bb2