Skip to content

Commit

Permalink
docs: add article about using sonar
Browse files Browse the repository at this point in the history
  • Loading branch information
okalachev committed Dec 12, 2018
1 parent 06df884 commit aa8ed76
Show file tree
Hide file tree
Showing 6 changed files with 132 additions and 0 deletions.
Binary file added docs/assets/hc-sr04-scheme.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added docs/assets/hc-sr04.jpg
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added docs/assets/rcw-0001.jpg
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added docs/assets/sonar-filtered.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
1 change: 1 addition & 0 deletions docs/ru/SUMMARY.md
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,7 @@
* [Камера \(компьютерное зрение\)](camera.md)
* [Светодиодная лента](leds.md)
* [Визуализация с помощью rviz](rviz.md)
* [Ультразвуковой дальномер](sonar.md)
* [Работа с SITL](sitl.md)
* [Автозапуск ПО](autolaunch.md)
* [Взаимодействие с Arduino](arduino.md)
Expand Down
131 changes: 131 additions & 0 deletions docs/ru/sonar.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,131 @@
# Работа с ультразвуковым дальномером

Ультразвуковой дальномер (*«сонар»*) — это датчик расстояния, принцип действия которого основан на измерении времени распространения звуковой волны (с частотой около 40 Гц) до препятствия и обратно. Сонар может измерять расстояние до 1,5–3 м с точностью до нескольких сантиметров.

## HC-SR04

<img src="../assets/hc-sr04.jpg" alt="hc-sr04" width=200>

### Подключение

Подключите HC-SR04 к Raspberry согласно схеме подключения.

<img src="../assets/hc-sr04-scheme.png" alt="Подключение HC-SR04">

Используйте резисторы на 1 и 2,2 кОм.

> **Hint** Вместо резистора на 2,2 кОм можно использовать два резистора на 1 кОм, соединенные последовательно.
<!-- -->

> **Hint** На Raspberry Pi есть несколько взаимозаменяемых пинов **GND** и **VCC 5V**. Используйте [распиновку](https://pinout.xyz), чтобы найти их.
### Чтение данных

Чтобы считать данных с дальномера HC-SR04 используется библиотека для работы с <abbr title="General-Purpose Input/Output – пины ввода/вывода общего назначения">GPIO</abbr> – [`pigpio`](http://abyz.me.uk/rpi/pigpio/index.html). Эта библиотека предустановлена на [образе Клевера](microsd_images.md), начиная с версии **v0.14**. Для более старых версий образа используйте [инструкцию по установке](http://abyz.me.uk/rpi/pigpio/download.html).

Для работы с `pigpio` необходимо запустить соответствующий демон:

```bash
sudo systemctl start pigpiod.service
```

Вы также можете включить автоматический запуск `pigpiod` при старте системы:

```bash
sudo systemctl enable pigpiod.service
```

Таким образом становится возможным взаимодействие с демоном `pigpiod` из языка Python:

```python
import pigpio
pi = pigpio.pi()
```

> **Hint** См. подробное описание Python API в [документации `pigpio`](http://abyz.me.uk/rpi/pigpio/python.html).
Пример кода для чтения данных с HC-SR04:

```python
import time
import threading
import pigpio

TRIG = 23 # пин, к которому подключен контакт Trig дальномера
ECHO = 24 # пин, к которому подключен контакт Echo дальномера

pi = pigpio.pi()
done = threading.Event()

def rise(gpio, level, tick):
global high
high = tick

def fall(gpio, level, tick):
global low
low = tick - high
done.set()

def read_distance():
done.clear()
pi.gpio_trigger(TRIG, 50, 1)
done.wait(timeout=5)
return low / 58.0 / 100.0

pi.set_mode(TRIG, pigpio.OUTPUT)
pi.set_mode(ECHO, pigpio.INPUT)
pi.callback(ECHO, pigpio.RISING_EDGE, rise)
pi.callback(ECHO, pigpio.FALLING_EDGE, fall)

while True:
# Читаем дистанцию:
print read_distance()

```

### Фильтрация данных

Для фильтрации (сглаживания) данных и удаления [выбросов](https://ru.wikipedia.org/wiki/Выброс_(статистика)) может быть использован [фильтр Калмана](https://ru.wikipedia.org/wiki/Фильтр_Калмана) или более простой [медианный фильтр](https://ru.wikipedia.org/wiki/Медианный_фильтр). Пример реализации медианной фильтрации:

```python
import collections
import numpy

# ...

history = collections.deque(maxlen=10) # 10 - количество сэмплов для усреднения

def read_distance_filtered():
history.append(read_distance())
return numpy.median(history)

while True:
print read_distance_filtered()
```

График исходных и отфильтрованных данных:

<img src="../assets/sonar-filtered.png">

Исходный код ROS-ноды, использовавшейся для построения графика можно найти [на Gist](https://gist.github.com/okalachev/feb2d7235f5c9636802c3cda43add253).

## RCW-0001

<img src="../assets/rcw-0001.jpg" width=200>

Ультразвуковой дальномер RCW-0001 совместим с дальномером HC-SR04. Используйте инструкцию выше для подключения и работы с ним.

## Полет

Пример полетной программы с использованием [simple_offboard](simple_offboard.md), которая заставляет коптер лететь вперед, пока подключенный ультразвуковой дальномер не задетектирует препятствие:

```python
set_velocity(x=0.5, frame_id='fcu_horiz', auto_arm=True) # полет вперед со скоростью 0.5 мс

while True:
if read_distance_filtered() < 1:
# если препятствие ближе, чем в 1 м, зависаем в точке
set_position(x=0, y=0, z=0, frame_id='fcu_horiz')
rospy.sleep(0.1)
```

0 comments on commit aa8ed76

Please sign in to comment.