Simple sonar integration to improve altitude hold (MW2.4)

This forum is dedicated to software development related to MultiWii.
It is not the right place to submit a setup problem.
Software download
Post Reply
Posts: 12
Joined: Thu Apr 21, 2016 8:56 pm

Simple sonar integration to improve altitude hold (MW2.4)

Post by asti »

In this post, I want to summarize the code pieces and knowledge that I acquired in the forums.
It is another implementation of a sonar sensor for altitude hold in MultiWii and the ProMini 328P board.

An archive containing all the code can be found here:
(171.85 KiB) Downloaded 288 times

In the following I will explain how to improve altitude hold using an ultrasonic sensor in a few steps:
0. (Optional but recommended) Prepare a development environment to program MultiWii
1. Add the ultrasonic sensor to the copter frame
2. Add configurable constants
3.1 Change the motor pins on a ProMini 328P board to have a free interrupt pin
3.2 Read the sonar value using interrupts
4. Use the sonar value to calculate the estimated altitude
5. Test the implementation

What is not covered here is adding a flight mode for the sonar. The mode to hold the altitide will still be the "baro mode", although the barometer will only be used to estimate the height if flying above a given range.
Furthermore, autolanding is not covered here. However, it should be fairly simple to implement this with a reliable ground distance.

0. Prepare a development environment to program MultiWii
MultiWii is a rather big and complex program compared to many other Arduino projects.
As such, the official Arduino IDE is not a suited environment to work with MultiWii code in my opinion.
I advise you to get a more powerful IDE such as the Arduino Eclipse IDE. It is available for Windows, Mac and Linux.
This will provide you with tools such as autocomplete (ctrl+space), refactoring (shift+alt+r), find references (shift+ctrl+g), go to declaration (ctrl+left click), and so on.

When you downloaded the Arduino Eclipse IDE, you can import the MultiWii code to a new project in a few steps.
1. Create a new Arduino project (File > New > Other > Arduino > New Arduino Sketch)
(The project must not have the name "MultiWii"!)
2. Delete the INO file in the project (Right click on file > Delete)
3. Import the MultiWii code (Right click on project > Import > Arduino > Import a folder ... )

The buttons to compile and upload are in the toolbar and look the same as in the default Arduino IDE. However, it is necessary to select the project you want to upload (or a file in this project) in the project explorer first.

1. Add the ultrasonic sensor to the copter frame
The sonar measures the time that it takes for an emitted sound to return its echo.
Although this gives pretty good values, it is effected by vibrations. So you don't want your sonar to be rigidly added to your frame. I attached my ultrasonic sensor using an elastic band and a (not too slim) piece of foam (e.g. from a sponge).

Afterwards, connect the trigger pin of the sensor to any free digital pin on the board. The echo pin has to be connected to a digital pin that can receive interrupts.
Here you can find which pins are usable for interrupts:

2. Add configurable constants
There are a few constants that are best placed in config.h

config.h (I put it in Section 6 - Optional Features)

Code: Select all

  /****           GENERIC SONAR                                    ****/

  /* The sonar is used to improve altitude hold. */
  #define SONAR_ECHOPULSE             // uncomment to activate sonar support
  #define SONAR_TRIGGER_PIN PIN6      // trigger pin
  #define SONAR_ECHO_PIN PIN3         // echo pin (must receive interrupts)

  // All ranges are in cm
  #define SONAR_MAX_RANGE 400
  #define SONAR_GROUND_OFFSET 4       // distance of the sonar sensor to the ground when the copter is on the ground
  #define SONAR_IMPACT 0.9f           // determines the impact of the current sonar measurement to previous ones (1: immediately adapt to new value, < 1: smoothly adapt to new value)

  // If using baro and sonar, the following is used to determine which value should be used
    #define SONAR_BARO_FUSION_LC 200                // low cut, fully trustworthy sonar value, below = full sonar
    #define SONAR_BARO_FUSION_HC SONAR_MAX_RANGE    // high cut, highest sonar value that is still reliable, above = full baro

3.1 change the motor pins on a ProMini 328P board to have a free interrupt pin
If your board has an unused interrupt pin, you can skip this section.

Because I have a 328P board from HobbyKing, the interrupt pins I could use are 2 and 3.

Sadly I use pin 2 already for SERIAL_SUM_PPM. And pin 3 is normally used as a motor pin.
I solved this by switching the motor on pin 3 to the analog pin A0. Therefore I changed the config to a HEX6X and adjusted the output mix for the motors, such that the throttle is still splitted between only four motors.
The original threads can be found here: viewtopic.php?f=23&t=2296&p=21416#p21216


