36 #include "generated/airframe.h"
57 #define SONAR_BEBOP_INX_DIFF_TO_DIST 340./(2.*160000.)
60 #define SONAR_BEBOP_ADC_MAX_VALUE 4095
63 #define SONAR_BEBOP_ADC_BUFFER_SIZE 8192
72 #define SONAR_BEBOP_TRANSITION_HIGH_TO_LOW 0.8
75 #define SONAR_BEBOP_TRANSITION_LOW_TO_HIGH 1.2
78 #define SONAR_BEBOP_TRANSITION_COUNT 7
83 #define SONAR_BEBOP_PEAK_THRESHOLD 100
86 #define SONAR_BEBOP_MIN_PEAK_VAL 1024 // max value is 4096
89 #define SONAR_BEBOP_MAX_TRANS_TIME 270
95 static uint8_t sonar_bebop_spi_d[2][16] = {{ 0xF0, 0xF0, 0xF0, 0xF0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
96 { 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0x00, 0x00, 0x00, 0x00 }};
123 pthread_setname_np(tid,
"pprz_sonar_thread");
166 if (start_send != 0 && stop_send != 0 && first_peak == 0 && adc_val < lowest_value) {
167 lowest_value = adc_val;
168 }
else if (start_send != 0 && stop_send != 0 && adc_val > lowest_value + 100 && adc_val > peak_value) {
170 peak_value = adc_val;
175 uint16_t diff = stop_send - start_send;
181 #if SONAR_BEBOP_FILTER_NARROW_OBSTACLES
208 #ifdef SENSOR_SYNC_SEND_SONAR
221 #if SONAR_BEBOP_FILTER_NARROW_OBSTACLES
223 #ifndef SONAR_BEBOP_FILTER_NARROW_OBSTACLES_JUMP
224 #define SONAR_BEBOP_FILTER_NARROW_OBSTACLES_JUMP 0.4f
226 PRINT_CONFIG_VAR(SONAR_BEBOP_FILTER_NARROW_OBSTACLES_JUMP)
228 #ifndef SONAR_BEBOP_FILTER_NARROW_OBSTACLES_TIME
229 #define SONAR_BEBOP_FILTER_NARROW_OBSTACLES_TIME 1.0f
231 PRINT_CONFIG_VAR(SONAR_BEBOP_FILTER_NARROW_OBSTACLES_TIME)
235 static float previous_distance = 0;
237 bool obstacle_is_in_view =
false;
238 float diff_pre_cur = distance_sonar - previous_distance;
239 if (diff_pre_cur < -SONAR_BEBOP_FILTER_NARROW_OBSTACLES_JUMP) {
240 z0 = previous_distance;
241 obstacle_is_in_view =
true;
244 previous_distance = distance_sonar;
246 if ((diff_pre_cur > SONAR_BEBOP_FILTER_NARROW_OBSTACLES_JUMP) || (time_since_reset >
USEC_OF_SEC(SONAR_BEBOP_FILTER_NARROW_OBSTACLES_TIME)) ) {
248 obstacle_is_in_view =
false;
251 if (obstacle_is_in_view) {
254 return distance_sonar;
void adc_enable(struct adc_t *adc, uint8_t value)
Start or stop the ADC readings.
void * sonar_bebop_read(void *data)
sonar_bebop_read Read ADC value to update sonar measurement
int adc_read(struct adc_t *adc, uint16_t *buf, uint16_t size)
Read the ADC buffer from the driver.
uint16_t output_length
number of data words to write
static struct spi_transaction sonar_bebop_spi_t
#define SONAR_BEBOP_ADC_BUFFER_SIZE
SONAR_BEBOP_ADC_BUFFER_SIZE ADC buffer size.
#define SONAR_BEBOP_PEAK_THRESHOLD
SONAR_BEBOP_PEAK_THRESHOLD minimum samples from broadcast stop.
Main include for ABI (AirBorneInterface).
Parrot Bebop Sonar driver.
static uint8_t sonar_bebop_spi_d[2][16]
sonar_bebop_spi_d the waveforms emitted by the sonar waveform 0 is long pulse used at high altitude w...
SPI transaction structure.
#define SONAR_BEBOP_INX_DIFF_TO_DIST
SONAR_BEBOP_INX_DIFF_TO_DIST conversion from index difference to distance based on time of flight ADC...
bool spi_submit(struct spi_periph *p, struct spi_transaction *t)
Submit SPI transaction.
#define SONAR_BEBOP_TRANSITION_HIGH_TO_LOW
SONAR_BEBOP_TRANSITION_HIGH_TO_LOW below this altitude we should use mode 0.
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
volatile uint8_t * output_buf
pointer to transmit buffer for DMA
enum SPISlaveSelect select
slave selection behavior
arch independent ADC (Analog to Digital Converter) API
Architecture independent SPI (Serial Peripheral Interface) API.
#define SONAR_BEBOP_MAX_TRANS_TIME
SONAR_BEBOP_MAX_TRANS_TIME maximum time for a reflection to travel and return in the adc measurement ...
Architecture independent timing functions.
float distance
Distance measured in meters.
struct MedianFilterFloat sonar_filt
static uint8_t pulse_transition_counter
#define SONAR_BEBOP_TRANSITION_LOW_TO_HIGH
SONAR_BEBOP_TRANSITION_LOW_TO_HIGH above this altitude we should use mode 1.
#define SONAR_BEBOP_TRANSITION_COUNT
SONAR_BEBOP_TRANSITION_COUNT number of samples before switching mode.
uint16_t input_length
number of data words to read
#define SONAR_BEBOP_ADC_MAX_VALUE
SONAR_BEBOP_ADC_MAX_VALUE maximum ADC output (12 bit ADC)
#define SONAR_BEBOP_MIN_PEAK_VAL
SONAR_BEBOP_MIN_PEAK_VAL minimum adc value of reflected peak that will be cosidered.
API to get/set the generic vehicle states.
static uint8_t mode
mode holds the current sonar mode mode = 0 used at high altitude, uses 16 wave patterns mode = 1 used...
void sonar_bebop_init(void)
slave is selected before transaction and unselected after
Common code for AP and FBW telemetry.
enum SPIDataSizeSelect dss
data transfer word size
uint32_t sonar_bebop_spike_timer
volatile uint8_t * input_buf
pointer to receive buffer for DMA
uint16_t offset
Sonar offset in ADC units.
uint16_t meas
Raw ADC value.
struct SonarBebop sonar_bebop
#define SysTimeTimerStart(_t)
uint16_t adc_buffer[SONAR_BEBOP_ADC_BUFFER_SIZE]
static float sonar_filter_narrow_obstacles(float distance_sonar)
enum SPITransactionStatus status