ardupilot/libraries/AP_RangeFinder/examples/MaxsonarXL_test/MaxsonarXL_test.pde

83 lines
2.4 KiB
Plaintext

/*
* 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 <AP_ADC_AnalogSource.h>
#include <Filter.h>
#include <AP_Buffer.h>
#include <AP_HAL_AVR.h>
// 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_HAL_BOARD == HAL_BOARD_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
////////////////////////////////////////////////////////////////////////////////
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
// 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();