Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 23:43:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

Arducopter, RangeFinder - added SONAR_TYPE parameter and properly support the XL (default), LV and long distance XL (aka XLL) sonar types

Browse Source
This commit is contained in:
Randy Mackay 2011-12-11 16:40:59 +09:00
parent f89fed02c3
commit 67855b207d
13 changed files with 108 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduCopter/ArduCopter.pde
View File

@ -250,11 +250,7 @@ ModeFilter sonar_mode_filter;
#elif CONFIG_SONAR_SOURCE == SONAR_SOURCE_ANALOG_PIN
AP_AnalogSource_Arduino sonar_analog_source(CONFIG_SONAR_SOURCE_ANALOG_PIN);
#endif
#if SONAR_TYPE == MAX_SONAR_XL
AP_RangeFinder_MaxsonarXL sonar(&sonar_analog_source, &sonar_mode_filter);
#else
#error Unrecognised SONAR_TYPE setting.
#endif
AP_RangeFinder_MaxsonarXL sonar(&sonar_analog_source, &sonar_mode_filter);
#endif
// agmatthews USERHOOKS

3
ArduCopter/Parameters.h
View File

@ -105,6 +105,7 @@ public:
k_param_input_voltage,
k_param_low_voltage,
k_param_ch7_option,
k_param_sonar_type, // 153
//
// 160: Navigation parameters
@ -192,6 +193,7 @@ public:
AP_Int16 RTL_altitude;
AP_Int8 sonar_enabled;
AP_Int8 sonar_type; // 0 = XL, 1 = LV, 2 = XLL (XL with 10m range)
AP_Int8 battery_monitoring; // 0=disabled, 1=3 cell lipo, 2=4 cell lipo, 3=total voltage only, 4=total voltage and current
AP_Int16 pack_capacity; // Battery pack capacity less reserve
AP_Int8 compass_enabled;
@ -306,6 +308,7 @@ public:
RTL_altitude (ALT_HOLD_HOME * 100, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")),
sonar_enabled (DISABLED, k_param_sonar, PSTR("SONAR_ENABLE")),
sonar_type (AP_RANGEFINDER_MAXSONARXL, k_param_sonar_type, PSTR("SONAR_TYPE")),
battery_monitoring (DISABLED, k_param_battery_monitoring, PSTR("BATT_MONITOR")),
pack_capacity (HIGH_DISCHARGE, k_param_pack_capacity, PSTR("BATT_CAPACITY")),
compass_enabled (MAGNETOMETER, k_param_compass_enabled, PSTR("MAG_ENABLE")),

4
ArduCopter/config.h
View File

@ -161,10 +161,6 @@
# define SONAR_ENABLED DISABLED
#endif
#ifndef SONAR_TYPE
# define SONAR_TYPE MAX_SONAR_XL
#endif
#ifndef CONFIG_SONAR
# define CONFIG_SONAR ENABLED
#endif

5
ArduCopter/defines.h
View File

@ -90,17 +90,12 @@
#define GPS_PROTOCOL_MTK16 6
#define GPS_PROTOCOL_AUTO 7
// SONAR types:
#define MAX_SONAR_UNKNOWN 0
#define MAX_SONAR_XL 1
#define CH_ROLL CH_1
#define CH_PITCH CH_2
#define CH_THROTTLE CH_3
#define CH_RUDDER CH_4
#define CH_YAW CH_4
#define RC_CHANNEL_ANGLE 0
#define RC_CHANNEL_RANGE 1
#define RC_CHANNEL_ANGLE_RAW 2

11
ArduCopter/sensors.pde
View File

@ -5,6 +5,17 @@
static void ReadSCP1000(void) {}
#if CONFIG_SONAR == ENABLED
static void init_sonar(void)
{
#if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC
sonar.calculate_scaler(g.sonar_type, 3.3);
#else
sonar.calculate_scaler(g.sonar_type, 5.0);
#endif
}
#endif
static void init_barometer(void)
{
#if HIL_MODE == HIL_MODE_SENSORS

8
ArduCopter/setup.pde
View File

@ -417,8 +417,12 @@ setup_sonar(uint8_t argc, const Menu::arg *argv)
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
g.sonar_enabled.set_and_save(false);
} else if (argc > 1 && (argv[1].i >= 0 && argv[1].i <= 2)) {
g.sonar_enabled.set_and_save(true); // if you set the sonar type, surely you want it on
g.sonar_type.set_and_save(argv[1].i);
}else{
Serial.printf_P(PSTR("\nOp:[on, off]\n"));
Serial.printf_P(PSTR("\nOp:[on, off, 0-2]\n"));
report_sonar();
return 0;
}
@ -802,9 +806,11 @@ static void report_wp(byte index = 255)
static void report_sonar()
{
g.sonar_enabled.load();
g.sonar_type.load();
Serial.printf_P(PSTR("Sonar\n"));
print_divider();
print_enabled(g.sonar_enabled.get());
Serial.printf_P(PSTR("Type: %d (0=XL, 1=LV, 2=XLL)"), (int)g.sonar_type);
print_blanks(2);
}

5
ArduCopter/system.pde
View File

@ -334,6 +334,11 @@ static void init_ardupilot()
init_barometer();
#endif
// initialise sonar
#if CONFIG_SONAR == ENABLED
init_sonar();
#endif
// initialize commands
// -------------------
init_commands();

3
ArduCopter/test.pde
View File

@ -974,6 +974,9 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
return (0);
}
// make sure sonar is initialised
init_sonar();
print_hit_enter();
while(1) {
delay(100);

25
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp
View File

@ -33,10 +33,33 @@
AP_RangeFinder_MaxsonarXL::AP_RangeFinder_MaxsonarXL(AP_AnalogSource *source,
ModeFilter *filter) :
RangeFinder(source, filter)
RangeFinder(source, filter), _scaler(AP_RANGEFINDER_MAXSONARXL_SCALER)
{
max_distance = AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE;
min_distance = AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE;
}
// Public Methods //////////////////////////////////////////////////////////////
float AP_RangeFinder_MaxsonarXL::calculate_scaler(int sonar_type, float adc_refence_voltage)
{
float type_scaler = 1.0;
switch(sonar_type) {
case AP_RANGEFINDER_MAXSONARXL:
type_scaler = AP_RANGEFINDER_MAXSONARXL_SCALER;
min_distance = AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE;
max_distance = AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE;
break;
case AP_RANGEFINDER_MAXSONARLV:
type_scaler = AP_RANGEFINDER_MAXSONARLV_SCALER;
min_distance = AP_RANGEFINDER_MAXSONARLV_MIN_DISTANCE;
max_distance = AP_RANGEFINDER_MAXSONARLV_MAX_DISTANCE;
break;
case AP_RANGEFINDER_MAXSONARXLL:
type_scaler = AP_RANGEFINDER_MAXSONARXLL_SCALER;
min_distance = AP_RANGEFINDER_MAXSONARXLL_MIN_DISTANCE;
max_distance = AP_RANGEFINDER_MAXSONARXLL_MAX_DISTANCE;
break;
}
_scaler = type_scaler * adc_refence_voltage / 5.0;
return _scaler;
}

23
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h
View File

@ -3,14 +3,31 @@
#include "RangeFinder.h"
// XL-EZ0 (aka XL)
#define AP_RANGEFINDER_MAXSONARXL 0
#define AP_RANGEFINDER_MAXSONARXL_SCALER 1.0
#define AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE 20
#define AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE 700
#define AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE 765
// LV-EZ0 (aka LV)
#define AP_RANGEFINDER_MAXSONARLV 1
#define AP_RANGEFINDER_MAXSONARLV_SCALER (2.54/2.0)
#define AP_RANGEFINDER_MAXSONARLV_MIN_DISTANCE 15
#define AP_RANGEFINDER_MAXSONARLV_MAX_DISTANCE 645
// XL-EZL0 (aka XLL)
#define AP_RANGEFINDER_MAXSONARXLL 2
#define AP_RANGEFINDER_MAXSONARXLL_SCALER 2.0
#define AP_RANGEFINDER_MAXSONARXLL_MIN_DISTANCE 20
#define AP_RANGEFINDER_MAXSONARXLL_MAX_DISTANCE 1068
class AP_RangeFinder_MaxsonarXL : public RangeFinder
{
public:
AP_RangeFinder_MaxsonarXL(AP_AnalogSource *source, ModeFilter *filter);
int convert_raw_to_distance(int _raw_value) { return _raw_value; } // read value from analog port and return distance in cm
int convert_raw_to_distance(int _raw_value) { return _raw_value * _scaler; } // read value from analog port and return distance in cm
float calculate_scaler(int sonar_type, float adc_refence_voltage);
private:
float _scaler; // used to account for different sonar types
};
#endif

10
libraries/AP_RangeFinder/RangeFinder.cpp
View File

@ -30,17 +30,17 @@ void RangeFinder::set_orientation(int x, int y, int z)
// Read Sensor data - only the raw_value is filled in by this parent class
int RangeFinder::read()
{
int temp_dist;
raw_value = _analog_source->read();
raw_value = convert_raw_to_distance(raw_value);
// convert analog value to distance in cm (using child implementation most likely)
raw_value = convert_raw_to_distance(raw_value);
temp_dist = convert_raw_to_distance(raw_value);
// ensure distance is within min and max
raw_value = constrain(raw_value, min_distance, max_distance);
temp_dist = constrain(temp_dist, min_distance, max_distance);
distance = _mode_filter->get_filtered_with_sample(raw_value);
distance = _mode_filter->get_filtered_with_sample(temp_dist);
return distance;
}

4
libraries/AP_RangeFinder/RangeFinder.h
View File

@ -19,13 +19,9 @@
#define AP_RANGEFINDER_ORIENTATION_FRONT_LEFT 5, 5, 0
*/
// define Pitot tube's ADC Channel
#define AP_RANGEFINDER_PITOT_TYPE_ADC_CHANNEL 7
class RangeFinder
{
protected:
//GPS(Stream *s) : _port(s) {};
RangeFinder(AP_AnalogSource * source, ModeFilter *filter) :
_analog_source(source),
_mode_filter(filter)

33
libraries/AP_RangeFinder/examples/AP_RangeFinder_test/AP_RangeFinder_test.pde
View File

@ -11,15 +11,27 @@
#include <AP_AnalogSource.h>
#include <ModeFilter.h> // mode filter
// 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
// declare global instances
Arduino_Mega_ISR_Registry isr_registry;
AP_TimerProcess adc_scheduler;
AP_ADC_ADS7844 adc;
ModeFilter mode_filter;
// uncomment the appropriate line corresponding to where sonar is connected
AP_AnalogSource_ADC adc_source(&adc, AP_RANGEFINDER_PITOT_TYPE_ADC_CHANNEL, 0.25); // uncomment this line to use Pitot tube
//AP_AnalogSource_Arduino adc_source(A4); // uncomment this line to use A5 analog pin (far back-right pin on the oilpan near the CLI switch
#ifdef USE_ADC_ADS7844
AP_TimerProcess adc_scheduler;
AP_ADC_ADS7844 adc;
AP_AnalogSource_ADC adc_source(&adc, AP_RANGEFINDER_PITOT_TYPE_ADC_CHANNEL, 0.25); // use Pitot tube
#else
AP_AnalogSource_Arduino adc_source(A4); // use AN4 analog pin (APM1: on oilpan at back right near the CLI switch. APM2: on left)
#endif
// create the range finder object
//AP_RangeFinder_SharpGP2Y aRF(&adc_source, &mode_filter);
@ -29,10 +41,19 @@ void setup()
{
Serial.begin(115200);
Serial.println("Range Finder Test v1.1");
Serial.print("Sonar Type: ");
Serial.println(SONAR_TYPE);
#ifdef USE_ADC_ADS7844
isr_registry.init();
adc_scheduler.init(&isr_registry);
adc.filter_result = true;
adc.Init(&adc_scheduler); // APM ADC initialization
aRF.calculate_scaler(SONAR_TYPE,3.3); // setup scaling for sonar
#else
aRF.calculate_scaler(SONAR_TYPE,5.0); // setup scaling for sonar
#endif
}
void loop()