Paparazzi UAS  v5.8.2_stable-0-g6260b7c
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 
27 #include "sonar_bebop.h"
28 #include "generated/airframe.h"
29 #include "mcu_periph/adc.h"
30 #include "mcu_periph/spi.h"
31 #include "subsystems/abi.h"
32 #include <pthread.h>
34 
35 #ifdef SITL
36 #include "state.h"
37 #endif
38 
39 
41 static uint8_t sonar_bebop_spi_d[16] = { 0xF0,0xF0,0xF0,0xF0,0xF0,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00 };
43 static pthread_t sonar_bebop_thread;
44 static void *sonar_bebop_read(void *data);
45 
46 void sonar_bebop_init(void)
47 {
48  sonar_bebop.meas = 0;
49  sonar_bebop.offset = 0;
50 
58 
59  int rc = pthread_create(&sonar_bebop_thread, NULL, sonar_bebop_read, NULL);
60  if (rc < 0) {
61  return;
62  }
63 }
64 
68 static void *sonar_bebop_read(void *data __attribute__((unused)))
69 {
70  while(true) {
71 
72 #ifndef SITL
73  uint16_t i;
74  uint16_t adc_buffer[8192];
75 
76 
77  /* Start ADC and send sonar output */
78  adc_enable(&adc0, 1);
82  adc_read(&adc0, adc_buffer, 8192);
83  adc_enable(&adc0, 0);
84 
85  /* Find the peeks */
86  uint16_t start_send = 0;
87  uint16_t stop_send = 0;
88  uint16_t first_peek = 0;
89  uint16_t lowest_value = 4095;
90  for(i = 0; i < 8192; i++){
91  uint16_t adc_val = adc_buffer[i]>>4;
92  if(start_send == 0 && adc_val == 4095)
93  start_send = i;
94  else if(start_send != 0 && stop_send == 0 && adc_val != 4095)
95  {
96  stop_send = i-1;
97  i += 300;
98  continue;
99  }
100 
101  if(start_send != 0 && stop_send != 0 && first_peek == 0 && adc_val < lowest_value)
102  lowest_value = adc_val;
103  else if (start_send != 0 && stop_send != 0 && adc_val > lowest_value + 100) {
104  first_peek = i;
105  lowest_value = adc_val - 100;
106  }
107  else if(start_send != 0 && stop_send != 0 && first_peek != 0 && adc_val+100 < (adc_buffer[first_peek]>>4)) {
108  break;
109  }
110  }
111 
112  /* Calculate the distance from the peeks */
113  uint16_t diff = stop_send - start_send;
114  int16_t peek_distance = first_peek - (stop_send - diff/2);
115  if(first_peek <= stop_send || diff > 250)
116  peek_distance = 0;
117 
118  sonar_bebop.distance = peek_distance / 1000.0;
119 #else // SITL
121  Bound(sonar_bebop.distance, 0.1f, 7.0f);
122  uint16_t peek_distance = 1;
123 #endif // SITL
124 
125  usleep(10000);
126 
127  if(peek_distance > 0)
128  {
129  // Send ABI message
130  AbiSendMsgAGL(AGL_SONAR_ADC_ID, sonar_bebop.distance);
131 
132 #ifdef SENSOR_SYNC_SEND_SONAR
133  // Send Telemetry report
135 #endif
136  }
137  }
138 
139  return NULL;
140 }
141 
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
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
static pthread_t sonar_bebop_thread
Definition: sonar_bebop.c:43
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:42
#define AGL_SONAR_ADC_ID
Main include for ABI (AirBorneInterface).
Parrot Bebop Sonar driver.
SPI transaction structure.
Definition: spi.h:142
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:702
volatile uint8_t * output_buf
pointer to transmit buffer for DMA
Definition: spi.h:144
static void * sonar_bebop_read(void *data)
Read ADC value to update sonar measurement.
Definition: sonar_bebop.c:68
enum SPISlaveSelect select
slave selection behavior
Definition: spi.h:148
arch independent ADC (Analog to Digital Converter) API
static uint8_t sonar_bebop_spi_d[16]
Definition: sonar_bebop.c:41
Architecture independent SPI (Serial Peripheral Interface) API.
Definition: spi.h:84
bool_t spi_submit(struct spi_periph *p, struct spi_transaction *t)
Submit a spi transaction.
Definition: spi_arch.c:48
struct spi_periph spi0
Definition: spi.c:35
float distance
Distance measured in meters.
Definition: sonar_bebop.h:35
signed short int16_t
Definition: types.h:17
uint16_t input_length
number of data words to read
Definition: spi.h:145
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
void sonar_bebop_init(void)
Definition: sonar_bebop.c:46
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
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:40
enum SPITransactionStatus status
Definition: spi.h:156