• Main Page
  • Related Pages
  • Namespaces
  • Classes
  • Files
  • File List
  • File Members

AP_RangeFinder_MaxsonarLV.cpp

Go to the documentation of this file.
00001 // -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*-
00002 /*
00003         AP_RangeFinder_MaxsonarLV.cpp - Arduino Library for Maxbotix's LV-MaxSonar 
00004         Sonic proximity sensor
00005         Code by Jose Julio and Randy Mackay. DIYDrones.com
00006 
00007         This library is free software; you can redistribute it and/or
00008     modify it under the terms of the GNU Lesser General Public
00009     License as published by the Free Software Foundation; either
00010     version 2.1 of the License, or (at your option) any later version.
00011 
00012         Sparkfun URL: http://www.sparkfun.com/products/8502
00013         datasheet: http://www.maxbotix.com/uploads/LV-MaxSonar-EZ0-Datasheet.pdf
00014         
00015         Sensor should be connected to one of the analog ports
00016         
00017         Variables:
00018                 int raw_value : raw value from the sensor
00019                 int distance : distance in cm
00020                 int max_distance : maximum measurable distance (in cm)
00021                 int min_distance : minimum measurable distance (in cm)
00022         
00023         Methods:
00024                 init(int analogPort) : Initialization of sensor
00025                 read() : read value from analog port and returns the distance (in cm)
00026                 
00027 */
00028 
00029 // AVR LibC Includes
00030 #include "WConstants.h"
00031 #include "AP_RangeFinder_MaxsonarLV.h"
00032 
00033 // Public Methods //////////////////////////////////////////////////////////////
00034 void AP_RangeFinder_MaxsonarLV::init(int analogPort)
00035 {
00036     // local variables
00037     int i;
00038         
00039         // set the given analog port to an input
00040         pinMode(analogPort, INPUT);
00041         
00042     // initialise everything
00043     _analogPort = analogPort;
00044         max_distance = AP_RANGEFINDER_MAXSONARLV_MAX_DISTANCE;
00045         min_distance = AP_RANGEFINDER_MAXSONARLV_MIN_DISTANCE;
00046         
00047         // make first call to read to get initial distance
00048         read();
00049         
00050         // initialise history
00051         for( i=0; i<AP_RANGEFINDER_NUM_AVERAGES; i++ )
00052             _history[i] = distance;     
00053 }
00054 
00055 // Read Sensor data
00056 int AP_RangeFinder_MaxsonarLV::read()
00057 {
00058     // read raw sensor value and convert to distance
00059     raw_value = analogRead(_analogPort);
00060         
00061         // for this sensor, the sensor value is in inches, need to convert to cm
00062         distance = raw_value * 2.54;
00063         distance = constrain(distance,min_distance,max_distance);  // converts from inches to cm
00064         
00065         // return distance
00066         return filter(distance);
00067 }

Generated for ArduPilot Libraries by doxygen