Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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 #if SONAR_COMPENSATE_ROTATION
215  float phi = stateGetNedToBodyEulers_f()->phi;
216  float theta = stateGetNedToBodyEulers_f()->theta;
217  float gain = (float)fabs( (double) (cosf(phi) * cosf(theta)));
219 #endif
220 
221 #else // SITL
223  Bound(sonar_bebop.distance, 0.1f, 7.0f);
225 #endif // SITL
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 #ifndef SITL
241  }
242 #endif
243  usleep(10000); //100Hz FIXME: use SYS_TIME_FREQUENCY and divisor
244  }
245  return NULL;
246 }
247 
248 #if SONAR_BEBOP_FILTER_NARROW_OBSTACLES
249 
250 #ifndef SONAR_BEBOP_FILTER_NARROW_OBSTACLES_JUMP
251 #define SONAR_BEBOP_FILTER_NARROW_OBSTACLES_JUMP 0.4f
252 #endif
253 PRINT_CONFIG_VAR(SONAR_BEBOP_FILTER_NARROW_OBSTACLES_JUMP)
254 
255 #ifndef SONAR_BEBOP_FILTER_NARROW_OBSTACLES_TIME
256 #define SONAR_BEBOP_FILTER_NARROW_OBSTACLES_TIME 1.0f
257 #endif
258 PRINT_CONFIG_VAR(SONAR_BEBOP_FILTER_NARROW_OBSTACLES_TIME)
259 
260 static float sonar_filter_narrow_obstacles(float distance_sonar)
261 {
262  static float previous_distance = 0;
263  static float z0 = 0;
264  bool obstacle_is_in_view = false;
265  float diff_pre_cur = distance_sonar - previous_distance;
266  if (diff_pre_cur < -SONAR_BEBOP_FILTER_NARROW_OBSTACLES_JUMP) {
267  z0 = previous_distance;
268  obstacle_is_in_view = true;
270  }
271  previous_distance = distance_sonar;
272  float time_since_reset = SysTimeTimer(sonar_bebop_spike_timer);
273  if ((diff_pre_cur > SONAR_BEBOP_FILTER_NARROW_OBSTACLES_JUMP) || (time_since_reset > USEC_OF_SEC(SONAR_BEBOP_FILTER_NARROW_OBSTACLES_TIME)) ) {
274 
275  obstacle_is_in_view = false;
276  }
277 
278  if (obstacle_is_in_view) {
279  return z0;
280  } else {
281  return distance_sonar;
282  }
283 }
284 #endif
285 
sonar_bebop_init
void sonar_bebop_init(void)
Definition: sonar_bebop.c:115
uint16_t
unsigned short uint16_t
Definition: types.h:16
sonar_bebop_spi_t
static struct spi_transaction sonar_bebop_spi_t
Definition: sonar_bebop.c:109
SONAR_BEBOP_MIN_PEAK_VAL
#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
abi.h
spi_transaction::output_length
uint16_t output_length
number of data words to write
Definition: spi.h:152
spi.h
spi_transaction
SPI transaction structure.
Definition: spi.h:148
sonar_bebop.h
Parrot Bebop Sonar driver.
sonar_bebop_spi_d
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
stateGetPositionEnu_f
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
SonarBebop::offset
uint16_t offset
Sonar offset in ADC units.
Definition: sonar_bebop.h:34
stateGetNedToBodyEulers_f
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
SONAR_BEBOP_PEAK_THRESHOLD
#define SONAR_BEBOP_PEAK_THRESHOLD
SONAR_BEBOP_PEAK_THRESHOLD minimum samples from broadcast stop.
Definition: sonar_bebop.c:90
uint32_t
unsigned long uint32_t
Definition: types.h:18
adc_enable
void adc_enable(struct adc_t *adc, uint8_t value)
Start or stop the ADC readings.
Definition: adc_arch.c:101
spi0
struct spi_periph spi0
Definition: spi.c:35
adc.h
arch independent ADC (Analog to Digital Converter) API
SPISelectUnselect
@ SPISelectUnselect
slave is selected before transaction and unselected after
Definition: spi.h:63
SysTimeTimer
#define SysTimeTimer(_t)
Definition: sys_time.h:219
update_median_filter_f
static float update_median_filter_f(struct MedianFilterFloat *filter, float new_data)
Definition: median_filter.h:175
SPITransSuccess
@ SPITransSuccess
Definition: spi.h:99
SONAR_BEBOP_TRANSITION_HIGH_TO_LOW
#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
SONAR_BEBOP_TRANSITION_LOW_TO_HIGH
#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
EnuCoor_f::z
float z
in meters
Definition: pprz_geodetic_float.h:75
spi_transaction::output_buf
volatile uint8_t * output_buf
pointer to transmit buffer for DMA
Definition: spi.h:150
FloatEulers::theta
float theta
in radians
Definition: pprz_algebra_float.h:86
spi_transaction::select
enum SPISlaveSelect select
slave selection behavior
Definition: spi.h:154
get_sys_time_usec
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
USEC_OF_SEC
#define USEC_OF_SEC(sec)
Definition: sys_time.h:210
SonarBebop
Definition: sonar_bebop.h:32
adc_read
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
FloatEulers::phi
float phi
in radians
Definition: pprz_algebra_float.h:85
spi_submit
bool spi_submit(struct spi_periph *p, struct spi_transaction *t)
Submit SPI transaction.
Definition: spi_arch.c:511
SonarBebop::distance
float distance
Distance measured in meters.
Definition: sonar_bebop.h:35
pulse_transition_counter
static uint8_t pulse_transition_counter
Definition: sonar_bebop.c:86
SONAR_BEBOP_ADC_MAX_VALUE
#define SONAR_BEBOP_ADC_MAX_VALUE
SONAR_BEBOP_ADC_MAX_VALUE maximum ADC output (12 bit ADC)
Definition: sonar_bebop.c:60
sys_time.h
Architecture independent timing functions.
uint8_t
unsigned char uint8_t
Definition: types.h:14
adc_buffer
uint16_t adc_buffer[SONAR_BEBOP_ADC_BUFFER_SIZE]
Definition: sonar_bebop.c:146
SONAR_BEBOP_TRANSITION_COUNT
#define SONAR_BEBOP_TRANSITION_COUNT
SONAR_BEBOP_TRANSITION_COUNT number of samples before switching mode.
Definition: sonar_bebop.c:83
sonar_bebop
struct SonarBebop sonar_bebop
Definition: sonar_bebop.c:107
median_filter.h
SPIDss8bit
@ SPIDss8bit
Definition: spi.h:90
MedianFilterFloat
Definition: median_filter.h:142
spi_transaction::input_length
uint16_t input_length
number of data words to read
Definition: spi.h:151
AGL_SONAR_ADC_ID
#define AGL_SONAR_ADC_ID
Definition: abi_sender_ids.h:129
spi_transaction::dss
enum SPIDataSizeSelect dss
data transfer word size
Definition: spi.h:157
SONAR_BEBOP_MAX_TRANS_TIME
#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
SPITransDone
@ SPITransDone
Definition: spi.h:101
spi_transaction::input_buf
volatile uint8_t * input_buf
pointer to receive buffer for DMA
Definition: spi.h:149
sonar_bebop_spike_timer
uint32_t sonar_bebop_spike_timer
Definition: sonar_bebop.c:48
init_median_filter_f
static void init_median_filter_f(struct MedianFilterFloat *filter, uint8_t size)
Definition: median_filter.h:148
SONAR_BEBOP_ADC_BUFFER_SIZE
#define SONAR_BEBOP_ADC_BUFFER_SIZE
SONAR_BEBOP_ADC_BUFFER_SIZE ADC buffer size.
Definition: sonar_bebop.c:63
mode
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
sonar_filt
struct MedianFilterFloat sonar_filt
Definition: sonar_bebop.c:47
state.h
SonarBebop::meas
uint16_t meas
Raw ADC value.
Definition: sonar_bebop.h:33
sonar_bebop_read
void * sonar_bebop_read(void *data)
sonar_bebop_read Read ADC value to update sonar measurement
Definition: sonar_bebop.c:150
SysTimeTimerStart
#define SysTimeTimerStart(_t)
Definition: sys_time.h:218
spi_transaction::status
enum SPITransactionStatus status
Definition: spi.h:162
SONAR_BEBOP_INX_DIFF_TO_DIST
#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