 ### Stability Control Loop Improvement

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

}

### Activity

How to use the new @donkey_car graphical UI to edit driving data for better training https://www.youtube.com/watch?v=J5-zHNeNebQ
Monday
RT @SmallpixelCar: Wrote a program to find the light positions at @circuitlaunch. Here is the hypothesis of the light locations updating ba…
Nov 26
RT @SmallpixelCar: Broke my @HokuyoUsa Lidar today. Luckily the non-cone localization, based on @a1k0n LightSLAM idea, works. It will help…
Nov 25
@gclue_akira CC @NVIDIAEmbedded
Nov 23
RT @luxonis: OAK-D PoE Autonomous Vehicle (Courtesy of zonyl in our Discord: https://discord.gg/EPsZHkg9Nx) https://t.co/PNDewvJdrb
Nov 23
RT @f1tenth: It is getting dark and rainy on the F1TENTH racetrack in the @LGSVLSimulator. Testing out the new flood lights for the racetra…
Nov 23
RT @JoeSpeeds: Live Now! Alex of @IndyAChallenge winning @TU_Muenchen team talking about their racing strategy and open source @OpenRobotic…
Nov 20
RT @DAVGtech: Live NOW! Alexander Wischnewski of Indy Autonomous Challenge winning TUM team talking racing @diyrobocars @Heavy02011 @Ottawa…
Nov 20
Incredible training performance with Donkeycar https://www.youtube.com/watch?v=9yy7ASttw04
Nov 9
RT @JoeSpeeds: Sat Nov 6 Virtual DonkeyCar (and other cars, too) Race. So bring any car? @diyrobocars @IndyAChallenge https://t.co/nZQTff5…
Oct 31
RT @JoeSpeeds: @chr1sa awesomely scary to see in person as our \$1M robot almost clipped the walls as it spun at 140mph. But it was also awe…
Oct 29
RT @chr1sa: Hey, @a1k0n's amazing "localize by the ceiling lights" @diyrobocars made @hackaday! It's consistently been the fastest in our…
Oct 25