diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h index 0f1da2fc08..5e3f941230 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder.h @@ -5,3 +5,4 @@ #include "AP_RangeFinder_SharpGP2Y.h" #include "AP_RangeFinder_MaxsonarXL.h" +#include "AP_RangeFinder_MaxsonarI2CXL.h" diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp new file mode 100644 index 0000000000..fb8756d89e --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp @@ -0,0 +1,70 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*- +/* + * AP_RangeFinder_MaxsonarI2CXL.cpp - Arduino Library for MaxBotix I2C XL sonar + * Code by 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. + * + * datasheet: http://www.maxbotix.com/documents/I2CXL-MaxSonar-EZ_Datasheet.pdf + * + * Sensor should be connected to the I2C port + * + * Variables: + * bool healthy : indicates whether last communication with sensor was successful + * + * Methods: + * take_reading(): ask the sonar to take a new distance measurement + * read() : read last distance measured (in cm) + * + */ + +// AVR LibC Includes +#include +#include "AP_RangeFinder_MaxsonarI2CXL.h" + +// Constructor ////////////////////////////////////////////////////////////// + +AP_RangeFinder_MaxsonarI2CXL::AP_RangeFinder_MaxsonarI2CXL( FilterInt16 *filter ) : + RangeFinder(NULL, filter), + healthy(true), + _addr(AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR) +{ + max_distance = AP_RANGE_FINDER_MAXSONARI2CXL_MIN_DISTANCE; + min_distance = AP_RANGE_FINDER_MAXSONARI2CXL_MAX_DISTANCE; +} + +// Public Methods ////////////////////////////////////////////////////////////// + +// take_reading - ask sensor to make a range reading +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) { + healthy = false; + return false; + }else{ + healthy = true; + return true; + } +} + +// read - return last value measured by sensor +int AP_RangeFinder_MaxsonarI2CXL::read() +{ + uint8_t buff[2]; + int16_t distance = 0; + + // take range reading and read back results + if (I2c.read(_addr, 2, buff) != 0) { + healthy = false; + }else{ + // combine results into distance + distance = buff[0] << 8 | buff[1]; + healthy = true; + } + + return distance; +} \ No newline at end of file diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h new file mode 100644 index 0000000000..5a4905ae71 --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h @@ -0,0 +1,42 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*- + +#ifndef __AP_RANGEFINDER_MAXSONARI2CXL_H__ +#define __AP_RANGEFINDER_MAXSONARI2CXL_H__ + +#include "RangeFinder.h" +#include // Arduino I2C lib + +#define AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR 0x70 + +#define AP_RANGEFINDER_MAXSONARI2CXL 4 +#define AP_RANGE_FINDER_MAXSONARI2CXL_SCALER 1.0 +#define AP_RANGE_FINDER_MAXSONARI2CXL_MIN_DISTANCE 20 +#define AP_RANGE_FINDER_MAXSONARI2CXL_MAX_DISTANCE 765 + +#define AP_RANGE_FINDER_MAXSONARI2CXL_COMMAND_TAKE_RANGE_READING 0x51 + +class AP_RangeFinder_MaxsonarI2CXL : public RangeFinder +{ + +public: + + // constructor + AP_RangeFinder_MaxsonarI2CXL(FilterInt16 *filter); + + // init - simply sets the i2c address + void init(uint8_t address = AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR) { _addr = address; } + + // take_reading - ask sensor to make a range reading + bool take_reading(); + + // read value from sensor and return distance in cm + int read(); + + // heath + bool healthy; + +protected: + uint8_t _addr; + +}; +#endif // __AP_RANGEFINDER_MAXSONARI2CXL_H__ diff --git a/libraries/AP_RangeFinder/examples/AP_RangeFinder_test/AP_RangeFinder_test.pde b/libraries/AP_RangeFinder/examples/AP_RangeFinder_test/AP_RangeFinder_test.pde index 13d4217356..690ecba928 100644 --- a/libraries/AP_RangeFinder/examples/AP_RangeFinder_test/AP_RangeFinder_test.pde +++ b/libraries/AP_RangeFinder/examples/AP_RangeFinder_test/AP_RangeFinder_test.pde @@ -7,6 +7,7 @@ #include #include #include +#include // Arduino I2C lib #include // Range finder library #include #include @@ -15,19 +16,30 @@ #include // mode filter #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 + //////////////////////////////////////////////////////////////////////////////// // Serial ports //////////////////////////////////////////////////////////////////////////////// FastSerialPort0(Serial); // FTDI/console -// comment out line below if using APM2 or analog pin instead of APM1's built in ADC -#define USE_ADC_ADS7844 // use APM1's built in ADC and connect sonar to pitot tube - -// 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 Pitot tube's ADC Channel #define AP_RANGEFINDER_PITOT_TYPE_ADC_CHANNEL 7 @@ -45,7 +57,16 @@ AP_AnalogSource_Arduino adc_source(A0); // use AN0 analog pin for APM2 on // create the range finder object //AP_RangeFinder_SharpGP2Y aRF(&adc_source, &mode_filter); + +// declare maxbotix sensors using analog pin +#if SONAR_TYPE >= AP_RANGEFINDER_MAXSONARXL && SONAR_TYPE <= AP_RANGEFINDER_MAXSONARHRLV AP_RangeFinder_MaxsonarXL aRF(&adc_source, &mode_filter); +#endif + +// declare I2C maxbotix sensor +#if SONAR_TYPE == AP_RANGEFINDER_MAXSONARI2CXL +AP_RangeFinder_MaxsonarI2CXL aRF(&mode_filter); +#endif void setup() { @@ -57,16 +78,21 @@ void setup() isr_registry.init(); timer_scheduler.init(&isr_registry); -#ifdef USE_ADC_ADS7844 + // initialise communication method (analog read or i2c) +#if SONAR_TYPE == AP_RANGEFINDER_MAXSONARI2CXL + I2c.begin(); + I2c.timeOut(5); + I2c.setSpeed(true); // initially set a fast I2c speed, and drop it on first failures +#else +# ifdef USE_ADC_ADS7844 adc.Init(&timer_scheduler); // APM ADC initialization aRF.calculate_scaler(SONAR_TYPE,3.3); // setup scaling for sonar -#else +# else // initialise the analog port reader AP_AnalogSource_Arduino::init_timer(&timer_scheduler); - aRF.calculate_scaler(SONAR_TYPE,5.0); // setup scaling for sonar +# endif #endif - } void loop() @@ -76,7 +102,12 @@ void loop() Serial.print("\traw:"); Serial.print(aRF.raw_value); Serial.println(); - delay(20); + +#if SONAR_TYPE == AP_RANGEFINDER_MAXSONARI2CXL + if( !aRF.healthy ) { + Serial.println("not healthy!"); + } + aRF.take_reading(); +#endif + delay(100); } - -