mirror of https://github.com/ArduPilot/ardupilot
140 lines
3.7 KiB
C++
140 lines
3.7 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::RangeFinder_State &_state);
|
|
|
|
~AP_RangeFinder_Bebop(void);
|
|
static bool detect();
|
|
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;
|
|
};
|
|
|