mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 22:48:29 -04:00
33a699f29c
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
141 lines
3.8 KiB
C++
141 lines
3.8 KiB
C++
/*
|
|
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;
|
|
};
|
|
|