Medidor de distancias HC-SR04 por ultrasonidos en MultiWii

Mientras me llegan mis nuevos juguetes de China, voy investigando otras cosas.

Ahora mismo en el foro oficial están empezando a mirar algo sobre el uso de un sonar de ultrasonidos para el control de altura, cuando se está muy cerca del suelo.

Este sonar es capaz de medir una distancia entre 2 y 450 cm. El problema es que el código que se suele encontrar sobre él no es muy eficiente porque se basa (simplificando mucho) en mandar una señal de ultrasonidos y esperar a que la señal rebote en el objeto que haya delante (si lo hay) y vuelva. Y sabiendo lo que ha tardado y sabiendo la velocidad del sonido pues calculamos la distancia.

El problema es que esto no nos vale para MultiWii ya que el procesador no puede estar esperando este tiempo, que es mucho para lo que tenemos entre manos.

Tras un par de horas he conseguido que funcione a base de interrupciones, de manera que ahora se le manda al sonar que envíe los ultrasonidos y continuamos la ejecución. Cuando se recibe la respuesta salta una interrupción que calcula la distancia sabiendo lo que ha tardado. Es mucho más eficiente.

Hasta aquí el primer paso, ahora hay que integrarlo en el código de MultiWii, que se me queda un poco grande y me tengo que empollar muy bien. La idea es que cuando esté cerca del suelo haga caso de la distancia que dice el sonar, y cuando esté lejos haga caso de lo que dice el barómetro.

El código, listo para pegar y probar en Arduino es el siguiente:

// setup pins and variables
#define HC_SR04_echoPin 3 //  (digital 3)
#define HC_SR04_trigPin 7 //  (digital 7)

volatile unsigned long Sonar_starTime = 0;
volatile unsigned long Sonar_endTime = 0;

void Sonar_changeDetected()
{
  if (digitalRead(HC_SR04_echoPin) == HIGH) {
    Sonar_starTime = micros(); // Apunto el tiempo de inicio
  }
  else {
    Sonar_endTime = micros();
  }
}

void Sonar_init()
{
  // Sonar init
   pinMode(HC_SR04_trigPin, OUTPUT);
   pinMode(HC_SR04_echoPin, INPUT);        // Set echo pin as input

   attachInterrupt(1, Sonar_changeDetected, CHANGE);
}

void Sonar_update()
{
  digitalWrite(HC_SR04_trigPin, LOW);      // Send 2ms LOW pulse to ensure we get a nice clean pulse
  delayMicroseconds(2);
  digitalWrite(HC_SR04_trigPin, HIGH); // send 10 microsecond pulse
  delayMicroseconds(10); // wait 10 microseconds before turning off
  digitalWrite(HC_SR04_trigPin, LOW); // stop sending the pulse

  Sonar_starTime = micros(); // Apunto el tiempo de inicio
}

unsigned long Sonar_read()
{
  return (Sonar_endTime - Sonar_starTime) / 58;
}

// Test
void setup()
{
  Serial.begin(9600);
  Sonar_init();
}

void loop()
{
  Serial.print(Sonar_read(), DEC);
  Serial.println(" cm");

  Sonar_update();

  delay(100);
}

Actualización con un nuevo y mejorado método

Con el método anterior estábamos limitados al usar “attachInterrupt” a los pines 2 y 3, que son los únicos que maneja esa instrucción.

Con esta nueva versión podemos usar cualquier pareja de pines digitales que tengamos disponibles.

#define HCSR04_EchoPin         8
#define HCSR04_TriggerPin      12

static int32_t  SonarAlt;

volatile unsigned long HCSR04_starTime = 0;
volatile unsigned long HCSR04_echoTime = 0;
volatile unsigned int HCSR04_waiting_echo = 0;
unsigned int HCSR04_current_loops = 0;

// The cycle time is between 3000 and 6000 microseconds
// The recommend cycle period for sonar request should be no less than 50ms -> 50000 microseconds
// A reading every 18 loops (50000 / 3000 aprox)
unsigned int HCSR04_loops = 18;

void Sonar_init()
{
  // Sonar init

  // this is ATMEGA168 specific, see page 70 of datasheet

  // Pin change interrupt control register - enables interrupt vectors
  // Bit 2 = enable PC vector 2 (PCINT23..16)
  // Bit 1 = enable PC vector 1 (PCINT14..8)
  // Bit 0 = enable PC vector 0 (PCINT7..0)
  PCICR  |= (1<= HCSR04_loops)
  {
    // Send 2ms LOW pulse to ensure we get a nice clean pulse
    digitalWrite(HCSR04_TriggerPin, LOW);      
    delayMicroseconds(2);

    // send 10 microsecond pulse
    digitalWrite(HCSR04_TriggerPin, HIGH); 
    // wait 10 microseconds before turning off
    delayMicroseconds(10);
    // stop sending the pulse
    digitalWrite(HCSR04_TriggerPin, LOW);

    HCSR04_waiting_echo = 1;
    HCSR04_current_loops = 0;
  }
}

