Low altitude hold, auto takeoff, autolanding with kk or Hk v2.1 + Arduino + Sonar

This post is under constant update until i find a complete solution

I’m publishing it even if it is not complete because i’ve found some working (into compiler) code and probably there are a lot of people out there in my same situation and we can help each other. So…

UPDATES 2012-12-06

    • I made all connections
    • Reviewed the code and renamed vars so you can actually understand while you read
    • I compiled and uploaded the sketch (the first one has errors)
    • I added an LED for signaling the status changes
    • I’m working on thelemetry (battery voltage)
    • I switched to a new ping library
    • I released new code
    • I updated this discussion about arduino + sonar/ping + kk/hk board
    • It works with any configuration (tricopter, Y4, Y6, xcopter, quadcopter, hexcopter, octocopter, 12 rotors and so…)


  • i’m planning to switch to another arduino board with ethernet ad sd slot in order to connect it to a raspberry pi and streaming the video (I will write about this too)
  • also i have a 10 dof and i’m not shure if it could help actually or if it could work better as standalone


Latest (2012-12-06): arduino-kk-altitude-hold
New ping Library: NewPing Arduino Library

Follows original post

Remember:LIFE IS A BETA!

So this is the new code, use it wisely:

Sonar hc-sr04

As promised in one of my my previous articles, I want to implement a ping/sonar sensor reading and reacting system on my quadcopter in order to obtain assistance during critical tasks like taking off, landing, and holding the altitude, wich is measured when I will trigger aux channel added for this purpose to my RC RX. Also some coloured leds can help understanding what’s going on.

So the first conclusion is that I’m going to use at least one more channel than usual (better 2). I can imagine with two added switches to my setup: one for enabling/disabling the altitude holding loop, another one to switch between to different loop timing interval in order to use this also to take off and/or (i don’t know how yet) landing.

This i my flamewheel clone I’m going to use for this project.
Picture of my FPV Quadcopter Flamewheel 450 Clone
Its weight, full loaded (included fpv gear) is 1.2 Kg. I had to use such a heavy but small platform because I live in Haarlem, Holland where winds give their best almost always and I don’t want loose days of practice because my tiny quadcopter flown away in the wind.


This stuff is what I should fit inside my quadcopter:

1 Meduino pro mini (no usb port thus we will program this one with another arduino).

Meduino 3-5v

Good boy, 3-5v selectable, some differences in pins order with arduino pro mini, light-weight, a shame to use such a heavy breadboard (see below), but I don’t really like the word “definitive” so I’m not going to put my hand to the solder.



1 Sonard hc-sr04, very cheap, does the job.

Sonar hc-sr04

Quiet big sensor, the real problem is to find a place suitable for this device on my aircraft. See below for modification I have made to the frame adding a layer for holding devices AND to protect my flight controller against unwanted manouvres, as we are testing something that potentially can give full throttle or suddenly shut down motors with predictable consequences.


1 Mini Breadboard, because I don’t want to solder for the moment, until it is already extensively tested.

The Mini Breadboard

This will hold my arduino clone tight to the RC receiver, so no long cables are needed. With double sided tape it’s perfect because it is similar to the Turnigy receiver in shape and size.



So i need a new layer to add this stuff without affecting performance because all this stuff together, will probably, vibrate a little much more than the flight controller can tolerate. To achieve this i took apart a quadcopter frame (X600), crappy enough, it was waiting for such a moment to be useful. What i’ve done, basically is to prolongue the board nylon screw with anoter nylon extender and then i used nuts and bolts to keep in place the new plate coming from the corp on the old frame (far enough from the board), and then this bolts were screwed to the nylon board holders. But an image is worth a million words so it follows:
The first image is about the starting point, you can see 3 holders out of 4 in place (3 are sufficent also this frame is not 50mm board friendly, at least, not the upper part).


As you can see although I added this clearance for the new upper plate, the vertical mounted gyro of the hk board is still taller than the nylon extenders. But I don’t have further extenders and it’s sunday: help, brain! The answer is to use some m2 x~20mm bolts, using nuts to keep the new plate distant enough from the flight controller. This is the result:

Now next problem to deal with is the housing of new components, that’s why I used other parts (arms) of the old quadcopter to make a rude landing skid to gain more clearance between the bottom plate and the ground so it can fit some way the sonar device. Wheight is not a problem, yet.