Code: Select all

#define HEX6X

Code: Select all

#define A0_A1_PIN_HEX

Output.cpp (near line 1178)

Code: Select all

  #elif defined( HEX6X )
    motor[0] = PIDMIX(-1,+1,-1); //REAR_R -> pin 9
    motor[1] = PIDMIX(-1,-1,+1); //FRONT_R -> pin 10
    motor[2] = PIDMIX(+1,+1,+1); //REAR_L -> pin 11
    motor[3] = 1000; // pin 3.. not used
    motor[4] = 1000; // pin 6 .. not used
    motor[5] = PIDMIX(+1,-1,-1); //FRONT_L -> pin 5 (now A0 because of the option A0_A1_PIN_HEX in config.h)

The motor pin order (which pin is motor[0], motor[1], motor[2], etc.) is defined at the top of Output.cpp

Code: Select all

#if defined(PROMINI)
  uint8_t PWM_PIN[8] = {9,10,11,3,6,5,A2,12};   //for a quad+: rear,right,left,front

Now the motor that was connected to pin 3 has to be connected to A0. In MultiWiiConf a HEX6X layout is displayed but the throttle is split between only the four real motors. So the quad flies normally.

3.2 read the sonar value using interrupts
MultiWii 2.4 has already code prepared to read a sonar. If you get this to work for you, great.
I use my own implementation, which is described below.

With an interrupt pin it is easy to use a sonar sensor.
There is a great tutorial on this here: ... r-Arduino/
I also use this code, but slightly modified.

There are also other threads with a different approach to get the sonar value. Sadly these did not work for me. Nevertheless, my code is based on this thread: viewtopic.php?f=8&t=6282&p=62603#p62603

In your MultiWii directory create the following files (the sonar library):

Code: Select all

* Library for HC SR04 ultrasonic sensor.
* Original code from

#ifndef HC_SR04_H
#define HC_SR04_H

#include <Arduino.h>

#define CM true
#define INCH false

#define DURATION_TO_CM 58
#define DURATION_TO_INCH 148

