Discussions

Hey gang,

I just implemented an idea I had a while back to improve the Arducopter stability control. It's basically using the stabilized acro idea and comes from noticing that my acro leveling code was faster at returning the copter to level than the actual stability control and I couldn't up the stability gains to improve it. The jist of it is that instead of running the STB control loop against the error difference between the stick angle and the copter angle, I'm creating an acceleration limited body frame trajectory plot to follow. This to allows much faster stick tracking without error windup. Here's the code:

First, to follow a trajectory, we need to know how far from the trajectory we are. This function is called right before update_yaw_mode()/update_roll_pitch_mode(). error_bf is the current body frame trajectory error, and target_ef is the new projected target position in the earth frame. (Both stored as vectors)

void update_axis_movement()
{
  float dt = ins.get_delta_time();

  error_bf = error_bf - ((omega * DEGX100) * dt);

  // calculate target rotation so stabilized modes see where we 'will' be
  Matrix3f temp = ahrs.get_dcm_matrix();
  temp.rotate(error_bf * RADX100);
  temp.to_euler(&target_ef.x, &target_ef.y, &target_ef.z);

  target_ef *= DEGX100;
  if(target_ef.z < 0) target_ef.z += 36000;
}

The earth frame rate calculations are a very simple P controller. Note that they are controlling against where the copter will be not where it is - this is important to avoid asking it to go beyond the desired position. ACRO_P is used to set the max speed and I'm currently using 10, which means a 450deg/s max speed. I'll get to the acceleration limiting below.

static void
get_stabilize_roll(int32_t target_angle)
{
  // convert constrained angle error to desired Rate:
  int32_t target_rate = constrain(wrap_180(target_angle - target_ef.x), -4500, 4500) * g.acro_p;

  // set targets for rate controller
  set_roll_rate_target(target_rate, EARTH_FRAME);
}

static void
get_stabilize_pitch(int32_t target_angle)
{
  // convert constrained angle error to desired Rate:
  int32_t target_rate = constrain(wrap_180(target_angle - target_ef.y), -4500, 4500) * g.acro_p;

  // set targets for rate controller
  set_pitch_rate_target(target_rate, EARTH_FRAME);
}

static void
get_stabilize_yaw(int32_t target_angle)
{
  // convert constrained angle error to desired Rate:
  int32_t target_rate = constrain(wrap_180(target_angle - target_ef.z), -4500, 4500) * g.acro_p;

  // set targets for rate controller
  set_yaw_rate_target(target_rate, EARTH_FRAME);
}

// Yaw with rate input and stabilized in the earth frame
static void
get_yaw_rate_stabilized_ef()
{

  set_yaw_rate_target(g.rc_4.control_in * g.acro_p, EARTH_FRAME);
}

Now the actual STB PID stabilization (I'm using PID instead of PI) is happening in the body frame reference. I moved it all into update_rate_contoller_targets(). It should look familiar, as it is the basic stabilized acro code (feed forward + correction) with acceleration limiting added. The idea here is to not ask for something you know you can't have. I'm currently using 5000deg/s/s. 

void
update_rate_contoller_targets()
{
  if( rate_targets_frame == EARTH_FRAME ) {
    // convert earth frame rates to body frame rates
    roll_rate_target_bf = roll_rate_target_ef - sin_pitch * yaw_rate_target_ef;
    pitch_rate_target_bf = cos_roll_x * pitch_rate_target_ef + sin_roll * cos_pitch_x * yaw_rate_target_ef;
    yaw_rate_target_bf = cos_pitch_x * cos_roll_x * yaw_rate_target_ef - sin_roll * pitch_rate_target_ef;
  }

  // now do body frame trajectory control:

  // limit acceleration to avoid unattainable error accumulation

  prev_roll_rate_bf = constrain(roll_rate_target_bf, prev_roll_rate_bf-g.acro_acclim_roll, prev_roll_rate_bf+g.acro_acclim_roll);

  prev_pitch_rate_bf = constrain(pitch_rate_target_bf, prev_pitch_rate_bf-g.acro_acclim_pitch, prev_pitch_rate_bf+g.acro_acclim_pitch);

  prev_yaw_rate_bf = constrain(yaw_rate_target_bf, prev_yaw_rate_bf-g.acro_acclim_yaw, prev_yaw_rate_bf+g.acro_acclim_yaw);

  // adjust rate target with correction

  roll_rate_target_bf = prev_roll_rate_bf + g.pid_stabilize_roll.get_p(error_bf.x) + g.pid_stabilize_roll.get_i(error_bf.x, G_Dt) + g.pid_stabilize_roll.get_d(error_bf.x, G_Dt);

  pitch_rate_target_bf = prev_pitch_rate_bf + g.pid_stabilize_pitch.get_p(error_bf.y) + g.pid_stabilize_pitch.get_i(error_bf.y, G_Dt) + g.pid_stabilize_pitch.get_d(error_bf.y, G_Dt);

  yaw_rate_target_bf = prev_yaw_rate_bf + g.pid_stabilize_yaw.get_p(error_bf.z) + g.pid_stabilize_yaw.get_i(error_bf.z, G_Dt) + g.pid_stabilize_yaw.get_d(error_bf.z, G_Dt);

  if (motors.armed() == false || ((g.rc_3.control_in == 0) && !ap.failsafe)) {
    error_bf.x = 0; error_bf.y = 0; error_bf.z = 0;
    prev_roll_rate_bf = 0; prev_pitch_rate_bf = 0; prev_yaw_rate_bf = 0;
    roll_rate_target_bf = 0; pitch_rate_target_bf = 0; yaw_rate_target_bf = 0;
  }

  // Add requested movement to error for next iteration:

  error_bf.x = constrain(error_bf.x + (prev_roll_rate_bf * G_Dt), -MAX_BF_ROLL_OVERSHOOT, MAX_BF_ROLL_OVERSHOOT);

  error_bf.y = constrain(error_bf.y + (prev_pitch_rate_bf * G_Dt), -MAX_BF_PITCH_OVERSHOOT, MAX_BF_PITCH_OVERSHOOT);

  error_bf.z = constrain(error_bf.z + (prev_yaw_rate_bf * G_Dt), -MAX_BF_YAW_OVERSHOOT, MAX_BF_YAW_OVERSHOOT);

}

With the original control loop, you really couldn't use I or D terms because the integrator would basically windup the whole time the copter was moving towards the requested angle, and of course would drive it past it before starting to wind down. The D term just wouldn't work properly since it's just one big saturated error. With this coding, the I and D terms can be used to keep the motion on target and avoid overshooting. Any comments?

I'm not flying the official acro code, but as an example, here's how my acro code simplified with this change:

static void
get_roll_rate_stabilized_bf()
{
  // Convert the input to the desired roll rate
  int32_t roll_rate = g.rc_1.control_in * g.acro_p;

  // Calculate rate limiter to avoid discontinuity when crossing 180
  float lf = CalcLevelMix() * (g.acro_balance_roll/100.0);
  int32_t limit = CalcLimit(roll_rate, lf * g.acro_p * 4500);

  // Calculate level rate for blending (similar to MultiWiii horizon mode), ignore roll level as we approach pitch +-90 (cos_pitch_x)
  int32_t level_rate = g.acro_p * constrain(-target_ef.x, -4500, 4500) * lf * cos_pitch_x;

  // combine/limit rate
  roll_rate = constrain((roll_rate + level_rate), -limit, limit);

  // Set body frame target for rate controller
  set_roll_rate_target(roll_rate, BODY_FRAME);

}