While I am waiting for my new toys from China, I’m researching other things.

Currently on the official forum are beginning to look at something on using ultrasound sonar altitude control, when it is very close to the ground.

This sonar is capable of measuring a distance between 2 and 450 cm. The problem is that the code you usually find on it is not very efficient because it is based (oversimplifying) to send a signal ultrasound and wait for the signal to bounce off the object you have in front (if any) and return. And knowing what it took and knowing the speed of sound then calculate the distance..

The problem is this does not applies to MultiWii because the processor can not be waiting this time, which is a lot for what we need.

After a couple of hours I’ve gotten it to work based in interruptions, so that now sends the ultrasonic sound and continue execution. When the response is received an interrupt is trigger and the distance is calculated. It is much more efficient.

So far the first step, now we have to integrate it into the MultiWii code, that I have a little big and I have to cram fine. The idea is that when you are close to the ground distance use the sonar distance , and when you’re away use what the barometer says.

The code and test ready to paste in Arduino is:

// 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);
}

Updated with a new and improved method

With the above method were limited to using «attachInterrupt» to pins 2 and 3, which are the only ones who managed that instruction.

With this new version we can use any two digital pins available to us.

#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);
}