I've been trying to get my head around the best method of controlling the throttle channel on my KK tricopter to get it to stick to a set altitude (anywhere between 0.5 to 8m) using just my XL sonar I pulled from my APM based quad and an Arduino Uno board.  

To start with, I coded the sketch to use channel 5 to turn the alt hold code on or off.  When off, the source throttle PWM value is just passed to the throttle out as is.  When on, the current altitude and input throttle PWM are captured and used as a reference.  When the height drops below the captured height the heigh difference is added to the captured PWM (hover PWM) to push the tricopter back up.  If the measured hight is higher than the captured reference height the height difference is subtracted from the hover PWM value to allow the tricopter to come back down.

This is obviously a very simplistic approach to trying to control the throttle to make it stick to a set height.  In practice this just results in an ever increasing bounce up and down as soon as I flick the switch on channel 5.

I did try a measured pulse up to start it coming back up and then a measured throttle drop as soon as the hight was obtained but as soon as wind was brought into the equation this just resulted in the tricopter drifting away from the desired height constantly.  

The frustration of all this is getting the better of me so now I am looking into what PID calculations are and how they could apply to this requirement.

I have also ordered a 3 axis accelerometer to see if detecting the fall/rise motion as well as the height will increase the accuracy.

I know this is a large request, but has anyone got some code lying around which specifically uses PID algorithms to calculate the required output throttle given the following input values -

1) hover PWM (the PWM captured when the mode is changed to alt hold)

2) desired height (cm captured when initially set to alt hold)

3) current height (cm)

4) optional z acceleration

I did have a look at the code for arducopter but there seemed to be a number of calls that were not applicable to the Uno, just the Mega.  

Any simplified examples of PID code for height control would be handy right now.


Views: 3663

Reply to This

Replies to This Discussion

Hi there!

I'm actually trying to do EXACTLY the same thing as you are..... ( except mines not for a Uno ) - just a mega.

I've only JUST starting to program with Arduino, but can I possibly take a look at what you have so far?,- ( possibly ) between the 2 of us, we could find some sort of answer to this project.

 Yes, I DID say I was "NEW" to this,.... but I have been programming for many years.. -your thoughts?


Im in the same boat. How's it going?

I've somewhat moved on from the KK board and tinkering with an external Arduino/sonar to get alt hold.

My thing now is the MultiWii platform.  I'm now stuck trying to work out what is involved in getting my I2C sonar working with it. The MW 2.1 code support the sonar I have however the data is only fed to Debug and not actually used for height control.  Still waiting for someone to post how they have been able to get the code to work.

i'm at the point that my arduino can actually take control over the throttle channel, but im' going to rewrite part of it in order to take into account the speed with wich it falls or climbs. Because at this point it does the job but it is very late and slow doing the throttle wizardry.

Im' stuck with Speed=(space/time).

So i'm introducing a new var that could represent tha last measure before a change, something like



    if(measure_now != measure_before)




//now we need to know how much time is passed from the previous measurement

//is it doable controlling the Delay() value? so the speed is diff/delay? cm/milliseconds or microseconds  ?

//should i add a pot (or a var)  in order to controll the "strenght" with wich the arduino will act?






I'm trying to understand how is best to manage the speed. I really don't know if it would be better to invest this time in improving the hk v2.1 or, better, try to make my 10dof+ardu pro mini flying. I think this is a good place where play with math/phisics/gravity laws, but on the other side, It's just i have to solder the headers plug for the arduino and the imu, pullup resistors, leds etc and last time i messed up using to much cable i think because the rear of the board looks like it has hairs:) (yes i used a normal prototyping board and thin wires), but i'm not quiet experienced with this stuff, so if anyone cal halp me understanding if it is better using a board design software etc or if it simple enough to do it "by hand"... I've seen there is a lot of pre-designed schematics around but i like to have controll over things so i can improve them in future, so i would prefer to do it by myself...

any help in both (kk-ping or board design) is appreciated.

About the hk/kk board + sonar, i will post a small footage of the retarded sonar-related behavior.


