Hi all,
My motivation to get involved in the development team was to bring a good ACRO mode to ArduCopter. Three or four months ago Robert Lefebvre suggested that we could integrate the rate error and use it to correct the platform in the body frame. He went on to demonstrate this on Heli's. We have since learned that MultiWii use a similar approach and code was developed by Bob Dorion based on MultiWii and discussed here http://www.diydrones.com/forum/topics/flipping-arducopter.
I have taken Robert's approach and integrated it with the current ACRO trainer aids with the help of Robert and Bob. Here is the result:
This code is based on ArduCopter 3.01rc1 so if you haven't got this flying yet, don't bother with this code.
Changes:
Replace Earth Frame ACRO with Body Frame ACRO
Reduce the dead band on the roll and pitch inputs
In the same way as you could before you can disable all trainer functions by setting
ACRO_BAL_PITCH,0
ACRO_BAL_ROLL,0
ACRO_TRAINER,0
For my flight testing I have been using:
ACRO_BAL_PITCH,50
ACRO_BAL_ROLL,50
ACRO_P,4.5
ACRO_TRAINER,0
Things to discuss are:
Is the performance acceptable?
What additional features are needed? (switched trainer functions, zero throttle doesn't stop motors)
Is there a better algorithm?
Do you see any problems with the code?
How does the performance and feel compare with other systems? (please only comment if you have flown those systems and this system on a well set up copter, anything less is a waist of every bodies time)
I have spent at least 20 minutes doing nothing but flips and rolls of various combinations and mixtures and a similar amount of time doing yank and bank maneuvers. I have not found a problem or any bad habits.
I look forward to your feedback and ideas.
Comments
Just tested out the above code - huge improvement for stiff leveling. Now I'm playing with adding a DTerm to the acro correction:
set_roll_rate_target(g.pi_stabilize_roll.get_p(roll_axis) + acro_roll_Dterm.get_d(roll_axis, G_Dt) + acro_rate, BODY_FRAME);
(acro_roll_Dterm is just the regular PID Dterm used by the rate controllers, I'm using 6.5 for P and 0.47 for D)
My thinking is that this will give a boost when it's falling behind and back off it's catching up. If this works well, I think it might be worth restructuring the stabilize mode to work in a similar fashion. That is, maintain an axis target that plots out the path to the desired angle. The current structure couldn't really use a Dterm since it only knows the end point. With a plotted path and a Dterm, stabilize mode could use more P gain and follow rapid stick movements better.
Any thoughts?
Hey Leonard - I found my leveling oscillation source. I was calculating the level rate based on the ahrs sensor value, and not where the copter was supposed to be once it catches up to the target.
As an example, say the sensor said the copter was at 10deg but the target integrator was already wound up to -10deg (which would be relative to the current position of 10deg). My original code would try and subtract more off the target and overshoot, when instead it should just let the copter catch up to the absolute target of 10deg + (-10deg) = 0deg. Here it is in code:
int32_t level_rate = g.pi_stabilize_roll.get_p(constrain(-(ahrs.roll_sensor+roll_axis), -4500, 4500)) * lf;
"roll_axis" is my relative target integrator, which always represents degrees, and that lf factor on the end is just the blending/balance. I'm not sure if this applies to your code, just thought I'd share and let you think through your implementation.
No idea - Mine is at 0.19 and doing pretty well :)
But why doesn´t ACRO need almost any I term at all (and is unsensitive to value changes) whilst standard 3.0.1 loses that much of stability with same settings?
Leonard you were right, switching back to acro from stab doesn't revert to last attitude. All good :)
I love flying with this mode!! Seriously well done team :)
Bob, I still need to look more at your code. Sorry for the delay.
Thanks mate,
Yeh, Low I terms will mess you up when slowing down because there is a big toque on the copter when traveling fast (called motor flapping). As the I term is made smaller it takes longer for the I term to reduce so it is fighting the controller.
Glad you are enjoying the ACRO!
Installed 3.0.1 yesterday to get a picture of its GPS improvements (marginal in terms of FPV flying). Did not take the time to change PIDs, just flew around a bit. I don´t know if it is because of my low I terms, but 3.0.1 is A LOT less stable when slowing down harshly from speed to hover with sharp turns, Especially on roll there is a tendency to overshoot, copter does not hold the attitude when stick is centered, does not stop rolling, kind of undercuts sometimes mixed with a bit of yaw.
I must admit I simply went back to rc1ACRO instead of tweaking PIDs as it fits my needs far more. But the difference was impressive!
Hey Leonard - the orange lines in the plots are where the rate is near zero, so you can see in the second plot that you get a constant line where the leveling is cancelling out the acro rate - meaning you can hold any arbitrary angle with that stick deflection. The particular deflection where that occurs will depend on your StabKp / BAL / AcroKp factors. Basically inside that deflection the copter auto levels to a particular angle (see the orange diagonal line up there) and outside it rolls at a constant rate. I flew it tonight and I was very impressed - it lets you do big roll arcs at the rate you decide, even with stiff leveling. Before this mod, I could only do half an arc and then it would essentially snap back to level on the underside at full rate.
One thing I noticed is that I couldn't run my full 6.5 StabKp without oscillations, so I had to back it off (did that using 75% balance factor). I don't know if bringing the stability integrator in would clear that up (might just make it worse?) - what might help though is a D term, so I might try that next.
As for the bleeding, I don't know it it's necessary. I added it early on because I had a little takeoff mishap. The 300/301 factor I'm using takes 2 seconds to bleed half the error, so it takes quite a bit of time to fully settle out (10 seconds to get down to 3%). You are right that it will contribute to an small error in angle holding - I just don't notice it because I tend to rely on the leveling to hold an angle.
I've also seen big Yaw jumps when switching out of acro until I added this line of code to set_mode() to reset the nav_yaw target on every mode switch. Further down I reset the angle integrators for body frame mode (but I think you already do that):
nav_yaw = ahrs.yaw_sensor; // always set yaw target to current heading on mode change
switch(control_mode)
{
case ACRO:
ap.manual_throttle = true;
ap.manual_attitude = true;
set_yaw_mode(YAW_ACRO);
set_roll_pitch_mode(ROLL_PITCH_ACRO);
set_throttle_mode(THROTTLE_MANUAL);
if(!g.axis_enabled)
{
// initialize for body frame stabilization (these are used as angle error integrators)
roll_axis = 0;
pitch_axis = 0;
yaw_axis = 0;
}
else
{
// reset acro axis targets to current attitude (yaw set above)
roll_axis = ahrs.roll_sensor;
pitch_axis = ahrs.pitch_sensor;
yaw_axis = ahrs.yaw_sensor;
}
break;
Here's the code I flew tonight (still 2.9.1b):
#define _BLEED 300
#define BLEED_CONST (_BLEED/(_BLEED + 1.0))
// use max stick deflection of roll, pitch or yaw to determine level mix
float CalcLevelMix()
{
float r = ((float)labs(g.rc_1.control_in)) / MAX_INPUT_ROLL_ANGLE;
float p = ((float)labs(g.rc_2.control_in)) / MAX_INPUT_PITCH_ANGLE;
float y = ((float)labs(g.rc_4.control_in)) / MAX_INPUT_YAW_ANGLE;
p = max(y,max(r,p));
return 1.0 - p;
}
int32_t CalcLimit(int32_t acro_rate, int32_t max_level_rate)
{
return abs(abs(acro_rate) - max_level_rate);
}
// Roll with rate input and stabilized to body frame
static void
get_roll_rate_stabilized_bf(int32_t roll_stick_angle)
{
// Convert the input to the desired roll rate
int32_t acro_rate = roll_stick_angle * g.acro_p;
// Calculate rate limiter to avoid discontinuity when crossing 180
float lf = CalcLevelMix() * (g.acro_balance_roll/100.0) * cos_pitch_x;
int32_t limit = CalcLimit(acro_rate, lf*g.pi_stabilize_roll.get_p(4500));
// Calculate level rate for blending (similar to MultiWiii horizon mode), ignore roll level as we approach pitch +-90
int32_t level_rate = g.pi_stabilize_roll.get_p(constrain(-ahrs.roll_sensor, -4500, 4500)) * lf;
// combine/limit rate
acro_rate = constrain((acro_rate + level_rate), -limit, limit);
// Update axis movement
roll_axis -= ((omega.x * DEGX100) * ins.get_delta_time());
if (motors.armed() == false || ((g.rc_3.control_in == 0) && !ap.failsafe)) {
roll_axis = 0;
acro_rate = 0;
}
// Set body frame target for rate controller (correction rate + acro rate)
set_roll_rate_target(g.pi_stabilize_roll.get_p(roll_axis) + acro_rate, BODY_FRAME);
// Calculate target for next iteration
roll_axis += (acro_rate * G_Dt);
// Bleed axis error
roll_axis = roll_axis * BLEED_CONST;
// Error with maximum of +- max_angle_overshoot
roll_axis = constrain(roll_axis, -MAX_BF_ROLL_OVERSHOOT, MAX_BF_ROLL_OVERSHOOT);
}
// Pitch with rate input and stabilized to body frame
static void
get_pitch_rate_stabilized_bf(int32_t pitch_stick_angle)
{
// Convert the input to the desired pitch rate
int32_t acro_rate = pitch_stick_angle * g.acro_p;
// Calculate rate limiter to avoid discontinuity when crossing 180
float lf = CalcLevelMix() * (g.acro_balance_pitch/100.0) * cos_roll_x;
int32_t limit = CalcLimit(acro_rate, lf*g.pi_stabilize_pitch.get_p(4500));
// Calculate level rate for blending (similar to MultiWiii horizon mode)
int32_t level_rate = g.pi_stabilize_pitch.get_p(constrain(-ahrs.pitch_sensor, -4500, 4500)) * lf;
// combine/limit rate
acro_rate = constrain((acro_rate + level_rate), -limit, limit);
// Update axis movement
pitch_axis -= ((omega.y * DEGX100) * ins.get_delta_time());
if (motors.armed() == false || ((g.rc_3.control_in == 0) && !ap.failsafe)) {
pitch_axis = 0;
acro_rate = 0;
}
// Set body frame target for rate controller (correction rate + acro rate)
set_pitch_rate_target(g.pi_stabilize_pitch.get_p(pitch_axis) + acro_rate, BODY_FRAME);
// Calculate target for next iteration
pitch_axis += (acro_rate * G_Dt);
// Bleed axis error
pitch_axis = pitch_axis * BLEED_CONST;
// Error with maximum of +- max_angle_overshoot
pitch_axis= constrain(pitch_axis, -MAX_BF_PITCH_OVERSHOOT, MAX_BF_PITCH_OVERSHOOT);
}
// Yaw with rate input and stabilized to body frame
static void
get_yaw_rate_stabilized_bf(int32_t yaw_stick_angle, int32_t pitch_stick_angle)
{
// Convert the input to the desired yaw rate
int32_t acro_rate = yaw_stick_angle * g.acro_p;
// Calculate rate limiter to avoid discontinuity when crossing 180
float lf = CalcLevelMix() * (g.acro_balance_pitch/100.0) * (-sin_roll);
int32_t limit = CalcLimit(acro_rate, lf*g.pi_stabilize_pitch.get_p(4500));
// Calculate level rate for blending (similar to MultiWiii horizon mode)
int32_t level_rate = g.pi_stabilize_yaw.get_p(constrain(-ahrs.pitch_sensor, -4500, 4500)) * lf;
// combine/limit rate
acro_rate = constrain((acro_rate + level_rate), -limit, limit);
// Update axis movement
yaw_axis -= ((omega.z * DEGX100) * ins.get_delta_time());
if (motors.armed() == false || ((g.rc_3.control_in == 0) && !ap.failsafe)) {
yaw_axis = 0;
acro_rate = 0;
}
// Set body frame target for rate controller (correction rate + acro rate)
set_yaw_rate_target(g.pi_stabilize_yaw.get_p(yaw_axis) + acro_rate, BODY_FRAME);
// Calculate target for next iteration
yaw_axis += (acro_rate * G_Dt);
// Bleed axis error
yaw_axis = yaw_axis * BLEED_CONST;
// Error with maximum of +- max_angle_overshoot
yaw_axis = constrain(yaw_axis, -MAX_BF_YAW_OVERSHOOT, MAX_BF_YAW_OVERSHOOT);
}
//////////////////////////////////////////////////////////////////////////////
-
3
-
4
-
5
-
6
-
7
of 15 Next