// Test
void setup()
{
  Serial.begin(9600);
  Sonar_init();
}

void loop()
{
  Serial.print(SonarAlt, DEC);
  Serial.println(" cm");

  Sonar_update();

  delay(10);
}

Adjuntos

13 pensamientos en “Medidor de distancias HC-SR04 por ultrasonidos en MultiWii

  1. Hola quiero hacer funsionar el hc-sr04 con un micro at89s52, y uso para programar el bascom-8051, y tengo problemas para generr el pulso de 10 micro segundos, hice una función y pierdo los valores de th0 y tl0, me podras dar una mano? gracias.
    Te paso lo que hice
    $crystal = 12000000
    $large

    Declare Sub Pulseint (v As Integer)
    Declare Sub Pulseout (v As Integer)

    Dim V As Integer
    Dim W1 As Single
    Dim W2 As Single

    P0 = 0
    P1 = 0
    P2 = 0
    P3 = 0
    Cls
    Do

    Call Pulseout(v)
    Call Pulseint(v)

    Locate 1 , 1
    Lcd “V=” ; V
    W1 = 2 * V
    W2 = W1 / 115
    Locate 2 , 1
    Lcd W2 ; ” cm”
    Loop
    End

    Sub Pulseout (v As Integer)
    Config Timer0 = Timer , Gate = Internal , Mode = 1
    On Timer0 Timer_0_int
    Mov tl0,#251
    Mov th0,#255
    Enable Interrupts
    Enable Timer0
    Start Timer0
    P0.2 = 1
    Inicio:
    Goto Inicio
    Timer_0_int:
    P0.2 = 0
    End Sub

    Sub Pulseint (v As Integer)

    Config Timer0 = Timer , Gate = External , Mode = 1
    On Timer0 Timer_0_int1
    Mov tl0,#0
    Mov th0,#0
    Enable Interrupts
    Enable Timer0
    P0.2 = 0
    Inicio1:
    If P0.2 = 0 Then Goto Pasa
    Goto Inicio1
    Pasa:
    If P0.2 = 1 Then Goto Medir
    Goto Pasa
    Medir:
    Start Timer0
    Salto:
    If P0.2 = 0 Then Goto Enviar
    Goto Salto
    Enviar:
    Stop Timer0
    V = Th0 * 256
    V = V + Tl0

    Mov tl0,#0
    Mov th0,#0
    Wait 1
    Goto Salida
    Timer_0_int1:
    Print “error”
    Salida:
    End Sub

  2. amigo cordial saludo, estoy haciendo un programa usando éste sensor con la idea q cuando detecte un objeto a 1.5m o menos se me active una salida del micrcontrolador 16f84, acá esta mi codigo pero no me funciona por si depronto me podes colaborar en algo

    mil gracias.

    void main(){
    int centimetros;
    trisb=0;
    trisa=255;
    porta=0;
    portb=0;
    centimetros=0;
    while(1){

    portb=2;
    delay_us(10);
    portb=0;

    while(porta.f3==0)

    while(porta.f3==1) { // eco
    centimetros++;
    delay_us(58);

    }

    if(centimetros=150){
    portb=16;
    delay_ms(2000);
    portb=0;
    centimetros=0;
    delay_ms(500);
    }
    else
    portb=0;
    delay_ms(500);

    } }

  3. que tal mucho gusto, quisiera saber si sabes programar un sensor ultrasonico en pic simulator ide o en si el funcionamiento de este en un Pic 16f, ya que no me esta funcionando a mi manera de programarlo, gracias de antemano por tu respuesta Oscar.

  4. gg, nuestro profesor nos dijo que ese arduino es solo para ignorantes , es solo para gente cuyo cerebro es tan diminuto que no le da para programar en un buen lenguaje, para que usar esa pendejada de arduino si todo eso se puede hacer en un buen lenguaje de programacion como el pic c , mikro c , proton, etc…. osea les mandan de tarea un proyecto completo en la Universidad y se compran unos 10 modulos de arduino para hacer toda la tares ? Dios, acá en País en una conferencia de masters en electrónica dijo que esos modulos lo usan solo las personas que no saben programar, osea es solo para niños para aprendizaje y que jamás es usado por gente inteligente—

    • Pues tu profesor dijó muchas tonterías, punto. No voy a entrar a explicar porque digo esto porque creo que es más que obvio.

Deja un comentario

Tu dirección de correo electrónico no será publicada. Los campos obligatorios están marcados con *

Loading Facebook Comments ...