Bogusław Kempny

Ultradźwiękowy pomiar odległości

×
Autor adres English
Początek HC-SR04 LCD Kamera fork() sms strfry() GPIO impulsy Klawiatura Brama GPIO PWM SG90 RFID Grafolog RCP Shutdown Temperatura ....











No to mamy takie cudo:

Kupiliśmy sobie, dla zabawy, z ciekawości, z zamiarem użycia w poważnym projekcie...
Kosztował niewiele, kilka złotych. Co z tym możemy zrobić?

Zasada działania jest prosta. Ma tylko 4 piny, do jednego podłączamy zasilanie, +5V, do drugiego masę.
Dwa pozostałe służą do pomiaru odległości. Po podaniu impulsu TTL 5V na pin "Trig" czujnik wysyła 8 impulsów ultradźwiękowych o częstotliwości 40kHz, odbiera odbite od przeszkody echo po czym na pin "Echo" wysyła impuls o czasie trwania proporcjonalnym do zmierzonej odległości.

No i tu już pierwszy raz pogrymaszę.

Sygnał wyzwalający pomiar według specyfikacji powinien trwać co najmniej 10 mikrosekund. U mnie nawet najkrótsze sygnały jakie byłem w stanie wygenerowac ten pomiar rozpoczynały.

No i druga, ważniejsza sprawa. Na pinie "echo", znów, według dokumentacji, pojawia się sygnał o czasie trwania od 0,15 ms (2,5 cm) do 25 ms (4,3 m), lub 38 ms (6,5 m) w przypadku braku przeszkody odbijającej sygnal.

Poklikajcie w internecie, znajdziecie zasięg pracy od metra do 5 metrów.

Skąd te różnice? Najwyrażniej każdy azjatycki producent robi swoją wersję. Różnią sie już choćby tym, co widac gołym okiem, częstotliwością kwarcu, typem wzmacniacza operacyjnego, układem do sterowania przetwornikiem ultradzwiękowym.

Oznaczenie mikroprocesora tradycyjnie zatarte, ale ewidentnie oprogramowanie w nim bywa różnorakie.

Moje trzy egzemplarze (niestety pochodzące z tego samego źródła) zachowywaly się identycznie, po przekroczeniu 90 cm nie zwiększały już szerokości impulsu. Nawet jak mierzyłem odległość do chmur.

No ale czas zacząć zabawę.

Zwykle w tym miejscu pojawia się odmiana prościutkiego programu dla Arduino, generujemy Strob, czekamy na sygnał Echo, mierzymy czas jego trwania, dzielimy przez 58 i gotowe.
Nie przejmujemy się zakłóceniami.

My jednak podłączymy nasz czujnik do Raspberry.

Są dwie istotne różnice.

Pierwsza to zasilanie.

Raspberry zasilany jest napięciem 5V, ale na pinach łączówki GPIO sygnały są w standardzie CMOS, 3,3V. HC-SR04 pracuje w standardzie TTL, 5V. Podanie napięcia wyższego niż 3,3V na pin GPIO może skończyć się jego uszkodzeniem.

No i druga istotna rzecz, Raspberry ma wielozadaniowy system operacyjny, precyzyjne zmierzenie krótkiego odcinka czasu nie jest możliwe, nasz proces nie ma wyłącznego dostępu do procesora, może czekać na swoją kolej aż inny proces lub jądro zakończy pracę.

Z pierwszym problemem łatwo sobie poradzić, wystarczy sygnał z pinu Echo podpiąć do GPIO przez zwykły dzielnik z dwu rezystorów:

Z drugim problemem zmierzymy się programowo.

No to łączymy druciki:

Piny 29 i 31 wybrałem arbitralnie, można użyć innych pinów GPIO, te jednak zostały użyte w programie, który za chwilę będziemy omawiać, wiec może lepiej odłożyć eksperymenty na później.

Numery i nazwy pinów bez problemu znaleźć można w internecie, wygodna jednak jest komenda

gpio readall

