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: 3659

Reply to This

Replies to This Discussion

Yes, that is correct.  Here is the link to an EKF.


Yes, it was built from the ground up (roughly 25k lines of code and several months of work), tested and sensors characterized to deal with rather large errors in MEMs "automotive-grade" sensors which are used on the APM, MultiWii, AutoQuad, etc.  My professional work uses tactical and navigation grade sensors ($50-100k USD), but these are far to expensive for hobbyist projects ($100-$500 USD).  My "hobby" code is based on the work done by the MultiWii and the UAV Dev Board community.

As for the antenna tracker, yes I have had some luck with the code.  I built a MatLab simulation, using my mission planner and 6DOF simulator (both hand coded in MatLab, about 120k lines of code, I built this during my PhD work) to generate a trajectory, and then calculate the antenna servo positions from various locations.  The trajectory was a simple NASCAR style race track with 5 Km legs, altitude of 200 m (MSL).  I placed the antenna in three locations on the "ground" (0 m MSL):

- off to one side of the track: to test the tracking left to right and up and down.

- in the center: to test the pan limits +/-180deg (I am not using continuous rotation servos)

- directly under one leg: to test the tilt limits 0-90 deg.

All the tests worked very well.  The code modification to the MultiWii Serial.ino was a simple addition to the MSP and I added a few serial ID's.  I will need to post this code, but it is not quite finished.  If you wan to have a peek I can post it later tonight.

Excellent! Thanks for that Paul.  I'll have a look through the code and see if I can get my head around what you included/changed to support the Maxbotix sonar.

Exceedingly difficult to tap into the analog ports on the Paris V4r5 board as the 328 chip does not have any break outs for anything other than the A0, A1 pins, which are reserved for Cam Servo out.  I'll have to look at the schematic of the 328 SMA chip to see which pins are analog.

At the moment I am not using A0,A1 for the gimbal (separate MW controller is handling that).

Might see if I can disable any references to A1 and tap into that.

What type of multi are you flying?

Try to stick with the digital ports if you can, i.e. flying a quad or hexa on a 328p processor.

Oh, and not sure what you use for code compare, but I like WinMerge.  Works great for a freeware.

The Paris V4 MW board runs off a 328p SMA chip rather than having a ProMini connected via it's pins.  #define A0_A1_PIN_HEX is in use to allow A0/A1 for the 5th and 6th motor outputs (not cam as I thought earlier).  I have to use this define as D5/D6 are tied up with the Rx connector and I'm flying a hexacopter.

I'm using UltraCompare from IDM to spot the changes.  

Your code is using D11 so must be pulse width rather than analog from the sonar.  In which case D12 has a break out pin on the Paris MW board so I can tap into that.

Will give it a shot tomorrow..  Any PID setting tips for ALT Hold?

Can't recall off the top of my head but is the SONAR option visible in the Config app for the AUX channel switch positions?

Sorry, SMD is what I meant to type.

Just trying your code now and I'm getting an odd 1Hz delay (at least it looks like a delay at the Config app end).  The values only change every 1Hz.  Code also fails to arm.  Just commenting out the #define MaxSonarLV to see if it is related to that code.

Sonar values seem to be working but it concerns me that the original Baro value was in metres and now it is in CM.

Also, do I need the AUX channel to enable Baro and Sonar at the same time?

Yup, something in the MaxSonarLV code is bogging down the loop.  With the define removed all is fine (apart from not seeing the sonar CM in ALT).

Just going to look through the calls line by line in the loop to see what could cause the issue.

I have #define DISABLE_POWER_PIN to stop the code from trying to make use of D12.  Wondering if some other portion of the code tries to use D12?

Ah, I was not connected up to Pin 2 on the XL MaxSonar EZ0 MB1200 sonar.  Pulse width was timing out giving me the delay.  Connected up to pin 2 the config app seems to be running more smoothly, but there still seems to be a delay in there that is making the attitude feedback sluggish.

Using pulseIn is really asking for trouble.  The delay in my case while it waits for the pulse is enough to make the hex dangerous as the RC input has a bad delay.  It's just not safe to fly with that delay.  Possibly the version of your sonar has a very small pulse width.  The XL sonar I have uses 58uS / cm on pin 2.  I might try pulseIn set to LOW.  Failing that I may need to go back to the original plan of tapping into an Analog pin.  Looks like A2 is free as long as CAM_TRIG is disabled.

