Программа C для ультразвукового датчика HC-SR04 не показывает правильное расстояние

Question

У меня есть Raspberry Pi Zero с AlphaBot2, в котором есть ультразвуковой датчик HC-SR04. Реализация с использованием Python работает хорошо. Я хочу реализовать в C, потому что мне нужно связать его с другой программой также на C и по причинам оптимизации. Первое, что я заметил, это то, что код Python использует библиотеку RPi.GPIO, а в C я должен использовать wiringPi или bcm2835. Поэтому я решил использовать wiringPi lib. Моя программа выполняется, но расстояние неверно. Одна вещь, которая отличается от реализаций, которые я нашел в сети, заключается в том, что я использую TRIG 22 и ECHO 27, потому что мой HC-SR04 подключен к AlphaBot2. Я не использую 2 резистора для подключения его к Raspberry Pi. Когда я ставлю какой-то барьер перед датчиком, я получаю только 3 и 5 сантиметров, даже если я перемещаю его на 30 сантиметров.

Distance: 3905724cm
    Distance: 5cm
    Distance: 5cm
    Distance: 5cm
    Distance: 5cm
    Distance: 3cm
    Distance: 3cm
    Distance: 5cm
    Distance: 5cm

Вот мой код:

#include <stdio.h>
#include <stdlib.h>
#include <wiringPi.h>

#include "ultrasonicClient.h"

#define TRUE (1==1)

// HC-SR04 ultrasonic sensor on AlphaBot2 Pi Zero
#define TRIG 22
#define ECHO 27

static volatile long startTimeUsec;
static volatile long endTimeUsec;
double speedOfSoundMetersPerSecond = 340.29;

void recordPulseLength() {
    startTimeUsec = micros();
    while (digitalRead(ECHO) == HIGH);
    endTimeUsec = micros();
}

void setupUltrasonic() {
    wiringPiSetup();
    pinMode(TRIG, OUTPUT);
    pinMode(ECHO, INPUT);

    // TRIG pin must start LOW
    // Initialize the sensor's trigger pin to low. If we don't pause
    // after setting it to low, sometimes the sensor doesn't work right.
    digitalWrite(TRIG, LOW);
    delay(500); // .5 seconds
}

int getCM() {
    // Send trig pulse
    // Triggering the sensor for 10 microseconds will cause it to send out
    // 8 ultrasonic (40Khz) bursts and listen for the echos.
    digitalWrite(TRIG, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG, LOW);

    int now = micros();
    // Wait for echo start
    // The sensor will raise the echo pin high for the length of time that it took
    // the ultrasonic bursts to travel round trip.
    while (digitalRead(ECHO) == LOW && micros() - now < 30000);
    recordPulseLength();

    long travelTimeUsec = endTimeUsec - startTimeUsec;
    double distanceMeters = 100 * ((travelTimeUsec / 1000000.0) * 340.29) / 2;

    //Wait for echo end
    long startTime = micros();
    while (digitalRead(ECHO) == HIGH);
    long travelTime = micros() - startTime;

    //Get distance in cm
    int distance = travelTime * 34000 / 2;

    return distanceMeters * 100;
}

int runUltrasonicClient() {
    int count = 0;
    setupUltrasonic();

    while (count < 60) {
        printf("Distance: %dcm\n", getCM());
        count++;
        delay(500); // 0.5 second
    }
    return 0;
}

Answer 1

Я нашел код для выполнения с помощью bcm2835.

static uint64_t cyclePulse(int trigger, int echo) {
    if (!bcm2835_init())
        return 1;

    // Set RPi pin echo to be an input pin
    bcm2835_gpio_fsel(echo, BCM2835_GPIO_FSEL_INPT);
    // Set RPi pin P1-11 to be an output pin
    bcm2835_gpio_fsel(trigger, BCM2835_GPIO_FSEL_OUTP);

    // Declare the unsigned int timer variables to measure pulses
    uint64_t width, begin, start, end;
    int max = 80, check;

    begin = bcm2835_st_read();

    // Emit pulse for 10 microseconds
    bcm2835_gpio_write(trigger, HIGH); // Set trigger state HIGH
    bcm2835_delayMicroseconds(10);  // Wait 10 microseconds
    bcm2835_gpio_write(trigger, LOW);  // Set trigger state LOW

    // Infinite loop until a pulse is received
    while (bcm2835_gpio_lev(echo) == LOW && check < max) {
        start = bcm2835_st_read();
        check = (int) begin - start;
    }

    // Loop and delay for one microsecond until falling edge detected
    while (bcm2835_gpio_lev(echo) == HIGH) {
        bcm2835_delayMicroseconds(1);
    }
    // Record the ending time of the pulse to get the pulse width
    end = bcm2835_st_read();

    // Get the final with of the pulse
    width = end - start;

    //Close the bcm2835 bridge
    bcm2835_close();

    // Return the total width of the returned pulse
    return width;
}