Wyświetli nie tylko nazwy i numery pinów, ale też jak zostały zaprogramowane:
 +-----+-----+---------+------+---+---Pi 3---+---+------+---------+-----+-----+
 | BCM | wPi |   Name  | Mode | V | Physical | V | Mode | Name    | wPi | BCM |
 +-----+-----+---------+------+---+----++----+---+------+---------+-----+-----+
 |     |     |    3.3v |      |   |  1 || 2  |   |      | 5v      |     |     |
 |   2 |   8 |   SDA.1 |   IN | 1 |  3 || 4  |   |      | 5v      |     |     |
 |   3 |   9 |   SCL.1 |   IN | 1 |  5 || 6  |   |      | 0v      |     |     |
 |   4 |   7 | GPIO. 7 |   IN | 1 |  7 || 8  | 1 | ALT5 | TxD     | 15  | 14  |
 |     |     |      0v |      |   |  9 || 10 | 1 | ALT5 | RxD     | 16  | 15  |
 |  17 |   0 | GPIO. 0 |  OUT | 0 | 11 || 12 | 0 | OUT  | GPIO. 1 | 1   | 18  |
 |  27 |   2 | GPIO. 2 |  OUT | 1 | 13 || 14 |   |      | 0v      |     |     |
 |  22 |   3 | GPIO. 3 |  OUT | 0 | 15 || 16 | 0 | IN   | GPIO. 4 | 4   | 23  |
 |     |     |    3.3v |      |   | 17 || 18 | 0 | IN   | GPIO. 5 | 5   | 24  |
 |  10 |  12 |    MOSI |   IN | 0 | 19 || 20 |   |      | 0v      |     |     |
 |   9 |  13 |    MISO |   IN | 0 | 21 || 22 | 0 | IN   | GPIO. 6 | 6   | 25  |
 |  11 |  14 |    SCLK |   IN | 0 | 23 || 24 | 0 | OUT  | CE0     | 10  | 8   |
 |     |     |      0v |      |   | 25 || 26 | 0 | OUT  | CE1     | 11  | 7   |
 |   0 |  30 |   SDA.0 |   IN | 1 | 27 || 28 | 1 | IN   | SCL.0   | 31  | 1   |
 |   5 |  21 | GPIO.21 |   IN | 1 | 29 || 30 |   |      | 0v      |     |     |
 |   6 |  22 | GPIO.22 |   IN | 1 | 31 || 32 | 0 | IN   | GPIO.26 | 26  | 12  |
 |  13 |  23 | GPIO.23 |   IN | 0 | 33 || 34 |   |      | 0v      |     |     |
 |  19 |  24 | GPIO.24 |   IN | 0 | 35 || 36 | 0 | IN   | GPIO.27 | 27  | 16  |
 |  26 |  25 | GPIO.25 |   IN | 0 | 37 || 38 | 0 | IN   | GPIO.28 | 28  | 20  |
 |     |     |      0v |      |   | 39 || 40 | 0 | IN   | GPIO.29 | 29  | 21  |
 +-----+-----+---------+------+---+----++----+---+------+---------+-----+-----+
 | BCM | wPi |   Name  | Mode | V | Physical | V | Mode | Name    | wPi | BCM |
 +-----+-----+---------+------+---+---Pi 3---+---+------+---------+-----+-----+

Żeby ożywić nasz czujnik potrzebujemy:

. Jeśli nie interesuje cię jak program działa, to właściwie już wszystko. Skompilować gotowy program gotowym skryptem i uruchomić:
./cp
./dist

Taka techniczna uwaga, jeśli zobaczysz komunikat

Permission denied

to prawie na pewno potrzebujesz praw użytkownika root.

Zaloguj się jako root, albo poprzedź komendę uwielbianym w środowisku Debiana magicznym słówkiem sudo.

Jeśli jednak przyciągnęła Cię tu ciekawość swiata, zachęcam do zapoznania się z resztą moich wypocin.

Program jest prościutki i krótki, tylko 163 linie, ale korzysta z przerwań (sprzętowych i programowych), mierzy nanosekundowe opóźnienia, no i oczywiście steruje też pinami GPIO.

Standardowe pliki nagłówkowe unixa, ale dodatkowo wiringPi.h:

#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/resource.h>
#include <time.h>
#include <wait.h>
#include <wiringPi.h>

Definiujemy, do których pinów podpięliśmy sygnały Trig i Echo czujnika:

