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

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;

  // 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
    // Save last sign
    iLAccSign = iCAccSign;

  // 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
  • 1
    $\begingroup$ How about using a sonar pointing down to measure the altitude? $\endgroup$
    – dm76
    Aug 15, 2014 at 6:49
  • 2
    $\begingroup$ Why do you use a threshold of 0.4g? Why not, as you say, always send the Z acceleration into the PID? $\endgroup$ Aug 15, 2014 at 10:43
  • $\begingroup$ 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. $\endgroup$
    – dgrat
    Aug 15, 2014 at 11:22
  • $\begingroup$ Run the accelerometer data through a digital low-pass filter? $\endgroup$ Aug 19, 2014 at 8:33
  • 4
    $\begingroup$ 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? $\endgroup$
    – Ian
    Aug 20, 2014 at 21:38

2 Answers 2


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.

enter image description here

  • $\begingroup$ 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... $\endgroup$ Feb 9, 2015 at 11:27
  • $\begingroup$ 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. $\endgroup$
    – Jon
    Feb 9, 2015 at 11:32
  • $\begingroup$ Likewise, when the gyro says the copter is upside-down, the accelerometer knows down is still down. $\endgroup$
    – Jon
    Feb 9, 2015 at 11:35
  • $\begingroup$ 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..." $\endgroup$ Feb 9, 2015 at 11:36
  • $\begingroup$ 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. $\endgroup$
    – Jon
    Feb 9, 2015 at 11:39

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.