Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
sonar_bebop.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 Freek van Tienen <freek.v.tienen@gmail.com>
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  *
21  */
22 
35 #include "sonar_bebop.h"
36 #include "generated/airframe.h"
37 #include "mcu_periph/adc.h"
38 #include "mcu_periph/spi.h"
39 #include "mcu_periph/sys_time.h"
40 #include "subsystems/abi.h"
41 #include <pthread.h>
42 #include <unistd.h>
43 #include "subsystems/datalink/downlink.h"//FIXME, include only when link need
44 
45 #include "filters/median_filter.h"
46 
49 
50 #ifdef SITL
51 #include "state.h"
52 #endif
53 
57 #define SONAR_BEBOP_INX_DIFF_TO_DIST 340./(2.*160000.)
58 
60 #define SONAR_BEBOP_ADC_MAX_VALUE 4095
61 
63 #define SONAR_BEBOP_ADC_BUFFER_SIZE 8192
64 
69 static uint8_t mode;
70 
72 #define SONAR_BEBOP_TRANSITION_HIGH_TO_LOW 0.8
73 
75 #define SONAR_BEBOP_TRANSITION_LOW_TO_HIGH 1.2
76 
78 #define SONAR_BEBOP_TRANSITION_COUNT 7
79 
81 
83 #define SONAR_BEBOP_PEAK_THRESHOLD 100
84 
86 #define SONAR_BEBOP_MIN_PEAK_VAL 1024 // max value is 4096
87 
89 #define SONAR_BEBOP_MAX_TRANS_TIME 270
90 
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 }};
97 
99 
101 void *sonar_bebop_read(void *data);
102 static float sonar_filter_narrow_obstacles(float distance_sonar);
103 
105 {
106  mode = 0; // default mode is low altitude
108 
109  sonar_bebop.meas = 0;
110  sonar_bebop.offset = 0;
111 
119 
120 #if USE_SONAR
121  pthread_t tid;
122  pthread_create(&tid, NULL, sonar_bebop_read, NULL);
123  pthread_setname_np(tid, "pprz_sonar_thread");
124 #endif
125 
128 }
129 
134 void *sonar_bebop_read(void *data __attribute__((unused)))
135 {
136  while (true) {
137 #ifndef SITL
138  uint16_t i;
139 
140  /* Start ADC and send sonar output */
141  adc_enable(&adc0, 1);
147  adc_enable(&adc0, 0);
148 
149  /* Find the peaks */
150  uint16_t start_send = 0;
151  uint16_t stop_send = 0;
152  uint16_t first_peak = 0;
153  uint16_t lowest_value = SONAR_BEBOP_ADC_MAX_VALUE;
154  uint16_t peak_value = 0;
155 
156  for (i = 0; i < SONAR_BEBOP_ADC_BUFFER_SIZE; i++) {
157  uint16_t adc_val = adc_buffer[i] >> 4;
158  if (start_send == 0 && adc_val >= SONAR_BEBOP_ADC_MAX_VALUE) {
159  start_send = i;
160  } else if (start_send && stop_send == 0 && adc_val < SONAR_BEBOP_ADC_MAX_VALUE - SONAR_BEBOP_MIN_PEAK_VAL) {
161  stop_send = i - 1;
163  continue;
164  }
165 
166  if (start_send != 0 && stop_send != 0 && first_peak == 0 && adc_val < lowest_value) {
167  lowest_value = adc_val; // find trough from initial broadcast signal
168  } else if (start_send != 0 && stop_send != 0 && adc_val > lowest_value + 100 && adc_val > peak_value) {
169  first_peak = i;
170  peak_value = adc_val;
171  }
172  }
173 
174  /* Calculate the distance from the peeks */
175  uint16_t diff = stop_send - start_send;
176  if (diff && diff < SONAR_BEBOP_MAX_TRANS_TIME
177  && peak_value > SONAR_BEBOP_MIN_PEAK_VAL) {
178  sonar_bebop.meas = first_peak - (stop_send - diff / 2);
180 
181 #if SONAR_BEBOP_FILTER_NARROW_OBSTACLES
183 #endif
184 
185  // set sonar pulse mode for next pulse based on altitude
188  mode = 1;
190  }
193  mode = 0;
195  }
196  } else {
198  }
199 
200 #else // SITL
202  Bound(sonar_bebop.distance, 0.1f, 7.0f);
204 #endif // SITL
205 
206  // Send ABI message
207  AbiSendMsgAGL(AGL_SONAR_ADC_ID, sonar_bebop.distance);
208 #ifdef SENSOR_SYNC_SEND_SONAR
209  // Send Telemetry report
211 #endif
212 #ifndef SITL
213  }
214 #endif
215  usleep(10000); //100Hz
216  }
217  return NULL;
218 }
219 
220 
221 #if SONAR_BEBOP_FILTER_NARROW_OBSTACLES
222 
223 #ifndef SONAR_BEBOP_FILTER_NARROW_OBSTACLES_JUMP
224 #define SONAR_BEBOP_FILTER_NARROW_OBSTACLES_JUMP 0.4f
225 #endif
226 PRINT_CONFIG_VAR(SONAR_BEBOP_FILTER_NARROW_OBSTACLES_JUMP)
227 
228 #ifndef SONAR_BEBOP_FILTER_NARROW_OBSTACLES_TIME
229 #define SONAR_BEBOP_FILTER_NARROW_OBSTACLES_TIME 1.0f
230 #endif
231 PRINT_CONFIG_VAR(SONAR_BEBOP_FILTER_NARROW_OBSTACLES_TIME)
232 
233 static float sonar_filter_narrow_obstacles(float distance_sonar)
234 {
235  static float previous_distance = 0;
236  static float z0 = 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;
243  }
244  previous_distance = distance_sonar;
245  float time_since_reset = SysTimeTimer(sonar_bebop_spike_timer);
246  if ((diff_pre_cur > SONAR_BEBOP_FILTER_NARROW_OBSTACLES_JUMP) || (time_since_reset > USEC_OF_SEC(SONAR_BEBOP_FILTER_NARROW_OBSTACLES_TIME)) ) {
247 
248  obstacle_is_in_view = false;
249  }
250 
251  if (obstacle_is_in_view) {
252  return z0;
253  } else {
254  return distance_sonar;
255  }
256 }
257 #endif
258 
void adc_enable(struct adc_t *adc, uint8_t value)
Start or stop the ADC readings.
Definition: adc_arch.c:101
unsigned short uint16_t
Definition: types.h:16
void * sonar_bebop_read(void *data)
sonar_bebop_read Read ADC value to update sonar measurement
Definition: sonar_bebop.c:134
int adc_read(struct adc_t *adc, uint16_t *buf, uint16_t size)
Read the ADC buffer from the driver.
Definition: adc_arch.c:113
uint16_t output_length
number of data words to write
Definition: spi.h:146
static struct spi_transaction sonar_bebop_spi_t
Definition: sonar_bebop.c:100
#define AGL_SONAR_ADC_ID
#define SONAR_BEBOP_ADC_BUFFER_SIZE
SONAR_BEBOP_ADC_BUFFER_SIZE ADC buffer size.
Definition: sonar_bebop.c:63
#define SONAR_BEBOP_PEAK_THRESHOLD
SONAR_BEBOP_PEAK_THRESHOLD minimum samples from broadcast stop.
Definition: sonar_bebop.c:83
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...
Definition: sonar_bebop.c:95
SPI transaction structure.
Definition: spi.h:142
#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...
Definition: sonar_bebop.c:57
bool spi_submit(struct spi_periph *p, struct spi_transaction *t)
Submit SPI transaction.
Definition: spi_arch.c:458
#define SONAR_BEBOP_TRANSITION_HIGH_TO_LOW
SONAR_BEBOP_TRANSITION_HIGH_TO_LOW below this altitude we should use mode 0.
Definition: sonar_bebop.c:72
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
volatile uint8_t * output_buf
pointer to transmit buffer for DMA
Definition: spi.h:144
enum SPISlaveSelect select
slave selection behavior
Definition: spi.h:148
arch independent ADC (Analog to Digital Converter) API
Architecture independent SPI (Serial Peripheral Interface) API.
Definition: spi.h:84
#define SONAR_BEBOP_MAX_TRANS_TIME
SONAR_BEBOP_MAX_TRANS_TIME maximum time for a reflection to travel and return in the adc measurement ...
Definition: sonar_bebop.c:89
Architecture independent timing functions.
struct spi_periph spi0
Definition: spi.c:35
float distance
Distance measured in meters.
Definition: sonar_bebop.h:35
unsigned long uint32_t
Definition: types.h:18
struct MedianFilterFloat sonar_filt
Definition: sonar_bebop.c:47
static void init_median_filter_f(struct MedianFilterFloat *filter, uint8_t size)
static uint8_t pulse_transition_counter
Definition: sonar_bebop.c:80
#define SONAR_BEBOP_TRANSITION_LOW_TO_HIGH
SONAR_BEBOP_TRANSITION_LOW_TO_HIGH above this altitude we should use mode 1.
Definition: sonar_bebop.c:75
static float update_median_filter_f(struct MedianFilterFloat *filter, float new_data)
#define USEC_OF_SEC(sec)
Definition: sys_time.h:210
#define SONAR_BEBOP_TRANSITION_COUNT
SONAR_BEBOP_TRANSITION_COUNT number of samples before switching mode.
Definition: sonar_bebop.c:78
uint16_t input_length
number of data words to read
Definition: spi.h:145
#define SysTimeTimer(_t)
Definition: sys_time.h:219
#define SONAR_BEBOP_ADC_MAX_VALUE
SONAR_BEBOP_ADC_MAX_VALUE maximum ADC output (12 bit ADC)
Definition: sonar_bebop.c:60
#define SONAR_BEBOP_MIN_PEAK_VAL
SONAR_BEBOP_MIN_PEAK_VAL minimum adc value of reflected peak that will be cosidered.
Definition: sonar_bebop.c:86
unsigned char uint8_t
Definition: types.h:14
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...
Definition: sonar_bebop.c:69
void sonar_bebop_init(void)
Definition: sonar_bebop.c:104
float z
in meters
slave is selected before transaction and unselected after
Definition: spi.h:57
enum SPIDataSizeSelect dss
data transfer word size
Definition: spi.h:151
uint32_t sonar_bebop_spike_timer
Definition: sonar_bebop.c:48
volatile uint8_t * input_buf
pointer to receive buffer for DMA
Definition: spi.h:143
uint16_t offset
Sonar offset in ADC units.
Definition: sonar_bebop.h:34
uint16_t meas
Raw ADC value.
Definition: sonar_bebop.h:33
struct SonarBebop sonar_bebop
Definition: sonar_bebop.c:98
#define SysTimeTimerStart(_t)
Definition: sys_time.h:218
uint16_t adc_buffer[SONAR_BEBOP_ADC_BUFFER_SIZE]
Definition: sonar_bebop.c:130
static float sonar_filter_narrow_obstacles(float distance_sonar)
enum SPITransactionStatus status
Definition: spi.h:156