class HC_SR04 {
    HC_SR04(int trigger, int echo);
    void begin();
    void start();
    bool isFinished(){ return _finished; }
    unsigned int getRange(bool units=CM);
    unsigned int getRange(unsigned int duration, bool unit=CM);
    unsigned int getDuration();
    static HC_SR04* instance(){ return _instance; }
    static void _echo_isr();
    int _trigger, _echo, _int;
    volatile unsigned long _start, _end;
    volatile bool _finished;
    static HC_SR04* _instance;



Code: Select all

* Library for HC SR04 ultrasonic sensor.
* Original code from

#include "HC_SR04.h"

HC_SR04 *HC_SR04::_instance(NULL);

HC_SR04::HC_SR04(int trigger, int echo)
    : _trigger(trigger), _echo(echo), _int(digitalPinToInterrupt(echo)), _finished(false)
  if(_instance==0) _instance=this;   

void HC_SR04::begin(){
  pinMode(_trigger, OUTPUT);
  digitalWrite(_trigger, LOW);
  pinMode(_echo, INPUT); 
  attachInterrupt(_int, _echo_isr, CHANGE);

void HC_SR04::start(){
  digitalWrite(_trigger, HIGH);
  digitalWrite(_trigger, LOW); 

unsigned int HC_SR04::getRange(unsigned int duration, bool unit){
  return duration/((unit == CM) ? DURATION_TO_CM : DURATION_TO_INCH);

unsigned int HC_SR04::getRange(bool unit){
  return getRange((_end-_start), unit);

unsigned int HC_SR04::getDuration(){
  return (micros() - _start);

void HC_SR04::_echo_isr(){
  HC_SR04* _this=HC_SR04::instance();
    case HIGH:
    case LOW:

Besides this lib, which makes it very easy to read an ultrasonic sensor, I also use a lib to accumulate sensor values and then calculate their average and median:
Download the ZIP file from
Extract the archive and copy the directory libraries/RunningMedian to ./RunningMedian in your MultiWii directory.


Now we will add code to read the sensor.
Sensors.cpp (at the beginning)

Code: Select all

  #include "RunningMedian/RunningMedian.h"
  #include "Sonar/HC_SR04.h"

Sensors.cpp (near line 1516)

Code: Select all

// ************************************************************************************************************
// Generic Sonar Support
// ************************************************************************************************************
#if defined(SONAR_ECHOPULSE)
RunningMedian sonarValues = RunningMedian(20);
int16_t sonarAlt;

void Sonar_init() {

void Sonar_update() {
  if (sonar.isFinished()) {
    // The sonar finished successfully.
    // Ignore values if tilted too much.
    if (abs(att.angle[PITCH]) < 150 && abs(att.angle[ROLL]) < 150) {
      int32_t tmpSonarAlt = sonar.getRange(CM) - SONAR_GROUND_OFFSET;
      sonarAlt = sonarValues.getMedian();
  } else if (sonar.getRange(sonar.getDuration(), CM) > SONAR_MAX_RANGE) {
    // The sonar did not finish successfully.
    // Don't trust sonar value and restart ping.
    sonarAlt = sonarValues.getMedian();

inline void Sonar_init() {}
void Sonar_update() {}

In the code above we create an instance of the sonar sensor HC_SR04. Further, we create an instance of the RunningMedian to store multiple sonar values (20) and compute their median.

The update function checks if the sonar is finished.
There are two possibilities:
1. The sonar finishes normally
Then the sonar value is added to the other sonar values and the current sonar altitude is computed as the median. However, this is only done if the copter is not tilted to much (15°). Afterwards we send out the next ping.
2. The sonar does not receive an echo (e.g. because the ground is too far away)
In this case the range of the distance that the sonar is returning will eventually be greater than SONAR_MAX_RANGE. We remember that this measurement is not trustworthy by adding a -1 as distance. Then the sonar altitide is calculated as the median. Because we calculate the median, the final sonar altitude will be -1 (not trustworthy) when more then half of the measurements are -1.
Finally we send out a new ping.

Note that it is important to use the median here and not the average because the values of -1 are not distances but have a semantic meaning. Thus they can not be mixed with the normal distances.

4. Use the sonar value to calculate the estimated altitude
The Inertial Measurement Unit (IMU) is where flight relevant foreces are calculated.
IMU.cpp contains the function in which the altitude as well as the throttle for altitude hold (BaroPID) is calculated.


Code: Select all

#if BARO
RunningMedian baroValues = RunningMedian(30);
int32_t estBaroAlt = 0;
int32_t estSonarAlt = 0;

uint8_t getEstimatedAltitude() {
  int32_t BaroAlt;
  static float baroGroundTemperatureScale, logBaroGroundPressureSum;
  static float vel = 0.0f;
  static uint16_t previousT;
  uint16_t currentT = micros();
  uint16_t dTime;

  dTime = currentT - previousT;
  if (dTime < UPDATE_INTERVAL)
    return 0;
  previousT = currentT;

  if (calibratingB > 0) {
    logBaroGroundPressureSum = log(baroPressureSum);
    baroGroundTemperatureScale = ((int32_t) baroTemperature + 27315)
        * (2 * 29.271267f); // 2 *  is included here => no need for * 2  on BaroAlt in additional LPF

  // baroGroundPressureSum is not supposed to be 0 here
  // see:
  int32_t tmpBaroAlt = (logBaroGroundPressureSum - log(baroPressureSum)) * baroGroundTemperatureScale;
  BaroAlt = baroValues.getAverage();

  // Estimated altitude from barometer
  estBaroAlt = (estBaroAlt * 6 + BaroAlt) >> 3;
  // Estimated altitude from ultrasonic sensor
  if (sonarAlt >= 0 && sonarAlt <= SONAR_MAX_RANGE) {
    estSonarAlt = estSonarAlt * SONAR_IMPACT + sonarAlt * (1 - SONAR_IMPACT);

In this code two separate altitude values are introduced, which are updated using either the barometer (estBaroAlt) or the sonar sensor (estSonarAlt). Note that the estSonarAlt will always be in range 0 to SONAR_MAX_RANGE. A erroneous measurement of the sonar (negative value) will not be used to calculate estSonarAlt.

Furthermore I use the RunningMedian lib here as well for the barometer to improve accuracy. The final baro value is calculated as the average of 30 measurements. Note that in contrast to the sonar sensor, where the median has to be used, the average is suited here. This is because all values from the barometer are altitude values that are equally trustworthy and there are no values with semantic meaning.

Choosing the sonar or baro altitude or a mix of both is done afterwards in the same method.

Code: Select all

#if BARO && !SONAR
  // Use baro altitude
  alt.EstAlt = estBaroAlt;
#elif SONAR && !BARO
  // Use sonar altitude
  alt.EstAlt = estSonarAlt;
#elif SONAR && BARO
  // Use sonar altitude when near ground (lower than SONAR_BARO_FUSION_LC).
  // Use baro altitude when flying high (greater than SONAR_BARO_FUSION_HC)
  // or when there are too many failed measurements (negative value).
  // Mix baro and sonar between SONAR_BARO_FUSION_LC and SONAR_BARO_FUSION_HC.
  if (sonarAlt >= 0 && sonarAlt <= SONAR_BARO_FUSION_LC) {
    alt.EstAlt = estSonarAlt;
  } else if (sonarAlt >= 0 && sonarAlt <= SONAR_BARO_FUSION_HC) {
    // Linearly decrease sonar weight between LC (weight is 1) and HC (weight is 0)
    float fade = ((float) (SONAR_BARO_FUSION_HC - sonarAlt)) / (SONAR_BARO_FUSION_HC - SONAR_BARO_FUSION_LC);
    fade = constrain(fade, 0.0f, 1.0f);

    alt.EstAlt = estSonarAlt * fade + estBaroAlt * (1 - fade);
  } else {
    alt.EstAlt = estBaroAlt;

Mixing the altitude values from the sonar and barometer should work in theory because the baro gives a height in cm, just like the sonar sensor. When the copter is armed, the baro value is reset to 0. So on the ground it should give values around zero, just like the sonar sensor.

At the end of the method there is the calculation of the throttle, which has to be applied to hold the altitude (BaroPID). It is not necessary to change this. The alt hold is improved already because the estimated altitude (alt.EstAlt) is more accurate using the sonar sensor.
However, I removed the "applyDeadband" statements because I trust my sensor values and the sonar is more accurate that +-10cm. And I increased the max values for P and D from +-150 to +-200.


Code: Select all

#if (defined(VARIOMETER) && (VARIOMETER != 2)) || !defined(SUPPRESS_BARO_ALTHOLD)
  int16_t error16 = constrain(AltHold - alt.EstAlt, -300, 300);
//    applyDeadband(error16, 10); //remove small P parametr to reduce noise near zero position
  BaroPID = constrain(([PIDALT].P8 * error16 >> 7), -200, +200);

  errorAltitudeI +=[PIDALT].I8 * error16 >> 6;
  errorAltitudeI = constrain(errorAltitudeI, -30000, 30000);
  BaroPID += errorAltitudeI >> 9; //I in range +/-60

//    applyDeadband(accZ, ACC_Z_DEADBAND);

  static int32_t lastBaroAlt;
  // could only overflow with a difference of 320m, which is highly improbable here
  int16_t baroVel = mul((alt.EstAlt - lastBaroAlt),
      (1000000 / UPDATE_INTERVAL));

  lastBaroAlt = alt.EstAlt;

  baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s
//    applyDeadband(baroVel, 10); // to reduce noise near zero

  // Integrator - velocity, cm/sec
  vel += accZ * ACC_VelScale * dTime;

  // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
  // By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
  vel = vel * 0.985f + baroVel * 0.015f;

  alt.vario = vel;
//    applyDeadband(alt.vario, 5);
  BaroPID -= constrain([PIDALT].D8 * alt.vario >> 4, -200, 200);

To see the calculated values in MultiWiiConf, you can write them to the debug array (e.g. before the return statement):

Code: Select all

  debug[0] = sonarAlt;
  debug[1] = BaroAlt;
  debug[2] = AltHold;
  debug[3] = BaroPID;
  return 1;

5. test the implementation
The presented implementation works well for me.
However, before you go out flying, you should check that the sonar gives reasonable values. In particular you should check that
1. The sonar works on the ground you are flying over (not all surfaces return an echo).
2. The value of sonarAlt is -1 if you hold the sonar in a way such that it can't receive an echo.
3. The sonar values are still reasonable if the motors are spinning fastly (no vibration issues).

If everything works fine, the copter should be able to hold altitude quite well near the ground. And of course it adapts its height when you fly over obstacles or a ramp.

Have fun!

Posts: 2
Joined: Tue Oct 18, 2022 5:55 am

Re: Simple sonar integration to improve altitude hold (MW2.4)



I am looking to build a Hydrofoil catamaran
I have a simple RC with a PWM output that cannot use PPM without changing RC
I have stock of pro mini dev board with AT328 MCU and better use those for their small size.

For this project, it seems that plane mode should be the best as the project is very similar to a plane but I need also to use SONAR to control altitude over the water else the foil jump out of the water and the flight is not good.

I found out your code, many thanks for that to bring me in the right way,
But this uses pins 3 and 6 to drive the sonar explaining that this has to use an interrupt to drive the SONAR, and pin3 is already used for motor output control.

Is there a way to "move" the RUDDER PIN3 output to a free pin ?
Else move THROTTLE pin 2 to somewhere else ?
At that time I have A2, A1 A0 A7 and A6 totally free (from my temporary schematics) or can change(remove) the lipo alarm on D8

Post Reply