mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AP_RangeFinder: Add support for bebop Rangefinder
This rangefinder uses an spi device to send pulses and an iio driver in buffer mode to get data The data is then analyzed and the maximum pulse received is considered to represent the echo of the pulses that have been sent. The distance in time between the pulse that is sent and the pulse with the maximum amplitude is used to calculate the altitude based on the speed of sound. There is a dependency with libiio, and in order to build, there is a need to provide a rootfs that includes libiio.a. The other solution is to build dynamically after having updated the rootfs to use on compiled with a more recent toolchain and include libiio
This commit is contained in:
Julien Beraud
Lucas De Marchi
parent
c22d791bfc
commit
33a699f29c
467
libraries/AP_RangeFinder/AP_RangeFinder_Bebop.cpp
Normal file
467
libraries/AP_RangeFinder/AP_RangeFinder_Bebop.cpp
Normal file
@ -0,0 +1,467 @@
|
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <utility>
|
||||
|
||||
#if (CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP) && \
|
||||
defined(HAVE_LIBIIO)
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <fcntl.h>
|
||||
#include <string.h>
|
||||
#include <sys/mman.h>
|
||||
#include <linux/types.h>
|
||||
#include <errno.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <float.h>
|
||||
#include <math.h>
|
||||
#include <time.h>
|
||||
#include <iio.h>
|
||||
#include "AP_RangeFinder_Bebop.h"
|
||||
#include <AP_HAL_Linux/Thread.h>
|
||||
#include <AP_HAL_Linux/GPIO.h>
|
||||
#include <AP_HAL_Linux/GPIO_Bebop.h>
|
||||
|
||||
/*
|
||||
* this mode is used at low altitude
|
||||
* send 4 wave patterns
|
||||
* gpio in low mode
|
||||
*/
|
||||
#define RNFD_BEBOP_DEFAULT_MODE 1
|
||||
|
||||
/*
|
||||
* the number of p7s in the iio buffer
|
||||
*/
|
||||
#define RNFD_BEBOP_P7_COUNT 8192
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
static const uint16_t waveform_mode0[14] = {
|
||||
4000, 3800, 3600, 3400, 3200, 3000, 2800,
|
||||
2600, 2400, 2200, 2000, 1800, 1600, 1400,
|
||||
};
|
||||
|
||||
static const uint16_t waveform_mode1[32] = {
|
||||
4190, 4158, 4095, 4095, 4095, 4095, 4095, 4095, 4095,
|
||||
4095, 4090, 4058, 3943, 3924, 3841, 3679, 3588, 3403,
|
||||
3201, 3020, 2816, 2636, 2448, 2227, 2111, 1955, 1819,
|
||||
1675, 1540, 1492, 1374, 1292
|
||||
};
|
||||
|
||||
AP_RangeFinder_Bebop::AP_RangeFinder_Bebop(RangeFinder &_ranger,
|
||||
uint8_t instance, RangeFinder::RangeFinder_State &_state) :
|
||||
AP_RangeFinder_Backend(_ranger, instance, _state),
|
||||
_thread(new Linux::Thread(FUNCTOR_BIND_MEMBER(&AP_RangeFinder_Bebop::_loop, void)))
|
||||
{
|
||||
_init();
|
||||
_freq = RNFD_BEBOP_DEFAULT_FREQ;
|
||||
_filtered_capture_size = _adc.buffer_size / _filter_average;
|
||||
_filtered_capture = (unsigned int*) calloc(1, sizeof(_filtered_capture[0]) *
|
||||
_filtered_capture_size);
|
||||
_mode = RNFD_BEBOP_DEFAULT_MODE;
|
||||
/* SPI and IIO can not be initialized just yet */
|
||||
memset(_tx[0], 0xF0, 16);
|
||||
memset(_tx[1], 0xF0, 4);
|
||||
memset(_purge, 0xFF, RNFD_BEBOP_NB_PULSES_PURGE);
|
||||
_tx_buf = _tx[_mode];
|
||||
}
|
||||
|
||||
AP_RangeFinder_Bebop::~AP_RangeFinder_Bebop()
|
||||
{
|
||||
iio_buffer_destroy(_adc.buffer);
|
||||
_adc.buffer = NULL;
|
||||
iio_context_destroy(_iio);
|
||||
_iio = NULL;
|
||||
}
|
||||
|
||||
bool AP_RangeFinder_Bebop::detect(RangeFinder &_ranger, uint8_t instance)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
unsigned short AP_RangeFinder_Bebop::get_threshold_at(int i_capture)
|
||||
{
|
||||
uint16_t threshold_value = 0;
|
||||
|
||||
/*
|
||||
* We define several kinds of thresholds signals ; for an echo to be
|
||||
* recorded, its maximum amplitude has to be ABOVE that threshold.
|
||||
* There is one kind of threshold per mode (mode 0 is "low" and mode 1 is
|
||||
* "high")
|
||||
* Basically they look like this :
|
||||
*
|
||||
* on this part
|
||||
* of the capture
|
||||
* amplitude we use
|
||||
* ^ the waveform
|
||||
* | <---------->
|
||||
* 4195 +-----+
|
||||
* |
|
||||
* |
|
||||
* |
|
||||
* |
|
||||
* 1200| +----------------+
|
||||
* +-------------------------------------->
|
||||
* + low high time
|
||||
* limit limit
|
||||
*
|
||||
* */
|
||||
switch (_mode) {
|
||||
case 0:
|
||||
if (i_capture < 139)
|
||||
threshold_value = 4195;
|
||||
else if (i_capture < 153)
|
||||
threshold_value = waveform_mode0[i_capture - 139];
|
||||
else
|
||||
threshold_value = 1200;
|
||||
break;
|
||||
|
||||
case 1:
|
||||
if (i_capture < 73)
|
||||
threshold_value = 4195;
|
||||
else if (i_capture < 105)
|
||||
threshold_value = waveform_mode1[i_capture - 73];
|
||||
else if (i_capture < 617)
|
||||
threshold_value = 1200;
|
||||
else
|
||||
threshold_value = 4195;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
return threshold_value;
|
||||
}
|
||||
|
||||
int AP_RangeFinder_Bebop::_apply_averaging_filter(void)
|
||||
{
|
||||
|
||||
int i_filter = 0; /* index in the filtered buffer */
|
||||
int i_capture = 0; /* index in the capture buffer : starts incrementing when
|
||||
the captured data first exceeds
|
||||
RNFD_BEBOP_THRESHOLD_ECHO_INIT */
|
||||
unsigned int filtered_value = 0;
|
||||
bool first_echo = false;
|
||||
unsigned char *data;
|
||||
unsigned char *start;
|
||||
unsigned char *end;
|
||||
ptrdiff_t step;
|
||||
|
||||
step = iio_buffer_step(_adc.buffer);
|
||||
end = (unsigned char *) iio_buffer_end(_adc.buffer);
|
||||
start = (unsigned char *) iio_buffer_first(_adc.buffer, _adc.channel);
|
||||
|
||||
for (data = start; data < end; data += step) {
|
||||
unsigned int current_value = 0;
|
||||
iio_channel_convert(_adc.channel, ¤t_value, data);
|
||||
|
||||
/* We keep on advancing in the captured buffer without registering the
|
||||
* filtered data until the signal first exceeds a given value */
|
||||
if (!first_echo && current_value < threshold_echo_init)
|
||||
continue;
|
||||
else
|
||||
first_echo = true;
|
||||
|
||||
filtered_value += current_value;
|
||||
if (i_capture % _filter_average == 0) {
|
||||
_filtered_capture[i_filter] = filtered_value / _filter_average;
|
||||
filtered_value = 0;
|
||||
i_filter++;
|
||||
}
|
||||
i_capture++;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int AP_RangeFinder_Bebop::_search_local_maxima(void)
|
||||
{
|
||||
int i_echo = 0; /* index in echo array */
|
||||
|
||||
for (int i_capture = 1; i_capture <
|
||||
(int)_filtered_capture_size - 1; i_capture++) {
|
||||
if (_filtered_capture[i_capture] >= get_threshold_at(i_capture)) {
|
||||
unsigned short curr = _filtered_capture[i_capture];
|
||||
unsigned short prev = _filtered_capture[i_capture - 1];
|
||||
unsigned short next = _filtered_capture[i_capture + 1];
|
||||
|
||||
if (curr >= prev && (curr > next || prev <
|
||||
get_threshold_at(i_capture - 1))) {
|
||||
_echoes[i_echo].max_index = i_capture;
|
||||
i_echo++;
|
||||
if (i_echo >= RNFD_BEBOP_MAX_ECHOES)
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
_nb_echoes = i_echo;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int AP_RangeFinder_Bebop::_search_maximum_with_max_amplitude(void)
|
||||
{
|
||||
unsigned short max = 0;
|
||||
int max_idx = -1;
|
||||
|
||||
for (int i_echo = 0; i_echo < _nb_echoes ; i_echo++) {
|
||||
unsigned short curr = _filtered_capture[_echoes[i_echo].max_index];
|
||||
if (curr > max) {
|
||||
max = curr;
|
||||
max_idx = i_echo;
|
||||
}
|
||||
}
|
||||
|
||||
if (max_idx >= 0)
|
||||
return _echoes[max_idx].max_index;
|
||||
else
|
||||
return -1;
|
||||
}
|
||||
|
||||
void AP_RangeFinder_Bebop::_loop(void)
|
||||
{
|
||||
int max_index;
|
||||
|
||||
while(1) {
|
||||
_launch();
|
||||
|
||||
_capture();
|
||||
|
||||
if (_apply_averaging_filter() < 0) {
|
||||
hal.console->printf(
|
||||
"AR_RangeFinder_Bebop: could not apply averaging filter");
|
||||
}
|
||||
|
||||
if (_search_local_maxima() < 0) {
|
||||
hal.console->printf("Did not find any local maximum");
|
||||
}
|
||||
|
||||
max_index = _search_maximum_with_max_amplitude();
|
||||
if (max_index >= 0) {
|
||||
_altitude = (float)(max_index * RNFD_BEBOP_SOUND_SPEED) /
|
||||
(2 * (RNFD_BEBOP_DEFAULT_ADC_FREQ / _filter_average));
|
||||
_mode = _update_mode(_altitude);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void AP_RangeFinder_Bebop::update(void)
|
||||
{
|
||||
static bool first_call = true;
|
||||
|
||||
if (first_call) {
|
||||
_thread->start("RangeFinder_Bebop", SCHED_FIFO, 11);
|
||||
first_call = false;
|
||||
}
|
||||
|
||||
state.distance_cm = (uint16_t) (_altitude * 100);
|
||||
update_status();
|
||||
}
|
||||
|
||||
/*
|
||||
* purge is used when changing mode
|
||||
*/
|
||||
int AP_RangeFinder_Bebop::_launch_purge()
|
||||
{
|
||||
iio_device_attr_write(_adc.device, "buffer/enable", "1");
|
||||
_spi->transfer(_purge, RNFD_BEBOP_NB_PULSES_PURGE, nullptr, 0);
|
||||
return 0;
|
||||
}
|
||||
|
||||
void AP_RangeFinder_Bebop::_configure_gpio(int value)
|
||||
{
|
||||
switch (value) {
|
||||
case 1: // high voltage
|
||||
_gpio->write(LINUX_GPIO_ULTRASOUND_VOLTAGE, 1);
|
||||
break;
|
||||
case 0: // low voltage
|
||||
_gpio->write(LINUX_GPIO_ULTRASOUND_VOLTAGE, 0);
|
||||
break;
|
||||
default:
|
||||
hal.console->printf("bad gpio value (%d)", value);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* reconfigure the pulse that will be sent over spi
|
||||
* first send a purge then configure the new pulse
|
||||
*/
|
||||
void AP_RangeFinder_Bebop::_reconfigure_wave()
|
||||
{
|
||||
/* configure the output buffer for a purge */
|
||||
/* perform a purge */
|
||||
if (_launch_purge() < 0)
|
||||
hal.console->printf("purge could not send data overspi");
|
||||
if (_capture() < 0)
|
||||
hal.console->printf("purge could not capture data");
|
||||
|
||||
switch (_mode) {
|
||||
case 1: /* low voltage */
|
||||
_configure_gpio(0);
|
||||
break;
|
||||
case 0: /* high voltage */
|
||||
_configure_gpio(1);
|
||||
break;
|
||||
default:
|
||||
hal.console->printf("WARNING, invalid value to configure gpio\n");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* First configuration of the the pulse that will be send over spi
|
||||
*/
|
||||
int AP_RangeFinder_Bebop::_configure_wave()
|
||||
{
|
||||
_configure_gpio(0);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*
|
||||
* Configure the adc to get the samples
|
||||
*/
|
||||
int AP_RangeFinder_Bebop::_configure_capture()
|
||||
{
|
||||
const char *adcname = "p7mu-adc_2";
|
||||
const char *adcchannel = "voltage2";
|
||||
/* configure adc interface using libiio */
|
||||
_iio = iio_create_local_context();
|
||||
if (!_iio)
|
||||
return -1;
|
||||
_adc.device = iio_context_find_device(_iio, adcname);
|
||||
|
||||
if (!_adc.device) {
|
||||
hal.console->printf("Unable to find %s", adcname);
|
||||
goto error_destroy_context;
|
||||
}
|
||||
_adc.channel = iio_device_find_channel(_adc.device, adcchannel,
|
||||
false);
|
||||
if (!_adc.channel) {
|
||||
hal.console->printf("Fail to init adc channel %s", adcchannel);
|
||||
goto error_destroy_context;
|
||||
}
|
||||
|
||||
iio_channel_enable(_adc.channel);
|
||||
|
||||
_adc.freq = RNFD_BEBOP_DEFAULT_ADC_FREQ >> RNFD_BEBOP_FILTER_POWER;
|
||||
_adc.threshold_time_rejection = 2.0 / RNFD_BEBOP_SOUND_SPEED *
|
||||
_adc.freq;
|
||||
|
||||
/* Create input buffer */
|
||||
_adc.buffer_size = RNFD_BEBOP_P7_COUNT;
|
||||
if (iio_device_set_kernel_buffers_count(_adc.device, 1)) {
|
||||
hal.console->printf("cannot set buffer count");
|
||||
goto error_destroy_context;
|
||||
}
|
||||
_adc.buffer = iio_device_create_buffer(_adc.device,
|
||||
_adc.buffer_size, false);
|
||||
if (!_adc.buffer) {
|
||||
hal.console->printf("Fail to create buffer : %s", strerror(errno));
|
||||
goto error_destroy_context;
|
||||
}
|
||||
|
||||
return 0;
|
||||
|
||||
error_destroy_context:
|
||||
iio_buffer_destroy(_adc.buffer);
|
||||
_adc.buffer = NULL;
|
||||
iio_context_destroy(_iio);
|
||||
_iio = NULL;
|
||||
return -1;
|
||||
}
|
||||
|
||||
void AP_RangeFinder_Bebop::_init()
|
||||
{
|
||||
_spi = std::move(hal.spi->get_device("bebop"));
|
||||
|
||||
_gpio = AP_HAL::get_HAL().gpio;
|
||||
if (_gpio == NULL) {
|
||||
AP_HAL::panic("Could not find GPIO device for Bebop ultrasound");
|
||||
}
|
||||
|
||||
if (_configure_capture() < 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
_configure_wave();
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
/*
|
||||
* enable the capture buffer
|
||||
* send a pulse over spi
|
||||
*/
|
||||
int AP_RangeFinder_Bebop::_launch()
|
||||
{
|
||||
iio_device_attr_write(_adc.device, "buffer/enable", "1");
|
||||
_spi->transfer(_tx_buf, RNFD_BEBOP_NB_PULSES_MAX, nullptr, 0);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*
|
||||
* read the iio buffer
|
||||
* iio_buffer_refill is blocking by default, so this function is also
|
||||
* blocking untill samples are available
|
||||
* disable the capture buffer
|
||||
*/
|
||||
int AP_RangeFinder_Bebop::_capture()
|
||||
{
|
||||
int ret;
|
||||
|
||||
ret = iio_buffer_refill(_adc.buffer);
|
||||
iio_device_attr_write(_adc.device, "buffer/enable", "0");
|
||||
return ret;
|
||||
}
|
||||
|
||||
int AP_RangeFinder_Bebop::_update_mode(float altitude)
|
||||
{
|
||||
switch (_mode) {
|
||||
case 0:
|
||||
if (altitude < RNFD_BEBOP_TRANSITION_HIGH_TO_LOW
|
||||
&& !is_zero(altitude)) {
|
||||
if (_hysteresis_counter > RNFD_BEBOP_TRANSITION_COUNT) {
|
||||
_mode = 1;
|
||||
_hysteresis_counter = 0;
|
||||
_reconfigure_wave();
|
||||
} else {
|
||||
_hysteresis_counter++;
|
||||
}
|
||||
} else {
|
||||
_hysteresis_counter = 0;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
case 1:
|
||||
if (altitude > RNFD_BEBOP_TRANSITION_LOW_TO_HIGH
|
||||
|| !is_zero(altitude)) {
|
||||
if (_hysteresis_counter > RNFD_BEBOP_TRANSITION_COUNT) {
|
||||
_mode = 0;
|
||||
_hysteresis_counter = 0;
|
||||
_reconfigure_wave();
|
||||
} else {
|
||||
_hysteresis_counter++;
|
||||
}
|
||||
} else {
|
||||
_hysteresis_counter = 0;
|
||||
}
|
||||
break;
|
||||
}
|
||||
return _mode;
|
||||
}
|
||||
#endif
|
||||
140
libraries/AP_RangeFinder/AP_RangeFinder_Bebop.h
Normal file
140
libraries/AP_RangeFinder/AP_RangeFinder_Bebop.h
Normal file
@ -0,0 +1,140 @@
|
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include "RangeFinder.h"
|
||||
#include "RangeFinder_Backend.h"
|
||||
#include <AP_HAL_Linux/Thread.h>
|
||||
|
||||
/*
|
||||
* the size of the buffer sent over spi
|
||||
*/
|
||||
#define RNFD_BEBOP_NB_PULSES_MAX 32
|
||||
|
||||
/*
|
||||
* the size of the purge buffer sent over spi
|
||||
*/
|
||||
#define RNFD_BEBOP_NB_PULSES_PURGE 64
|
||||
|
||||
/*
|
||||
* default us frequency
|
||||
* 17 times by seconds
|
||||
*/
|
||||
#define RNFD_BEBOP_DEFAULT_FREQ 17
|
||||
|
||||
/*
|
||||
* default adc frequency
|
||||
*/
|
||||
#define RNFD_BEBOP_DEFAULT_ADC_FREQ 160000
|
||||
|
||||
/*
|
||||
* to filter data we make the average of (1 << this_value) datas
|
||||
*/
|
||||
#define RNFD_BEBOP_FILTER_POWER 2
|
||||
|
||||
/*
|
||||
* Speed of sound
|
||||
*/
|
||||
#define RNFD_BEBOP_SOUND_SPEED 340
|
||||
|
||||
/* above this altitude we should use mode 0 */
|
||||
#define RNFD_BEBOP_TRANSITION_HIGH_TO_LOW 0.75
|
||||
|
||||
/* below this altitude we should use mode 1 */
|
||||
#define RNFD_BEBOP_TRANSITION_LOW_TO_HIGH 1.5
|
||||
|
||||
/* count this times before switching mode */
|
||||
#define RNFD_BEBOP_TRANSITION_COUNT 5
|
||||
|
||||
/*
|
||||
* the number of echoes we will keep at most
|
||||
*/
|
||||
#define RNFD_BEBOP_MAX_ECHOES 30
|
||||
|
||||
struct echo {
|
||||
int max_index; /* index in the capture buffer at which the maximum is reached */
|
||||
int distance_index; /* index in the capture buffer at which the signal is for
|
||||
the first time above a fixed threshold below the
|
||||
maximum => this corresponds to the real distance
|
||||
that should be attributed to this echo */
|
||||
};
|
||||
|
||||
/*
|
||||
* struct related to adc
|
||||
* data to receive and process adc datas
|
||||
*/
|
||||
struct adc_capture {
|
||||
struct iio_device *device;
|
||||
struct iio_buffer *buffer;
|
||||
unsigned int buffer_size;
|
||||
struct iio_channel *channel;
|
||||
unsigned int freq;
|
||||
|
||||
/* Used in order to match two echoes of two ADC acquisitions */
|
||||
unsigned short threshold_time_rejection;
|
||||
};
|
||||
|
||||
class AP_RangeFinder_Bebop : public AP_RangeFinder_Backend {
|
||||
public:
|
||||
AP_RangeFinder_Bebop(RangeFinder &ranger,
|
||||
uint8_t instance, RangeFinder::RangeFinder_State &_state);
|
||||
|
||||
~AP_RangeFinder_Bebop(void);
|
||||
static bool detect(RangeFinder &ranger, uint8_t instance);
|
||||
void update(void);
|
||||
|
||||
private:
|
||||
void _init(void);
|
||||
int _launch(void);
|
||||
int _capture(void);
|
||||
int _update_mode(float altitude);
|
||||
void _configure_gpio(int value);
|
||||
int _configure_wave();
|
||||
void _reconfigure_wave();
|
||||
int _configure_capture();
|
||||
int _launch_purge();
|
||||
void _loop(void);
|
||||
|
||||
Linux::Thread *_thread;
|
||||
unsigned short get_threshold_at(int i_capture);
|
||||
int _apply_averaging_filter(void);
|
||||
int _search_local_maxima(void);
|
||||
int _search_maximum_with_max_amplitude(void);
|
||||
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> _spi;
|
||||
AP_HAL::GPIO *_gpio;
|
||||
|
||||
struct adc_capture _adc;
|
||||
struct iio_context *_iio;
|
||||
|
||||
unsigned char _tx[2][RNFD_BEBOP_NB_PULSES_MAX];
|
||||
unsigned char _purge[RNFD_BEBOP_NB_PULSES_PURGE];
|
||||
unsigned char* _tx_buf;
|
||||
int _hysteresis_counter;
|
||||
const unsigned int threshold_echo_init = 1500;
|
||||
int _fd = -1;
|
||||
uint64_t _last_timestamp;
|
||||
int _mode;
|
||||
int _nb_echoes;
|
||||
int _freq;
|
||||
float _altitude;
|
||||
unsigned int *_filtered_capture;
|
||||
unsigned int _filtered_capture_size;
|
||||
struct echo _echoes[RNFD_BEBOP_MAX_ECHOES];
|
||||
unsigned int _filter_average = 4;
|
||||
int16_t _last_max_distance_cm = 850;
|
||||
int16_t _last_min_distance_cm = 32;
|
||||
};
|
||||
|
||||
@ -23,6 +23,7 @@
|
||||
#include "AP_RangeFinder_BBB_PRU.h"
|
||||
#include "AP_RangeFinder_LightWareI2C.h"
|
||||
#include "AP_RangeFinder_LightWareSerial.h"
|
||||
#include "AP_RangeFinder_Bebop.h"
|
||||
|
||||
// table of user settable parameters
|
||||
const AP_Param::GroupInfo RangeFinder::var_info[] = {
|
||||
@ -527,7 +528,16 @@ void RangeFinder::detect_instance(uint8_t instance)
|
||||
drivers[instance] = new AP_RangeFinder_LightWareSerial(*this, instance, state[instance], serial_manager);
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP && defined(HAVE_LIBIIO)
|
||||
if (type == RangeFinder_TYPE_BEBOP) {
|
||||
if (AP_RangeFinder_Bebop::detect(*this, instance)) {
|
||||
state[instance].instance = instance;
|
||||
drivers[instance] = new AP_RangeFinder_Bebop(*this, instance, state[instance]);
|
||||
return;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
if (type == RangeFinder_TYPE_ANALOG) {
|
||||
// note that analog must be the last to be checked, as it will
|
||||
// always come back as present if the pin is valid
|
||||
|
||||
@ -46,7 +46,8 @@ public:
|
||||
RangeFinder_TYPE_PX4_PWM= 5,
|
||||
RangeFinder_TYPE_BBB_PRU= 6,
|
||||
RangeFinder_TYPE_LWI2C = 7,
|
||||
RangeFinder_TYPE_LWSER = 8
|
||||
RangeFinder_TYPE_LWSER = 8,
|
||||
RangeFinder_TYPE_BEBOP = 9
|
||||
};
|
||||
|
||||
enum RangeFinder_Function {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user