Quadcopter: Stabilization along the z-axis (for holding altitude)

I recently spent some work on my quadcopter firmware. The model is stabilizing its attitude relatively well now. However I noticed, that it is changing its altitude sometimes (maybe pressure changes, wind or turbulence). Now I want to get rid of these altitude drops and found not much literature. My approach is using the accelerometer:

• Calculates the current g-force of the z-axis
• if the g-force is > 0.25 g and longer than 25 ms, then I feed the accelerometer term (cm per s²) into the pid
• the output is sent to the motors

The model now reacts when it is falling down with an up-regulation of the motors. However, I am not sure, whether it is smart to feed the current acceleration into the regulator and I currently wonder, whether there is a smarter method to deal with sudden and smaller changes in altitude.

Current code:

# define HLD_ALTITUDE_ZGBIAS 0.25f
# define HLD_ALTITUDE_ZTBIAS 25

const float fScaleF_g2cmss = 100.f * INERT_G_CONST;
int_fast16_t iAccZOutput = 0; // Accelerometer

// Calc current g-force
bool bOK_G;
float fAccel_g = Device::get_accel_z_g(m_pHalBoard, bOK_G); // Get the acceleration in g

// Small & fast stabilization using the accelerometer
static short iLAccSign = 0;
if(fabs(fAccel_g) >= HLD_ALTITUDE_ZGBIAS) {
if(iLAccSign == 0) {
iLAccSign = sign_f(fAccel_g);
}

// The g-force must act for a minimum time interval before the PID can be used
uint_fast32_t iAccZTime = m_pHalBoard->m_pHAL->scheduler->millis() - m_iAccZTimer;
if(iAccZTime < HLD_ALTITUDE_ZTBIAS) {
return;
}

// Check whether the direction of acceleration changed suddenly
// If so: reset the timer
short iCAccSign = sign_f(fAccel_g);
if(iCAccSign != iLAccSign) {
// Reset the switch if acceleration becomes normal again
m_iAccZTimer = m_pHalBoard->m_pHAL->scheduler->millis();
// Reset the PID integrator
m_pHalBoard->get_pid(PID_ACC_RATE).reset_I();
// Save last sign
iLAccSign = iCAccSign;
return;
}

// Feed the current acceleration into the PID regulator
float fAccZ_cmss = sign_f(fAccel_g) * (fabs(fAccel_g) - HLD_ALTITUDE_ZGBIAS) * fScaleF_g2cmss;
iAccZOutput = static_cast<int_fast16_t>(constrain_float(m_pHalBoard->get_pid(PID_ACC_RATE).get_pid(-fAccZ_cmss, 1), -250, 250) );
} else {
// Reset the switch if acceleration becomes normal again
m_iAccZTimer = m_pHalBoard->m_pHAL->scheduler->millis();
// Reset the PID integrator
m_pHalBoard->get_pid(PID_ACC_RATE).reset_I();
}

• How about using a sonar pointing down to measure the altitude? – dm76 Aug 15 '14 at 6:49
• Why do you use a threshold of 0.4g? Why not, as you say, always send the Z acceleration into the PID? – Rocketmagnet Aug 15 '14 at 10:43
• The sonar is working just for 6m and I wanted to use the accelerometer just for sudden and small changes. I thought I should use a cutoff, because the accelerometer is prone to sensor noise. Furthermore the PID should just work when there is a bigger change, not for every small movement. – dgrat Aug 15 '14 at 11:22
• Run the accelerometer data through a digital low-pass filter? – Simon Richter Aug 19 '14 at 8:33
• Before you can hold an altitude with any accuracy, you need to be able to measure your altitude with accuracy. Before you can react quickly, you need to measure quickly. Assuming that you can instantaneously determine your altitude (and alter your desired thrust equally so), how long will it take your motors to speed up and how long will it take for that to have an effect on the mass & velocity of the quadcopter? What is the difference between that optimal latency and the latency you see now? – Ian Aug 20 '14 at 21:38

Two approaches are possible:

1. Combine the altitude (GPS or pressure) and vertical acceleration sensors' data to calculate a better geometric altitude, and tune your vertical controller using this feedback in your loop.

2. Employ a stability augmentation loop for the z-acceleration (in body frame). In this case, if your vehicle swings, as shown on Jon's answer, your vehicle will sense a z-acceleration and will try to correct that. This may not be best practice to work on the z-acceleration in body frame, as it will couple the roll with altitude as the aircraft rolls and moves around. So a trigonometric conversion can be done to convert the a_z data (in body frame) into a_z_inertial (in Inertial frame, e.g. in gravity). It's best to work this on paper (you have both roll and pitch, affecting the result).

About the current algorithm:

1. Filter your accelerations. Try a running average (low pass filter) of your accelerations, to get rid of the noise. It will probably be OK to have a running average of the last 0.2 second, for example.

2. Do not use cut-off, at all. It makes life non-linear, and it's not good. Let the controller handle all the events, and let it react to the small errors, before they grow high.

Cannot comment yet.

I would add a gyro and use a complementary or Kalman filter. Accelerometers are right, on average, but wrong, right now. Gyros are right, right now, but are wrong, on average. The filter weights the two inputs based on how wrong they are and outputs a value somewhere between right and right now.

• I don't see how the gyro can help in this case, since it measures angular speed and not altitude variation. The accelerometer does not give a direct measurement of the altitude but can provide feedback on linear movements through integration, well barely... – Ugo Pattacini Feb 9 '15 at 11:27
• If you zero a gyro and an accelerometer on your copter, then turn it 45 degrees on an axis, the gyro will read 45 right now, then start to be wrong. The accelerometer will be wrong, but soon show "down" to be at 45. It is likely your accelerometer is responding to unfiltered vibration. The gyro and filter will know that there is no reason for the accelerometer reading and correct it. – Jon Feb 9 '15 at 11:32
• Likewise, when the gyro says the copter is upside-down, the accelerometer knows down is still down. – Jon Feb 9 '15 at 11:35
• Ok, you've mentioned the classical complementary filter adjustment for correcting acceleration readings, but I think here it's more the point to use the acceleration to get altitude estimation. I mean, "imagine we have good acceleration values then what we need here is..." – Ugo Pattacini Feb 9 '15 at 11:36
• When the copter tilts, accelerometer-z changes and the copter sees "falling" that isn't happening. He's already coerced it to work with the null-zone but wants it to stop getting confused. – Jon Feb 9 '15 at 11:39