Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
34
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 "modules/core/abi.h"
41#include <pthread.h>
42#include <unistd.h>
43#include "modules/datalink/downlink.h"//FIXME, include only when link need
44
46
49
53#define SONAR_BEBOP_INX_DIFF_TO_DIST 340./(2.*160000.)
54
56#define SONAR_BEBOP_ADC_MAX_VALUE 4095
57
59#define SONAR_BEBOP_ADC_BUFFER_SIZE 8192
60
66
68#ifndef SONAR_BEBOP_TRANSITION_HIGH_TO_LOW
69#define SONAR_BEBOP_TRANSITION_HIGH_TO_LOW 0.8
70#endif
71
73#ifndef SONAR_BEBOP_TRANSITION_LOW_TO_HIGH
74#define SONAR_BEBOP_TRANSITION_LOW_TO_HIGH 1.2
75#endif
76
78#ifndef SONAR_BEBOP_TRANSITION_COUNT
79#define SONAR_BEBOP_TRANSITION_COUNT 7
80#endif
81
83
85#ifndef SONAR_BEBOP_PEAK_THRESHOLD
86#define SONAR_BEBOP_PEAK_THRESHOLD 50
87#endif
88
90#ifndef SONAR_BEBOP_MIN_PEAK_VAL
91#define SONAR_BEBOP_MIN_PEAK_VAL 512 // max value is 4096
92#endif
93
95#define SONAR_BEBOP_MAX_TRANS_TIME 370
96
101static uint8_t sonar_bebop_spi_d[2][16] = {{ 0xF0, 0xF0, 0xF0, 0xF0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
102 { 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0x00, 0x00, 0x00, 0x00 }};
104
106void *sonar_bebop_read(void *data);
107#if SONAR_BEBOP_FILTER_NARROW_OBSTACLES
108static float sonar_filter_narrow_obstacles(float distance_sonar);
109#endif
110
111static pthread_mutex_t sonar_mutex = PTHREAD_MUTEX_INITIALIZER;
112
114{
115 mode = 0; // default mode is low altitude
117
118 sonar_bebop.meas = 0;
119 sonar_bebop.offset = 0;
120 sonar_bebop.available = false;
121
122 /* configure spi transaction */
127 sonar_bebop_spi_t.output_length = 16;
128 sonar_bebop_spi_t.input_buf = NULL;
129 sonar_bebop_spi_t.input_length = 0;
130
132#if SONAR_BEBOP_FILTER_NARROW_OBSTACLES
134#endif
135
136#if USE_SONAR
137 pthread_t tid;
138 pthread_create(&tid, NULL, sonar_bebop_read, NULL);
139#ifndef __APPLE__
140 pthread_setname_np(tid, "sonar");
141#endif
142#endif
143 pthread_mutex_init(&sonar_mutex, NULL);
144}
145
147{
148 pthread_mutex_lock(&sonar_mutex);
149 if (sonar_bebop.available) {
150#if SONAR_COMPENSATE_ROTATION
151 float phi = stateGetNedToBodyEulers_f()->phi;
152 float theta = stateGetNedToBodyEulers_f()->theta;
153 float gain = (float)fabs( (double) (cosf(phi) * cosf(theta)));
154 sonar_bebop.distance = sonar_bebop.distance * gain;
155#endif
156
157#if SONAR_BEBOP_FILTER_NARROW_OBSTACLES
158 sonar_bebop.distance = sonar_filter_narrow_obstacles(sonar_bebop.distance);
159#endif
160
161 // Send ABI message
162 uint32_t now_ts = get_sys_time_usec();
163 AbiSendMsgAGL(AGL_SONAR_ADC_ID, now_ts, sonar_bebop.distance);
164
165#ifdef SENSOR_SYNC_SEND_SONAR
166 // Send Telemetry report
167 DOWNLINK_SEND_SONAR(DefaultChannel, DefaultDevice, &sonar_bebop.meas, &sonar_bebop.distance);
168#endif
169
170 sonar_bebop.available = false;
171 }
172 pthread_mutex_unlock(&sonar_mutex);
173}
174
179void *sonar_bebop_read(void *data __attribute__((unused)))
180{
181 while (true) {
182 uint16_t i;
183
184 /* Start ADC and send sonar output */
185 adc_enable(&adc0, 1);
187 usleep(100); //Sonar crashes without the delay once in a while :(
190 while (sonar_bebop_spi_t.status != SPITransSuccess);
191 usleep(100); //Sonar crashes without the delay once in a while :(
193 adc_enable(&adc0, 0);
194
195 /* Find the peaks */
196 uint16_t start_send = 0;
197 uint16_t stop_send = 0;
198 uint16_t first_peak = 0;
199 uint16_t lowest_value = SONAR_BEBOP_ADC_MAX_VALUE;
200 uint16_t peak_value = 0;
201
202 for (i = 0; i < SONAR_BEBOP_ADC_BUFFER_SIZE; i++) {
203 uint16_t adc_val = adc_buffer[i] >> 4;
204 if (start_send == 0 && adc_val >= SONAR_BEBOP_ADC_MAX_VALUE) {
205 start_send = i;
206 } else if (start_send && stop_send == 0 && adc_val < SONAR_BEBOP_ADC_MAX_VALUE - SONAR_BEBOP_MIN_PEAK_VAL) {
207 stop_send = i - 1;
209 continue;
210 }
211
212 if (start_send != 0 && stop_send != 0 && first_peak == 0 && adc_val < lowest_value) {
213 lowest_value = adc_val; // find trough from initial broadcast signal
214 } else if (start_send != 0 && stop_send != 0 && adc_val > lowest_value + 100 && adc_val > peak_value) {
215 first_peak = i;
216 peak_value = adc_val;
217 }
218 }
219
220 /* Calculate the distance from the peeks */
221 uint16_t diff = stop_send - start_send;
222 if (diff && diff < SONAR_BEBOP_MAX_TRANS_TIME
223 && peak_value > SONAR_BEBOP_MIN_PEAK_VAL) {
224 pthread_mutex_lock(&sonar_mutex);
225 sonar_bebop.meas = (uint16_t)(first_peak - (stop_send - diff / 2));
227
228 // set sonar pulse mode for next pulse based on altitude
229 if (mode == 0 && sonar_bebop.distance > SONAR_BEBOP_TRANSITION_LOW_TO_HIGH) {
231 mode = 1;
233 }
234 } else if (mode == 1 && sonar_bebop.distance < SONAR_BEBOP_TRANSITION_HIGH_TO_LOW) {
236 mode = 0;
238 }
239 } else {
241 }
242 sonar_bebop.available = true;
243 pthread_mutex_unlock(&sonar_mutex);
244 }
245
246 usleep(10000); //100Hz FIXME: use SYS_TIME_FREQUENCY and divisor
247 }
248 return NULL;
249}
250
251#if SONAR_BEBOP_FILTER_NARROW_OBSTACLES
252
253#ifndef SONAR_BEBOP_FILTER_NARROW_OBSTACLES_JUMP
254#define SONAR_BEBOP_FILTER_NARROW_OBSTACLES_JUMP 0.4f
255#endif
256PRINT_CONFIG_VAR(SONAR_BEBOP_FILTER_NARROW_OBSTACLES_JUMP)
257
258#ifndef SONAR_BEBOP_FILTER_NARROW_OBSTACLES_TIME
259#define SONAR_BEBOP_FILTER_NARROW_OBSTACLES_TIME 1.0f
260#endif
261PRINT_CONFIG_VAR(SONAR_BEBOP_FILTER_NARROW_OBSTACLES_TIME)
262
263static float sonar_filter_narrow_obstacles(float distance_sonar)
264{
265 static float previous_distance = 0;
266 static float z0 = 0;
267 bool obstacle_is_in_view = false;
268 float diff_pre_cur = distance_sonar - previous_distance;
269 if (diff_pre_cur < -SONAR_BEBOP_FILTER_NARROW_OBSTACLES_JUMP) {
270 z0 = previous_distance;
271 obstacle_is_in_view = true;
273 }
274 previous_distance = distance_sonar;
275 float time_since_reset = SysTimeTimer(sonar_bebop_spike_timer);
276 if ((diff_pre_cur > SONAR_BEBOP_FILTER_NARROW_OBSTACLES_JUMP) || (time_since_reset > USEC_OF_SEC(SONAR_BEBOP_FILTER_NARROW_OBSTACLES_TIME)) ) {
277
278 obstacle_is_in_view = false;
279 }
280
281 if (obstacle_is_in_view) {
282 return z0;
283 } else {
284 return distance_sonar;
285 }
286}
287#endif
288
Main include for ABI (AirBorneInterface).
#define AGL_SONAR_ADC_ID
arch independent ADC (Analog to Digital Converter) API
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
#define SONAR_BEBOP_TRANSITION_COUNT
Definition disco.h:108
#define SONAR_BEBOP_TRANSITION_LOW_TO_HIGH
Definition disco.h:107
#define SONAR_BEBOP_TRANSITION_HIGH_TO_LOW
Definition disco.h:106
#define SONAR_BEBOP_PEAK_THRESHOLD
Definition disco.h:109
float phi
in radians
float theta
in radians
bool spi_submit(struct spi_periph *p, struct spi_transaction *t)
Submit SPI transaction.
Definition spi_arch.c:548
struct spi_periph spi0
Definition spi.c:35
@ SPITransSuccess
Definition spi.h:95
@ SPITransDone
Definition spi.h:97
@ SPISelectUnselect
slave is selected before transaction and unselected after
Definition spi.h:59
@ SPIDss8bit
Definition spi.h:86
SPI transaction structure.
Definition spi.h:144
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition state.h:1325
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
void adc_enable(struct adc_t *adc, uint8_t value)
Start or stop the ADC readings.
Definition adc_arch.c:101
static float update_median_filter_f(struct MedianFilterFloat *filter, float new_data)
static void init_median_filter_f(struct MedianFilterFloat *filter, uint8_t size)
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
static Butterworth2LowPass sonar_filt
Definition sonar_adc.c:92
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...
#define SONAR_BEBOP_ADC_MAX_VALUE
SONAR_BEBOP_ADC_MAX_VALUE maximum ADC output (12 bit ADC).
Definition sonar_bebop.c:56
void sonar_bebop_init(void)
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:65
uint32_t sonar_bebop_spike_timer
Definition sonar_bebop.c:48
#define SONAR_BEBOP_ADC_BUFFER_SIZE
SONAR_BEBOP_ADC_BUFFER_SIZE ADC buffer size.
Definition sonar_bebop.c:59
void * sonar_bebop_read(void *data)
sonar_bebop_read Read ADC value to update sonar measurement
#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:91
static struct spi_transaction sonar_bebop_spi_t
#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:95
static uint8_t pulse_transition_counter
Definition sonar_bebop.c:82
#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:53
uint16_t adc_buffer[SONAR_BEBOP_ADC_BUFFER_SIZE]
struct SonarBebop sonar_bebop
static pthread_mutex_t sonar_mutex
void sonar_bebop_event(void)
Parrot Bebop Sonar driver.
Architecture independent SPI (Serial Peripheral Interface) API.
Architecture independent timing functions.
#define SysTimeTimerStart(_t)
Definition sys_time.h:329
#define USEC_OF_SEC(sec)
Definition sys_time.h:322
#define SysTimeTimer(_t)
Definition sys_time.h:330
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.