AP_RangeFinder: added support for MaxBotix XL I2C sonar

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"

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 <AP_Common.h>
+#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;
+}

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 <I2C.h>                        // 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__

libraries/AP_RangeFinder/examples/AP_RangeFinder_test/AP_RangeFinder_test.pde

@@ -7,6 +7,7 @@
 #include <FastSerial.h>
 #include <AP_Common.h>
 #include <AP_Math.h>
+#include <I2C.h>                // Arduino I2C lib
 #include <AP_RangeFinder.h>     // Range finder library
 #include <Arduino_Mega_ISR_Registry.h>
 #include <AP_PeriodicProcess.h>
@@ -15,19 +16,30 @@
 #include <ModeFilter.h>                 // mode filter
 #include <AP_Buffer.h>
 
+#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);
 }
-
-