The code:

I’ve looked around to find some piece of code to adapt to my project (mostly for the ppm part & distance/throttle algorythm,curves etc.):

I’ve found this graduation project (http://thanoseleftherakos.blogspot.nl/2011/11/quadcopter.html) that could possibly help (another sonar device here as well) but this guy no longer update his post on Diydrones, so I’ll try to make it work.
I found something on diydrones.com but in this case the code should work but the sonar device used is different, so i should check for some specific in output and try to make it work. This leads here: http://pastebin.com/c1G8qDCm and was taken as starting point for my project.
Here is the Arduino Sketch not tested in real life yet, it just compiles and to me it’s enough, each problem I will encounter, will have a solution for sure.

Please keep in mind this code is here for reference purposes only.

Download the latest zip package instead.


Hi, i'm glad you downloaded my arducode, please remember to take a look to my website http://giuseppeurso.net 
where you will find updates and troubleshooting tips.

Arduino pin 3 -> rc receiver chan 3 (signal only- the white or the orange cable)
Arduino pin 6 -> rc receiver chan 5 (or the channel you are going to use from your tx)
Arduino pin 9 -> rc throttle channel on flight controller
Arduino pin 10 -> LED for signaling the altitude hold mode (led gnd to arduino gnd)
Arduino pin 11 -> Sonar Eco Pin
Arduino pin 12 -> Sonar trigger pin
Arduino 5V -> Sonar vcc (you can use any 5v source as all these components have actually common ground)
Arduino GND -> Sonar Gnd (as previous) 



#define TRIGGER_PIN  12  // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN     11  // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 400 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
#define MINIMUM_ALTITUDE 120 //For takeoff
#define TX_THRESHOLD 50 //tollerance in PPM to be use while in altitude hold to autoland
my minimum read value for the throttle channel, without trim is ~ 972, middle ~1330, max ~1810

Since i've noticed that actually if altitude hold is switched on and the quad goes below the reference altitude, 
it takes too much to speed up the motors and too much to slow down resulting in a flown away quadcopter, so i think there are 2 different mods:
1) Calculate some increment in throttle in the way it is strong at beginning (going below altitude reference).

To do this i need to refer to a timing interval and then within this interval i can trigger a counter and use this counter within the 2° loop
to boost the signal at beginning in a case and at the end in the other (this could lead to a very very fast auto takeoff followed by altitude holding)

so firstly setup some vars: for example let's setup timing intervals:

#define MAIN_CLOCK 30 // these are milliseconds unless you switch from delay() to delayMicroseconds ...  this is use for main loop
//There are 1000 microseconds in a millisecond, and 1.000.000 microseconds in a second.  
//#define RESOLUTION 50 //

and soft at the and (just like manually) but this is not the same in every situations, because it can fall at different speed so it has to be related with speed
2) Apply a second algorythm to the first in order to take in account this value when outputting the throttle signal.

Obviously this very same thing happend if conversely we apply this to the machine that fly over the reference: it takes long time to react and once reacted it takes too long to slow don motors.

In a first place i thought it was related with the weight (in some way it is, but not so directly), in fact it has better to do with speed and you know what? that's why humans invented accelerometers. 
But this is nothing serious it is just a park for forever kids adults, so i am aware that out there i can buy som wii-whatever, i just want to reinvent the wheel, who knows...


NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
Servo myThrottleChOut;

int throttlePin1 = 9; //output 
int pinReadCh6 = 6; //Reads channel 6 from RX
int pinReadCh3 = 3; //Reads channel 3 from RX
int led = 10;
int takeoff = 0;
unsigned long throttle_read;
unsigned long alt_hold_switch;
unsigned long throttle_output; //duration to use inside the altitude hold loop
unsigned long throttle_min;
unsigned long throttle_max;
unsigned long throttle_takeoff;
int old_distance ;
String serial_string;

