Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Arducopter: Temp reversion to old Yaw controller.

This commit is contained in:
Jason Short 2012-07-14 12:26:13 -07:00
parent e3af0c7920
commit 44b16b7b61
1 changed files with 20 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
ArduCopter/Attitude.pde
View File

@ -129,6 +129,13 @@ get_acro_pitch(int32_t target_rate)
static int16_t static int16_t
get_acro_yaw(int32_t target_rate) get_acro_yaw(int32_t target_rate)
{
target_rate = g.pi_stabilize_yaw.get_p(target_rate);
return get_rate_yaw(target_rate);
}
static int16_t
get_acro_yaw2(int32_t target_rate)
{ {
static int32_t last_target_rate = 0; // last iteration's target rate static int32_t last_target_rate = 0; // last iteration's target rate
int32_t p,i,d; // used to capture pid values for logging int32_t p,i,d; // used to capture pid values for logging
@ -140,36 +147,36 @@ get_acro_yaw(int32_t target_rate)
target_rate = g.pi_stabilize_yaw.get_p(target_rate); target_rate = g.pi_stabilize_yaw.get_p(target_rate);
current_rate = omega.z * DEGX100; current_rate = omega.z * DEGX100;
rate_error = target_rate - current_rate; rate_error = target_rate - current_rate;
//Gain Scheduling: //Gain Scheduling:
//If the yaw input is to the right, but stick is moving to the middle //If the yaw input is to the right, but stick is moving to the middle
//and actual rate is greater than the target rate then we are //and actual rate is greater than the target rate then we are
//going to overshoot the yaw target to the left side, so we should //going to overshoot the yaw target to the left side, so we should
//strengthen the yaw output to slow down the yaw! //strengthen the yaw output to slow down the yaw!
#if (FRAME_CONFIG == HELI_FRAME || FRAME_CONFIG == TRI_FRAME) #if (FRAME_CONFIG == HELI_FRAME || FRAME_CONFIG == TRI_FRAME)
if ( target_rate > 0 && last_target_rate > target_rate && rate_error < 0 ){ if ( target_rate > 0 && last_target_rate > target_rate && rate_error < 0 ){
decel_boost = 1; decel_boost = 1;
} else if (target_rate < 0 && last_target_rate < target_rate && rate_error > 0 ){ } else if (target_rate < 0 && last_target_rate < target_rate && rate_error > 0 ){
decel_boost = 1; decel_boost = 1;
} else if (target_rate == 0 && abs(current_rate) > 1000){ } else if (target_rate == 0 && abs(current_rate) > 1000){
decel_boost = 1; decel_boost = 1;
} else { } else {
decel_boost = 0; decel_boost = 0;
} }
last_target_rate = target_rate; last_target_rate = target_rate;
#else #else
decel_boost = 0; decel_boost = 0;
#endif #endif
// separately calculate p, i, d values for logging // separately calculate p, i, d values for logging
// we will use d=0, and hold i at it's last value // we will use d=0, and hold i at it's last value
// since manual inputs are never steady state // since manual inputs are never steady state
p = g.pid_rate_yaw.get_p(rate_error); p = g.pid_rate_yaw.get_p(rate_error);
i = g.pid_rate_yaw.get_integrator(); i = g.pid_rate_yaw.get_integrator();
d = 0; d = 0;
@ -177,10 +184,10 @@ get_acro_yaw(int32_t target_rate)
if (decel_boost){ if (decel_boost){
p *= 2; p *= 2;
} }
output = p+i+d; output = p+i+d;
// output control: // output control:
// constrain output // constrain output
output = constrain(output, -4500, 4500); output = constrain(output, -4500, 4500);
@ -197,8 +204,6 @@ get_acro_yaw(int32_t target_rate)
#endif #endif
return output; return output;
} }
static int16_t static int16_t