Altitude hold with MB1242 sonar

3689677345?profile=originalThis video shows the 250mm quadcopter from my previous post, holding altitude by means of its MaxBotix MB1242 ulatrasonic rangefinder (sonar). The Flip32 flight controller onboard is running a modified version of the Baseflight firmware to support the MB1242.  Original Baseflight requires a barometer for alt-hold, and then fuses it with data from an HC-SR04 sonar if one is available.  So I removed the baro requirement and replaced the HC-SR04 code with code that uses the MB1242.

The real-time plot (made in Python with RealTimePlotter) shows AGL (Above Ground Level) in centimeters (blue) as well as the desired altitude that locks in when I flip a switch on the transmitter (red). The desired altitude starts out with its value from a previous run, which has no effect until I hit the switch, at which point it resets to the current AGL. The bottom plot is the PID (Position / Integral / Derivative) -based correction to the throttle in support of altitude hold, which likewise has no effect until I hit the switch.

Although the vehicle holds low altitude (here, around a meter) pretty well, the values being displayed are quite a bit off from what you expect to see (vehicle is holding AGL at around 100cm, though reported AGL is 60cm, with locked-in AGL at 40cm). At higher altitudes, big transient jumps in the sonar-measured AGL can cause the vehicle to jump slightly. So my next steps are to (1) figure out the discrepancy between the observed behavior and displayed signals; (2) add an Extended Kalman Filter to smoothe out transient jumps in the sonar signal.

E-mail me when people leave their comments –

You need to be a member of diydrones to add comments!

Join diydrones


  • Great work Simon!

  • Ach, sorry Linus, I misunderstood you and I keep forgetting, YouTube blocks DRMed content in some countries! I will try to to remember for next vid.  Meanwhile if you can't view the video, let me know and I'll remove the soundtrack.

  • Thanks, Linus! Good suggestion on the music, but I always buy each song from Amazon before using it.  

  • Developer

    Maybe you might consider choosing free music for your video next time. Else i have to say, geat stuff! As always Simon. Thanks for sharing

  • Cool, thanks for the tip Randy!

  • Developer

    What we do in Copter is maintain a separate target sonar alt and then use the error between the target sonar alt and actual sonar target alt to adjust the target baro+accel (i.e EKF) altitude.  The ultimate solution, as perhaps you're suggesting, is to have the EKF maintain separate alt-above-sea-level and alt-above-terrain values.

  • yes, the sonar is tricky.

    in my tests the ultrasoinic devices where a tad better as the maxbotix devices.

    will be very interesting to see the your nice kalman filter in action.



  • Update #2: built a more manageable rig for securing the vehicle level to the ground:


    In addition to the carpet shown above, I collected measurements over the tiled floor from the video.  Carpet vs. tile seemed to make no difference, so I just ran a little linear regression on the sonar measurements vs. ground truth (as measured with a tape measure from the downward-facing surface of the sonar barrel to the floor):  Got slope = 0.933 intercept = -2.894 rvalue = 1.000.  So that'll go into the part of the firmware that acquires data from the MB1242.

    For any Pythonistas reading this, here's the little program I wrote to calculate and plot the regression:

    from scipy.stats import linregress
    import numpy as np
    import matplotlib.pyplot as plt

    groundtruth = np.array([64, 123, 63, 104, 66, 397, 26, 26])
    sonar = np.array([57, 111, 55, 94, 59, 368, 22, 22])

    slope, intercept, rvalue, _, _ = linregress(groundtruth, sonar)

    print('slope = %3.3f intercept = %3.3f rvalue = %3.3f' %
    (slope, intercept, rvalue))

    plt.plot(groundtruth, sonar, 'o')
    plt.xlabel('Ground Truth (cm)')
    plt.ylabel('MB1242 Sonar (cm)')

  • Using apparatus below I read AGL as 123 cm off a tape measure.  (Vehicle is actually more level than it appears in photo.) Logfile shows undershoot plus noise.  Hence alt-hold value is likely to lock on to a value significantly higher or lower than the actual AGL at the instance when I flip the switch.



This reply was deleted.