Paparazzi UAS  v5.15_devel-187-g731cc33
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 #ifndef SONAR_BEBOP_TRANSITION_HIGH_TO_LOW
73 #define SONAR_BEBOP_TRANSITION_HIGH_TO_LOW 0.8
74 #endif
75 
77 #ifndef SONAR_BEBOP_TRANSITION_LOW_TO_HIGH
78 #define SONAR_BEBOP_TRANSITION_LOW_TO_HIGH 1.2
79 #endif
80 
82 #ifndef SONAR_BEBOP_TRANSITION_COUNT
83 #define SONAR_BEBOP_TRANSITION_COUNT 7
84 #endif
85 
87 
89 #ifndef SONAR_BEBOP_PEAK_THRESHOLD
90 #define SONAR_BEBOP_PEAK_THRESHOLD 50
91 #endif
92 
94 #ifndef SONAR_BEBOP_MIN_PEAK_VAL
95 #define SONAR_BEBOP_MIN_PEAK_VAL 512 // max value is 4096
96 #endif
97 
99 #define SONAR_BEBOP_MAX_TRANS_TIME 370
100 
105 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 },
106  { 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0x00, 0x00, 0x00, 0x00 }};
108 
110 void *sonar_bebop_read(void *data);
111 #if SONAR_BEBOP_FILTER_NARROW_OBSTACLES
112 static float sonar_filter_narrow_obstacles(float distance_sonar);
113 #endif
114 
116 {
117  mode = 0; // default mode is low altitude
119 
120  sonar_bebop.meas = 0;
121  sonar_bebop.offset = 0;
122 
123  /* configure spi transaction */
131 
133 #if SONAR_BEBOP_FILTER_NARROW_OBSTACLES
135 #endif
136 
137 #if USE_SONAR
138  pthread_t tid;
139  pthread_create(&tid, NULL, sonar_bebop_read, NULL);
140 #ifndef __APPLE__
141  pthread_setname_np(tid, "sonar");
142 #endif
143 #endif
144 }
145 
150 void *sonar_bebop_read(void *data __attribute__((unused)))
151 {
152  while (true) {
153  #ifndef SITL
154  uint16_t i;
155 
156  /* Start ADC and send sonar output */
157  adc_enable(&adc0, 1);
159  usleep(100); //Sonar crashes without the delay once in a while :(
163  usleep(100); //Sonar crashes without the delay once in a while :(
165  adc_enable(&adc0, 0);
166 
167  /* Find the peaks */
168  uint16_t start_send = 0;
169  uint16_t stop_send = 0;
170  uint16_t first_peak = 0;
171  uint16_t lowest_value = SONAR_BEBOP_ADC_MAX_VALUE;
172  uint16_t peak_value = 0;
173 
174  for (i = 0; i < SONAR_BEBOP_ADC_BUFFER_SIZE; i++) {
175  uint16_t adc_val = adc_buffer[i] >> 4;
176  if (start_send == 0 && adc_val >= SONAR_BEBOP_ADC_MAX_VALUE) {
177  start_send = i;
178  } else if (start_send && stop_send == 0 && adc_val < SONAR_BEBOP_ADC_MAX_VALUE - SONAR_BEBOP_MIN_PEAK_VAL) {
179  stop_send = i - 1;
181  continue;
182  }
183 
184  if (start_send != 0 && stop_send != 0 && first_peak == 0 && adc_val < lowest_value) {
185  lowest_value = adc_val; // find trough from initial broadcast signal
186  } else if (start_send != 0 && stop_send != 0 && adc_val > lowest_value + 100 && adc_val > peak_value) {
187  first_peak = i;
188  peak_value = adc_val;
189  }
190  }
191 
192  /* Calculate the distance from the peeks */
193  uint16_t diff = stop_send - start_send;
194  if (diff && diff < SONAR_BEBOP_MAX_TRANS_TIME
195  && peak_value > SONAR_BEBOP_MIN_PEAK_VAL) {
196  sonar_bebop.meas = (uint16_t)(first_peak - (stop_send - diff / 2));
198 
199  // set sonar pulse mode for next pulse based on altitude
202  mode = 1;
204  }
207  mode = 0;
209  }
210  } else {
212  }
213 
214  #else // SITL
216  Bound(sonar_bebop.distance, 0.1f, 7.0f);
217  sonar_bebop.meas = (uint16_t)(sonar_bebop.distance / SONAR_BEBOP_INX_DIFF_TO_DIST);
218  #endif // SITL
219 
220  #if SONAR_COMPENSATE_ROTATION
221  float phi = stateGetNedToBodyEulers_f()->phi;
222  float theta = stateGetNedToBodyEulers_f()->theta;
223  float gain = (float)fabs( (double) (cosf(phi) * cosf(theta)));
225  #endif
226 
227  #if SONAR_BEBOP_FILTER_NARROW_OBSTACLES
228  sonar_bebop.distance = sonar_filter_narrow_obstacles(sonar_bebop.distance);
229  #endif
230 
231  // Send ABI message
232  uint32_t now_ts = get_sys_time_usec();
233  AbiSendMsgAGL(AGL_SONAR_ADC_ID, now_ts, sonar_bebop.distance);
234 
235  #ifdef SENSOR_SYNC_SEND_SONAR
236  // Send Telemetry report
238  #endif
239  }
240  usleep(10000); //100Hz FIXME: use SYS_TIME_FREQUENCY and divisor
241  }
242  return NULL;
243 }
244 
245 #if SONAR_BEBOP_FILTER_NARROW_OBSTACLES
246 
247 #ifndef SONAR_BEBOP_FILTER_NARROW_OBSTACLES_JUMP
248 #define SONAR_BEBOP_FILTER_NARROW_OBSTACLES_JUMP 0.4f
249 #endif
250 PRINT_CONFIG_VAR(SONAR_BEBOP_FILTER_NARROW_OBSTACLES_JUMP)
251 
252 #ifndef SONAR_BEBOP_FILTER_NARROW_OBSTACLES_TIME
253 #define SONAR_BEBOP_FILTER_NARROW_OBSTACLES_TIME 1.0f
254 #endif
255 PRINT_CONFIG_VAR(SONAR_BEBOP_FILTER_NARROW_OBSTACLES_TIME)
256 
257 static float sonar_filter_narrow_obstacles(float distance_sonar)
258 {
259  static float previous_distance = 0;
260  static float z0 = 0;
261  bool obstacle_is_in_view = false;
262  float diff_pre_cur = distance_sonar - previous_distance;
263  if (diff_pre_cur < -SONAR_BEBOP_FILTER_NARROW_OBSTACLES_JUMP) {
264  z0 = previous_distance;
265  obstacle_is_in_view = true;
267  }
268  previous_distance = distance_sonar;
269  float time_since_reset = SysTimeTimer(sonar_bebop_spike_timer);
270  if ((diff_pre_cur > SONAR_BEBOP_FILTER_NARROW_OBSTACLES_JUMP) || (time_since_reset > USEC_OF_SEC(SONAR_BEBOP_FILTER_NARROW_OBSTACLES_TIME)) ) {
271 
272  obstacle_is_in_view = false;
273  }
274 
275  if (obstacle_is_in_view) {
276  return z0;
277  } else {
278  return distance_sonar;
279  }
280 }
281 #endif
282 
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:150
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
float phi
in radians
uint16_t output_length
number of data words to write
Definition: spi.h:146
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
static struct spi_transaction sonar_bebop_spi_t
Definition: sonar_bebop.c:109
#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:90
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:105
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:505
#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:73
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.
float theta
in radians
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:99
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:86
#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:78
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:83
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:95
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:115
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:107
#define SysTimeTimerStart(_t)
Definition: sys_time.h:218
uint16_t adc_buffer[SONAR_BEBOP_ADC_BUFFER_SIZE]
Definition: sonar_bebop.c:146
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