AP_Rangefinder: ported to AP_HAL. Only officially supports MaxsonarXL.

libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp

@@ -22,9 +22,10 @@
  */
 
 // AVR LibC Includes
-#include <AP_Common.h>
-#include <I2C.h>                // Arduino I2C lib
 #include "AP_RangeFinder_MaxsonarI2CXL.h"
+#include <AP_HAL.h>
+
+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;
-}
+}

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)
 {

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

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;

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
 
 };

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);
 

libraries/AP_RangeFinder/RangeFinder.h

@@ -1,8 +1,8 @@
-#ifndef RangeFinder_h
-#define RangeFinder_h
+#ifndef __RANGEFINDER_H__
+#define __RANGEFINDER_H__
 
 #include <AP_Common.h>
-#include <AP_AnalogSource.h>
+#include <AP_HAL.h>
 #include <Filter.h> // 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__

libraries/AP_RangeFinder/examples/MaxsonarXL_test/MaxsonarXL_test.pde

@@ -0,0 +1,92 @@
+/*
+ *  AP_RangeFinder_test
+ *  Code by DIYDrones.com
+ */
+
+// includes
+#include <AP_Common.h>
+#include <AP_Progmem.h>
+#include <AP_Param.h>
+#include <AP_HAL.h>
+#include <AP_Math.h>
+#include <AP_RangeFinder.h>
+#include <AP_ADC.h>
+#include <Filter.h>
+#include <AP_Buffer.h>
+#include <AP_HAL_AVR.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
+
+// 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();