diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Bebop.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Bebop.cpp
new file mode 100644
index 0000000000..09e9f185f3
--- /dev/null
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_Bebop.cpp
@@ -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 .
+ */
+#include
+#include
+
+#if (CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP) && \
+defined(HAVE_LIBIIO)
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include "AP_RangeFinder_Bebop.h"
+#include
+#include
+#include
+
+/*
+ * 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
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Bebop.h b/libraries/AP_RangeFinder/AP_RangeFinder_Bebop.h
new file mode 100644
index 0000000000..48da66fac7
--- /dev/null
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_Bebop.h
@@ -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 .
+ */
+#pragma once
+
+#include "RangeFinder.h"
+#include "RangeFinder_Backend.h"
+#include
+
+/*
+ * 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 _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;
+};
+
diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp
index 1f94175f5c..9fc3e7c8f3 100644
--- a/libraries/AP_RangeFinder/RangeFinder.cpp
+++ b/libraries/AP_RangeFinder/RangeFinder.cpp
@@ -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
diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h
index 922c36c435..74e0972988 100644
--- a/libraries/AP_RangeFinder/RangeFinder.h
+++ b/libraries/AP_RangeFinder/RangeFinder.h
@@ -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 {