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

Question

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();
}

Answer 1

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.

Answer 2

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.