void setup() {
  pinMode(led, OUTPUT);
  pinMode(pinReadCh3, INPUT);
  pinMode(pinReadCh6, INPUT);
  myThrottleChOut.attach(throttlePin1); //attach the "servo" to digital pin 7, this is the digital pin which is going to write to the KKboards throttle channel.
  //Serial.print="Initialized - ";

void loop()
  throttle_read = pulseIn(pinReadCh3, HIGH); //Reads the throttle level, returns 1150 - 1839 microseconds. 1 millisecond = 1000 microseconds -> throttle_read/1000: ex 1,736ms 
  alt_hold_switch = pulseIn(pinReadCh6, HIGH); //Reads the ch6 level 
  unsigned int uS = sonar.ping(); // Send ping, get ping time in microseconds (uS).
  int distance = (uS / US_ROUNDTRIP_CM); //Gets the altitude before entering altitude hold mode, this will be a reference 
  throttle_output = throttle_read;
  //serial_string="Th: "+throttle_read;
  //saving the previous distance is useful because it can tell us how much throttle has been used for take off 
  if (distance-old_distance>3 && old_distance 1700) //If the ch6 knob is turned almost to max, arduino enter altitude hold mode
  // we need to read the actual ch3 value
  // then create 2 value by adding and subtracting the threshold value of the constant ex actual 1150 the neutral position will be from 1550-threshold to 1150 + threshold
  // this way we can add an action for example landing when below min threshold
  unsigned long throttle_min=throttle_read-TX_THRESHOLD;
  unsigned long throttle_mmax=throttle_read-TX_THRESHOLD;     
        while (alt_hold_switch > 1700) // arduino enters altitude control loop.
            digitalWrite(led, HIGH);//fires up the led
            unsigned int uS2 = sonar.ping(); // Send ping, get ping time in microseconds (uS).
            int newDistance = ((uS2 / US_ROUNDTRIP_CM)); //Gets the current altitude, this will be compared to the reference altitude
            //switch to while
            if (newDistance > distance + 5) //+ 10 will give an intervall between desired altitude +- 10 cm in the upper and lower bounds.
                if(throttle_output > 1250) //this is to prevent the tricopter to throttling down to no power and fall from the sky
                  throttle_output -= 2;//autolanding feature to be added here
                  //serial_string+=" slowing. "+ pulseIn(throttle_output, HIGH);
            else if(newDistance < distance - 5 || newDistance < MINIMUM_ALTITUDE)
              if(throttle_output < 1680) //this is to prevent the tricopter from throttling up to max throttle
                throttle_output += 2;
                 //serial_string+=" speed-up. "+ throttle_output;
               //do nothing 
                //serial_string+=" do nothing. ";
               throttle_output = throttle_output;                
            alt_hold_switch = pulseIn(pinReadCh6, HIGH); //Reads the ch6 level, this is needed to be able to exit the while-iteration
            //if the pilot rise the throttle more than it is going to be written by arduino then exit from the alt hold mode and ignore the switch until is set back to off position
            serial_string=" ALT:Y ";
  digitalWrite(led, LOW);
  alt_hold_switch = pulseIn(pinReadCh6, HIGH); //Reads the ch6 level, this is needed to be able to re-enter the while-iteration 
  serial_string+="ALT:N ";
  //serial_string+="trhottle: "+  throttle_read;
   old_distance = distance;

if someone know tricks to make this safer for people and for the quadcopter itself, or to improve it under any way please leave a comment. 

(Visited 10,178 times, 1 visits today)

Author: Giuseppe Urso

Giuseppe lives in Haarlem now with his shiny dog, Filippa In 1982 received his first home computer, a Commodore 64, followed by Datasette and a 1541 Floppy Disk Drive. In 1999 he installed his first Linux distro (LRH6). In 2006 he switched to Debian as favourite OS. Giuseppe Urso actively sustains the Free Software Fundation and his founder Richard Mattew Stallman, he speaks to people trying to convince them to join the fight now, and about how important is to use Free Software only. He has a job as Infra Specialist at Hippo Enterprise Java Cms an Open Source Enterprise class Content Management System, one of the coolest company ever, in Amsterdam. He's always ready to install Debian on other people computers for free.

2 thoughts on “Low altitude hold, auto takeoff, autolanding with kk or Hk v2.1 + Arduino + Sonar”

  1. Hi, im really interested on your work, did you finished? did it work?
    do you have a complete code?

    Excellent work there!

Leave a Reply

Your email address will not be published. Required fields are marked *


This site uses Akismet to reduce spam. Learn how your comment data is processed.