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

}

## Replies

This is working well now with a 5hz LPF on the velocity! It finally smooths it out enough I can use Rate D on a heli! That's big news.

roll_angular_rate = constrain_int32(desired_rate, roll_angular_rate - (g.accel_roll * 100 * G_Dt), roll_angular_rate + (g.accel_roll * 100 * G_Dt));

feedforward_rate = roll_rate_filter.apply(roll_angular_rate);

controlled_roll_target += feedforward_rate * G_Dt;

target_angle = controlled_roll_target;

Hey Rob, I have my earth frame acc limiting implementated now and it appears to be jitter free. Just remember that in my code, the "target_ef" is the location the copter will be when the bodyframe stabilization zeros the error. (i.e. correction occurs in the bodyframe).

I found that with the implementation we've been discussing, the jitter returns if you're moving the stick. Filtering the output works, but seemed like a bandaid. This keeps the 'desired rate' more continuous :

#define ACC_SOFTEN 500 // linear region = +/-5deg

static void get_stabilize_roll(int32_t target_angle)

{

float remaining;

int32_t max_rate;

// error from here to target, adjusted for rate change delay

remaining = fwrap_180cd((float)target_angle-target_ef.x) - roll_slew_rate_ef/100.0;

// determin max rate from here

max_rate = safe_sqrt(fabs(remaining)*(200.0*g.stb_acc_roll));

// clip to max rate

if(max_rate > 100L*g.stb_max_rate) max_rate = 100L*g.stb_max_rate;

// sign and reduce max rate near the target

max_rate = (max_rate * (int)constrain(remaining, -ACC_SOFTEN, ACC_SOFTEN)) / ACC_SOFTEN;

// acceleration limited rate change (assuming perfect 100hz 100*deg/s/s*0.01 -> cd/s/sample)

roll_slew_rate_ef = constrain(max_rate, roll_slew_rate_ef - g.stb_acc_roll, roll_slew_rate_ef + g.stb_acc_roll);

// set targets for rate controller

set_roll_rate_target(roll_slew_rate_ef, EARTH_FRAME);

}

Here's what the feed forward filter does... i'm using 90% old + 10% new to exaggerate the effect a little:

The fix is in! Thanks, Bob.

https://github.com/R-Lefebvre/ardupilot/commit/da51fe04597a49aa2908...

I just took a look at the code... it looks like it works, but may not be doing exactly what you intended. I was trying to imagine how the angle error driving it would behave, and decided to do an excel sim - the assumption here is that the roll sensor moves exactly in step the the target rate that was requested in the previous iteration.

Being angle error driven with no feed forward means it has to fall behind to go anywhere - making the purple/green lines you hope for result in the pink/red lines you get. Also, the exponential growth of the projected stop may need to be considered. Here it only gets up to 160 deg/s and the projected stop is growing at 3 degrees / 100hz sample.

So, I'm trying to get rid of that jitter. I've got this as a first stab:

if (projected_stop < (target_angle-100)){

desired_rate = g.angle_rate_max;

} else if (projected_stop > (target_angle+100)){

desired_rate = -g.angle_rate_max;

} else {

desired_rate = (target_angle-projected_stop);

}

Seems to be working, by linearizing the last degree of movement. But I know that there's a discontinuity in the acceleration when you go past 1 degree of error. I just need to try and smooth that out.

desired_rate = (target_angle-projected_stop); ?

Should that maybe be:

desired_rate = roll_angular_rate; // hold current rate

I'm not sure "rate = error degrees" is what you intended... roll_angular_rate is likely much larger or smaller than that and will result in the same acceleration step as if one of the rate_max cases passed.

If the code snippet in my previous reply, should the feed forward be here?

set_roll_rate_target(roll_angular_rate + target_rate, EARTH_FRAME);

The idea was, when the error is smaller than 100 centidegrees, the desired rate changes to a linear relationship. So at 1 degree, the desired rate is only 1 degree/second. At 1/2 degree error, it's only 1/2 degree per second.

I want to make the rate slightly U-shaped, rather than a sharp V. If you have any better ideas, I'm all ears, but this is actually working pretty decent right now.

Though I think it's the LPF that really is helping smooth it out, and I might even be able to get rid of that cludge, we'll see. I'd still prefer a U-shape in any case.

Here's a a thought - scale max_rate down based on distance remaining to target, and make the desired_rate calculation continuous:

max_rate = constrain(sqrt(abs(D)*2*A), -g.angle_rate_max, g.angle_rate_max)

where D = target angle - controlled roll target and A = g.accel_roll * 100

desired_rate = constrain( E * max_rate/LINEAR, -max_rate, max_rate )

where E = target_angle-projected_stop and LINEAR is the number of cdeg you want to smooth out.

unfortunately there's a root in the calc, but it looks like it'd work pretty good. Here's 500deg/s with a +/-5deg linear region:

This is all on Pixhawk, so the root should hopefully not be a problem... Famous last words. ;)

I'll have a good look at this tomorrow. Does it still need the acceleration rate constrain? Or that's handled in the motion curve? And would it be hard to make that equation so the "linear" smoothed portion is at both ends? The LPF I put in really smoothed out the lead-in. If I can build it into the motion equation, then I can probably just get rid of the filter.

Really, what you're building here is a Jerk limiter, which is friggen awesome, I didn't know we'd get that far.

Today went VERY well. I made a few other improvements to the rate controller for helis. And combined with your 3/2 bug fix and then the smoothened output for the target rate, this thing is feeling completely awesome.

Something else to noodle on if you want... I want to change the Stab correction rate from a V-shape:

Next