I copied the Baro code in the MultiWii codebase, renamed some of the variables to SonarAlt, etc. and it works just fine.  I made changes to IMU.ino, Sensors.ino, MultiWii.ino, and Config.h.  I also added a checkbox to enable it via commanded and un-commanded baro.  My quad, hexa and octa will all hold within 5 cm of the set point.  I am also using a MaxBotic sonar from SparkFun, works great.

Hi Paul,

Were the code changes to the existing MW flight controller or an external Arduino with the MaxBotic sonar attached?  How did you get the MaxBotic sonar working with the MW code?  Reading via a spare analog port?

I did try the SRF02 but could not see how to get the SonarAlt variable to merge with the Baro Alt and transition between the two when the alt was too high for the sonar.

Read somewhere that the SRF02 can feed noisy data back the the code.  Wouldn't mind trying to get the MaxBotic sonar working somehow in the code.  The Paris board does not have any tracks to the analog ports on the 328 chip.

Hi Paul, can you clarify please: are you talking about using multiwii code with a custom sonar?

My weekend project instead, is to write my very own code in order to attach ANY arduino model to ANY flight controller with ANY sonar device.  To give this capability to all flight controller without such feature. Operation total cost: about 20€ and several Headache. At the end this can turn also in auto-takingoff and auto-landing by configuring a var for store a minimum altitude value to take off and hold the desired cruise altitude.

At this moment I'm stuck because i managed to fry my Transmitter and I'm waiting for the replacement, but i'm at the point that all works (auto take off too!), just too slow or too fast, then i thought that probably i have to implement an algorithm to determine the acceleration with wich the multirotor falls or climbs, so it can be applied as coefficent for the remodulation of the throttle signal made by arduino. Another thing i've noticed is that when you work in such this situations (i mean with no serial or lcd) sometimes it's very hard to understand what's going on, so the very first thing to do before beginning is to get a bluetooth module for arduino in order to get data on your laptop display while testing. This will turn into a talemetry system once finished (just add an osd because arduino already has tv-out functionality or a good device for video overlaying can't recall the name...).

Anyway, the lenght of this reply is due to my fried transmitter and my boring saturday :)

Happy weekends

@ Giuseppe & John

I will need to dig though my MultiWii code base, as i am not currently flying the Sonar.  I made the update to MultiWii v1.9, updated IMU.ino, Sensors.ino, MultiWii.ino, Config.h and Def.h.  I replaced the port I/O for the PowerMeter and made readings of a MaxBotic sensor at 50Hz.  This worked very well for me and took very little time (one evening) to get it flying with a good degree of accuracy (+/- 2 inches altitude hold) and no sensor fusion, e.g. fuse terrain altitude with vertical velocity.

I will dig through my code today, and make modifications for MultiWii v2.1 (as that is the current released code), and I will post it as soon as I have flown it (possibly tonight).

Lately I have been working on my Antenna Tracker, and a new flight control system running a GPS-INS with a rather large Extended Kalman Filter.

On the subject of the tracker, did you manage to find distance and bearing functions which work with the source int32_t (long) LAT/LON GPS values from the MultiWii protocol.  I'm finding the conversion to float is introducing noticeable rounding errors which you can get away with when flying >30m away, but any closer and you really need a diversity receiver as the float values returned are unreliable.  I'm currently using the calc_bearing/calc_distance function from ArduStation which work with float vars.  If these could work with integer maths then I could eliminate the rounding errors.  I then just have the GPS drift to worry about, which I guess is the reason you are trying the Kalman Filter.

Looks like you are referring to - http://en.wikipedia.org/wiki/GPS/INS

Did you write code from scratch for that?  Sounds extremely complex.

I updated the MultiWii 2.1 code for the Maxbotic sonar and finally flew it today.  I cannot upload it to the MultiWii Google Code base so I created a new project and you can grab it there.



Look at the read me file for specs on the quad frame that I was flying.  Basically the same as Warthox.

Reply to Discussion


© 2020   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service