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

Reply to This

Replies to This Discussion

OK, found some dev code which actually does a fairly good job of sonar hold by mixing the baro alt with sonar alt.  I just added in my smoothing routine above and used that to update sonarAlt.  Needs some tweaking on the mixing ratio's and the max sonar alt setting, which in my case is around 5-8m, not 1.5m.

Here is the link - http://www.multiwii.com/forum/download/file.php?id=1616

Just tested it for 15min.  Very stable at anything below 1.5m.  Above that baro alt kicks in and you get the usual 50cm variations.

Flew it over objects and it increased it's alt smoothly.

Hey guys, this post turned into a gold mine for my weekend project!

I can't wait saturday to read the entire discussion since last week!!

I tried this last night and it was an improvement.  It includes the vertical velocity calculated by baro.

// Terrain Height calculations: Sonar PID
if(SONAR) {
if (terrainMode) {
if (abs(rcCommand[THROTTLE]-initialThrottleHold)>20) {
terrainMode = 0;
errorAltitudeI = 0;
// *** Alt. Set Point stabilization PID ****
//error = constrain( AltHold - EstAlt, -1000, 1000); // +/-1m, 1 meter accuracy
error = AltHold - SonarAlt; // +/-1m, 1 meter accuracy
errorAltitudeI += error;
errorAltitudeI = constrain(errorAltitudeI,-30000,30000);

PTerm = P8[PIDALT]*error/100; // 16 bits is ok here
ITerm = (int32_t)I8[PIDALT]*errorAltitudeI/40000;

AltPID = PTerm + ITerm ;
// need to update this section
// // Gravity vector correction and projection to the local Z
// InstAcc = (accADC[YAW] * (1 - acc_1G * InvSqrt(isq(accADC[ROLL]) + isq(accADC[PITCH]) + isq(accADC[YAW])))) * AccScale + (Ki) * AltErrorI;
// #if defined(TRUSTED_ACCZ)
// //InstAcc = (accADC[YAW] * (1 - acc_1G * InvSqrt(isq(accADC[ROLL]) + isq(accADC[PITCH]) + isq(accADC[YAW])))) * AccScale + AltErrorI / 1000;
// InstAcc = (accSmooth[YAW] * (1 - acc_1G * InvSqrt(isq(accSmooth[ROLL]) + isq(accSmooth[PITCH]) + isq(accSmooth[YAW])))) * AccScale + AltErrorI / 1000;
// #else
// InstAcc = AltErrorI / 1000;
// #endif
// // Integrators
// Delta = InstAcc * dt + (Kp1 * dt) * AltError;
// EstAlt += (EstVelocity/5 + Delta) * (dt / 2) + (Kp2 * dt) * AltError;
// EstVelocity += Delta*10;

// *** Velocity stabilization PD ****
//error = constrain(EstVelocity*2, -30000, 30000);
error = EstVelocity*2;
delta = error - lastVelError;
lastVelError = error;

PTerm = (int32_t)error * P8[PIDVEL]/800;
DTerm = (int32_t)delta * D8[PIDVEL]/16;

//rcCommand[THROTTLE] = initialThrottleHold + constrain(AltPID - (PTerm - DTerm),-100,+100);
rcCommand[THROTTLE] = initialThrottleHold + (AltPID - (PTerm - DTerm));


Something new from Maxbotix - http://www.maxbotix.com/Ultrasonic_Sensors/I2C_Sensors.htm
I2C versions of their reliable sonars. Just ordered their MB1232 sonar. No concerns with processing time getting eaten into with these sonars. According to the specs for the MB1232 it is better suited to detecting large objects (cannot get bigger than the ground) and has better noise rejection than then wide beam versions.
Results from testing the SRF02 I2C sonar have been disappointing from the moment I received it last year. Cannot understand why any effort was made to support it in the MWC. Hopefully the readings from the new Maxbotix sonar will be on par with the MB1200.

Reply to Discussion


© 2019   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service