#define TRIGPIN 21
#define ECHOPIN 22

Dwie stałe których użyjemy do walki z zakłóceniami:

#define SERIES 100  //tyle pomiarow
#define SERIES1 5  // tyle najmniejszych i najwiekszych pomiarow odrzucic
                   // jako zaklocenia

Przez tę wartość, według producenta czujnika trzeba podzielić czas trwania sygnału Echo, żeby wyliczyć odległość. Ale prędkość dźwięku w powietrzu zależy od temperatury, w temperaturze 35 ℃ rozchodzi się 11% szybciej, niż przy -25 ℃. Być może wartość tę trzeba będzie w szczególnych przypadkach skorygować:

#define SPEEDDIV 58

Pomijamy definicje zmiennych, są przecież w załączonym programie źródłowym, skupimy się na programie.

int main (void)
{

PID procesu, który uruchomimy przyda się żeby łatwiej włączyć wyświertlanie komunikatów diagnostycznych:

  pid=getpid();
  printf("Start %d\n", pid);fflush(0);

Zwiększamy priorytet naszego procesu. Zwiększy to dokładność pomiaru czasu trwania sygnału Echo. Niewiele i niestety kosztem szybkości innych procesów. Jeśli jednak nasz program do pomiaru odległości jest częścią najważniejszego procesu działającego na naszym Raspberry...

  setpriority(0,0,-20);

Teraz debugging. Chcemy, żeby po wysłaniu do procesu sygnału USR1 zaczął wyświetlać komunikaty diagnostyczne, po wysłaniu USR2 przestał.
A więc kill - USR1 8823 program gada, kill -USR2 8823 przestaje gadać. To 8823 to PID naszego procesu, dlatego wcześniej go odczytaliśmy i wyświetliliśmy.

  signal(SIGUSR1, catch_USR);
  signal(SIGUSR2, catch_USR);

Po otrzymaniu sygnału USR1 lub USR2 wykonana zostanie funkcja catch_USR, która ustawi zmienną debug odpowiednio na 1 lub 0:

void catch_USR(int signal_num)
{
fflush(0);
if(signal_num==SIGUSR1)
debug=1;
if(signal_num==SIGUSR2)
debug=0;
}

Inicjalizacja wiringPI:

  if (wiringPiSetup () == -1)
  exit (1) ;

Programujemy ECHOPIN jako wejściowy, TRIGPIN jako wyjściowy.
ECHOPIN będzie jednak robił coś więcej niż zwykle. Po zmianie stanu sygnału na nim zostanie wygenerowane przerwanie, wykonany zostanie kod w funkcji int_impuls.


    pinMode (ECHOPIN, INPUT) ;
    wiringPiISR (ECHOPIN, INT_EDGE_BOTH, &int_impuls);
    pinMode (TRIGPIN, OUTPUT) ;

Wystarczy w funkcji int_impuls zapamiętać czas, kiedy sygnał stał się jedynką i czas, kiedy spadł do zera, aby w prosty sposób wyliczyć czas trwania impulsu Echo.


void int_impuls(void) {
      if (digitalRead (ECHOPIN) == 1)
       {
clock_gettime(CLOCK_REALTIME, &tim);
       }
      else 
       {
clock_gettime(CLOCK_REALTIME, &tim2);
        impuls=0;
       }
}

Zanim zaczniemy pomiar, jeszcze jedna uwaga. Jeśli proces dostanie przerwanie, sleep (i parę innych funkcji) nie wznowi działania po jego obsłużeniu.
Sleep 10 sekund może trwać sekundę, dlatego w naszym programie użyjemy własnej funkcji:


void Delay(int microsec)
{

 tim3.tv_sec = 0;
 tim3.tv_nsec = microsec * 1000;
     while(nanosleep(&tim3,&tim3)==-1)
          continue;
}

Żeby pozbyć się zakłóceń (może nietoperz przeleciał?) zamiast jednego pomiaru wykonamy 100 (#define SERIES 100):

  for(i=0;<SERIA;i++)
   {

Na pinie TRIGPIN ustawiamy 0, potem na 10 milisekund jedynkę i znów 0, HC-SR04 rozpocznie pomiar:

    digitalWrite (TRIGPIN,0) ; //LOW
    Delay(2);
    digitalWrite (TRIGPIN,1) ; //HIGH
    Delay(10);
    digitalWrite (TRIGPIN,0) ; //LOW

Czekamy na impuls na pinie Echo czujnika. Jeśli się pojawi, funkcja int_impuls zmieni wartość zmiennej impuls na zero.
Na wszelki wypadek nie czekamy w nieskończoność, bo jakby impuls na Echo się nie pojawił?

    impuls=1;
    for(k=1;k<9000;k++)
     { if (impuls==0) break;
       Delay(1);
     } 
    if (impuls==1) 
      {
       printf("Zgubiony impuls\n"); fflush(0);
       tim2.tv_nsec=tim.tv_nsec+1;
      }

Wyliczamy czas trwania impulsu Echo, w mikrosekundach, korygując zapamiętane przez int_impuls czasy jeśli trafiliśmy z pomiarem na koniec sekundy:

    if(tim2.tv_nsec < tim.tv_nsec) 
     tim2.tv_nsec= tim2.tv_nsec+1000000000;

    usec[i]=(tim2.tv_nsec -tim.tv_nsec)/1000;
    usectmp=usec[i];

Jeśli do procesu wysłaliśmy sygnał USR1 wyświetli się wyliczony czas:

if(debug)
printf("%4.1f microsec\n",usectmp);

Czekamy 90 milisekund żeby zanikło ewentualne echo ultradźwięków w pomieszczeniu. To na wypadek, gdyby nasz program działał w pętli (tak jak to robi dist.c)


    Delay(90000);
   }

No to mamy 100 pomiarów. Każdy trochę inny. Który jest najbliższy prawdziwej wartości?
Na początek posortujemy zmierzone wartości:

qsort (usec, SERIA, sizeof (float), compare_float);
  usectmp=0;

Możemy wyświetlić zmierzone wartości. Bywają czasami dziwaczne.

if (debug)
  for (i=0;i<SERIA;i++)
   { printf(" %d: %3.0f \n", i ,usec[i]); fflush(0);}

W tabeli roboczej zliczamy ile było takich samych pomiarów, grupując razem różniące się o pojedyncze mikrosekundy, odrzucamy pewną liczbę najmniejszych i największych:


  for(i=0;i<10000; i++) tab_rob[i]=0;
   
  for (i=SERIA1;i<SERIA-SERIA1;i++)
   {
     j=usec[i]/10;
// Jesli nie wykryto echa, indeks wyjdzie poza tablice
     if(j<10000) tab_rob[j]++;
   }

Szukamy, jaka wartość najwięcej razy wystąpiła w pomiarach, odrzucamy pomiary których było mniej niż 50% tych najczęstszych. Z tego, co pozostało wyliczamy średnią:

// Znajdz maksimum
  max=0 ;
  for (i=0;<10000;i++)
   if (tab_rob[i]>max)
    max=tab_rob[i]; 


  for (i=0;i<10000;i++)
   if (tab_rob[i]<max*0.5)
    tab_rob[i]=0; 

  sum1=0; sum2=0;
  for (i=0;i<10000;i++)
    if (tab_rob[i] >0)
      {
       sum1=sum1+tab_rob[i];
       sum2=sum2+i*tab_rob[i];
      }   

No i mamy wynik pomiaru. Wystarczy go wyświetlić (tu dodatkowo jeszcze czas przeprowadzenia pomiaru wyświetlamy), ale oczywiście można go do innych celów wykorzystać, zapisać w bazie danych, użyć do zapalenia sygnalizatorów, włączenia alarmu zbliżenia się na niedozwoloną odległość...

  usectmp=(float)sum2/sum1;   
  time (&czas);
  loctime = localtime (&czas);
  strftime (data, 50, "%Y-%m-%d %H:%M:%S", loctime);
  printf("%s %4.1f cm\n",data, usectmp/SPEEDDIV*10);
 }
    

Opisany tu program został użyty w praktyce do mierzenia ilości oleju napędowego w zbiorniku zakładowej stacji paliw. W zbiorniku o pojemności 5000 litrów błąd pomiaru jest mniejszy niż 10 litrów.