Well that's interesting - the debug[]= calls (the three you had defined to get feedback of the sonar vars) seem to screw with being able to write settings via the config app.  If I comment the debug assignment lines the issue goes away.  Could be too much overhead for all the data being communicated.  I was having an impossible time trying to get the ROLL P setting to stick.  It would keep reverting back to 0.0.  Couldn't even get the AUX settings to stick. 

All good now.

I was going to mention that I ran several tests to read the data quickly and accurately: 

1. My frist test was to read the Sonar via an analogue port, and the result requires at least 4 readings to get a good value.  This is too much delay.

2. My second attempt was to read with a digital PWM read.  This was the version that I stuck with, though there is a better way.

3. The final method, I tried to read with a second processor and create a I2C protocol to read to the primary processor, in the same way that one of the guys did with the GPS to I2C for the MultiWii.  This works best, but creates the need for a second processor.

I also ran several data acquisition rates.  After several tests, there is no appreciable difference at readings above 50Hz.  (This verified what I already assumed about an ultra sonic sensor) I placed the sonar read in the 50Hz loop just for simplicity.  Really it should be in its own dedicated loop and read at 10Hz.  Because of the ranging delay with the sonar, you will not see the advantage above 20Hz.  I work with Radar Altimeters professionally, and generally speaking you cannot run them faster than 40-50 Hz.

As for PID, that will depend on your frame, e.g. mass of the frame, inertia of the propellers, kv of the motors, etc.  I reached a stable solution with the following:

P=2.5, I=0.1, D=15

Last night my quad would hold within 10cm in my garage, and 15-20 on the grass outside.

I did not include the algorythms to account for pitch & roll, which worked well on MultiWii v1.9, but somehow do not on the current v2.1.  I will look at this some more and get back to you.  Basically it accounted for pitch and roll angles which are larger than the beam width, and in concert with vertical velocity, would aid in what I called "Terrain Altitude Hold"

Honestly, the architecture needs work. But it was something that I just wanted to work on quickly and I merged my updates in the v1.9 to the v2.1 code.  Honestly the MultiWii v1.9 is my preferred version, and it acts much differently than the v2.1 for a variety of reasons.  I am a little worried about the direction of the MultiWii code trying to be a one size fits all.  This is why for my personal "hobby" code, I also prefer the UAV Dev Board, as it uses one set of sensors and the focus is instead placed on the algorithms  and to a lesser extent a variety of aerial platforms.

I also found a significant difference in the read lag when using the XL sonar.  I have both a LV and an XL sonar, and after a few tests with the XL I switched back to the LV sonar.  If you find a fix which works on the XL sonar, I am eager to learn of it so that I can use that sensor.

Interesting comment about the debug[] statements.  I find it hard to believe that the 328p and the 32u processors would be that much different, but maybe we also have the difference of a quad versus a hexa.  You are certainly writing more data and computing more on the hexa.  But not by much.  interesting...

Final comment, yes the GUI has the box option for sonar.  I added this fix to the MultiWii.ino and Serial.ino files.  You should see this after you re-flash your board, and connect > start > read in the GUI. 

Ok, finally had a go at the sonar alt hold using your code.  As soon as I enable it the hexacopter will descend rapidly, catch itself and ascend rapidly and then just bounce up and down by greater amounts each time.

Any suggestions for tweeking the ALT PID values?  Just reduce the P term?

Noticed you commented out the height smoothing routine - Sonar_getADC.  Might need to reintroduce it as I am now using the analog pin 3 on the sonar connected to A2 on the MultiWii and calling analogRead(2).  Interestingly, the int returned is almost exactly in CM already - no need to divide the value.

Tried this code to smooth out the sonar reading, and with SonarAlt being written to debug[0] the cm alt looks fine when I move the hexacopter around. However with the ALT P term set down as low as 0.7 it still bounces violently. Running out of options. I'm using the same sonar cable and connectors I used on the APM 1.0 controller and that was working well for sonar hold.

void Sonar_getSmoothAlt() {
// subtract the last reading:
sonarTotal = sonarTotal - sonarReadings[sonarIndex];
// read from the sensor:
sonarReadings[sonarIndex] = analogRead(2);
// add the reading to the total:
sonarTotal = sonarTotal + sonarReadings[sonarIndex];
// advance to the next position in the array:
sonarIndex = sonarIndex + 1;
// if we're at the end of the array...
if (sonarIndex >= sonarNumReadings)
// ...wrap around to the beginning:
sonarIndex = 0;
// calculate the average:
sonarAverage = sonarTotal / sonarNumReadings;
SonarAlt = (int32_t)sonarAverage;
SonarAlt16 = sonarAverage;

Reply to Discussion


© 2020   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service