Paparazzi UAS  v5.15_devel-46-gd23ed71
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 }};
98 
100 void *sonar_bebop_read(void *data);
101 #if SONAR_BEBOP_FILTER_NARROW_OBSTACLES
102 static float sonar_filter_narrow_obstacles(float distance_sonar);
103 #endif
104 
106 {
107  mode = 0; // default mode is low altitude
109 
110  sonar_bebop.meas = 0;
111  sonar_bebop.offset = 0;
112 
113  /* configure spi transaction */
121 
123 #if SONAR_BEBOP_FILTER_NARROW_OBSTACLES
125 #endif
126 
127 #if USE_SONAR
128  pthread_t tid;
129  pthread_create(&tid, NULL, sonar_bebop_read, NULL);
130 #ifndef __APPLE__
131  pthread_setname_np(tid, "sonar");
132 #endif
133 #endif
134 }
135 
140 void *sonar_bebop_read(void *data __attribute__((unused)))
141 {
142  while (true) {
143 #ifndef SITL
144  uint16_t i;
145 
146  /* Start ADC and send sonar output */
147  adc_enable(&adc0, 1);
153  adc_enable(&adc0, 0);
154 
155  /* Find the peaks */
156  uint16_t start_send = 0;
157  uint16_t stop_send = 0;
158  uint16_t first_peak = 0;
159  uint16_t lowest_value = SONAR_BEBOP_ADC_MAX_VALUE;
160  uint16_t peak_value = 0;
161 
162  for (i = 0; i < SONAR_BEBOP_ADC_BUFFER_SIZE; i++) {
163  uint16_t adc_val = adc_buffer[i] >> 4;
164  if (start_send == 0 && adc_val >= SONAR_BEBOP_ADC_MAX_VALUE) {
165  start_send = i;
166  } else if (start_send && stop_send == 0 && adc_val < SONAR_BEBOP_ADC_MAX_VALUE - SONAR_BEBOP_MIN_PEAK_VAL) {
167  stop_send = i - 1;
169  continue;
170  }
171 
172  if (start_send != 0 && stop_send != 0 && first_peak == 0 && adc_val < lowest_value) {
173  lowest_value = adc_val; // find trough from initial broadcast signal
174  } else if (start_send != 0 && stop_send != 0 && adc_val > lowest_value + 100 && adc_val > peak_value) {
175  first_peak = i;
176  peak_value = adc_val;
177  }
178  }
179 
180  /* Calculate the distance from the peeks */
181  uint16_t diff = stop_send - start_send;
182  if (diff && diff < SONAR_BEBOP_MAX_TRANS_TIME
183  && peak_value > SONAR_BEBOP_MIN_PEAK_VAL) {
184  sonar_bebop.meas = first_peak - (stop_send - diff / 2);
186 
187 #if SONAR_BEBOP_FILTER_NARROW_OBSTACLES
188  sonar_bebop.distance = sonar_filter_narrow_obstacles(sonar_bebop.distance);
189 #endif
190 
191  // set sonar pulse mode for next pulse based on altitude
194  mode = 1;
196  }
199  mode = 0;
201  }
202  } else {
204  }
205 
206 #else // SITL
208  Bound(sonar_bebop.distance, 0.1f, 7.0f);
210 #endif // SITL
211 
212  // Send ABI message
213  uint32_t now_ts = get_sys_time_usec();
214  AbiSendMsgAGL(AGL_SONAR_ADC_ID, now_ts, sonar_bebop.distance);
215 #ifdef SENSOR_SYNC_SEND_SONAR
216  // Send Telemetry report
218 #endif
219 #ifndef SITL
220  }
221 #endif
222  usleep(10000); //100Hz
223  }
224  return NULL;
225 }
226 
227 
228 #if SONAR_BEBOP_FILTER_NARROW_OBSTACLES
229 
230 #ifndef SONAR_BEBOP_FILTER_NARROW_OBSTACLES_JUMP
231 #define SONAR_BEBOP_FILTER_NARROW_OBSTACLES_JUMP 0.4f
232 #endif
233 PRINT_CONFIG_VAR(SONAR_BEBOP_FILTER_NARROW_OBSTACLES_JUMP)
234 
235 #ifndef SONAR_BEBOP_FILTER_NARROW_OBSTACLES_TIME
236 #define SONAR_BEBOP_FILTER_NARROW_OBSTACLES_TIME 1.0f
237 #endif
238 PRINT_CONFIG_VAR(SONAR_BEBOP_FILTER_NARROW_OBSTACLES_TIME)
239 
240 static float sonar_filter_narrow_obstacles(float distance_sonar)
241 {
242  static float previous_distance = 0;
243  static float z0 = 0;
244  bool obstacle_is_in_view = false;
245  float diff_pre_cur = distance_sonar - previous_distance;
246  if (diff_pre_cur < -SONAR_BEBOP_FILTER_NARROW_OBSTACLES_JUMP) {
247  z0 = previous_distance;
248  obstacle_is_in_view = true;
250  }
251  previous_distance = distance_sonar;
252  float time_since_reset = SysTimeTimer(sonar_bebop_spike_timer);
253  if ((diff_pre_cur > SONAR_BEBOP_FILTER_NARROW_OBSTACLES_JUMP) || (time_since_reset > USEC_OF_SEC(SONAR_BEBOP_FILTER_NARROW_OBSTACLES_TIME)) ) {
254 
255  obstacle_is_in_view = false;
256  }
257 
258  if (obstacle_is_in_view) {
259  return z0;
260  } else {
261  return distance_sonar;
262  }
263 }
264 #endif
265 
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:140
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:99
#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:464
#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:105
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:97
#define SysTimeTimerStart(_t)
Definition: sys_time.h:218
uint16_t adc_buffer[SONAR_BEBOP_ADC_BUFFER_SIZE]
Definition: sonar_bebop.c:136
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
enum SPITransactionStatus status
Definition: spi.h:156