Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Implementing Yaw Fix. Tried to blend with Jason's changes.

Browse Source
This commit is contained in:
Robert Lefebvre 2012-07-14 00:24:22 -04:00
parent ed235a3924
commit 59e133235f
2 changed files with 71 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduCopter/ArduCopter.pde
View File

@ -1514,6 +1514,9 @@ void update_yaw_mode(void)
}else if (!yaw_stopped){
g.rc_4.servo_out = get_acro_yaw(0);
yaw_timer--;
if ( abs(omega.z * DEGX100) < 1000 ){
yaw_stopped = true;
}
if(yaw_timer == 0){
yaw_stopped = true;
nav_yaw = ahrs.yaw_sensor;

69
ArduCopter/Attitude.pde
View File

@ -130,8 +130,75 @@ get_acro_pitch(int32_t target_rate)
static int16_t
get_acro_yaw(int32_t 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 rate_error; // current yaw rate error
int32_t current_rate; // current real yaw rate
int32_t decel_boost; // gain scheduling if we are overshooting
int32_t output; // output to rate controller
target_rate = g.pi_stabilize_yaw.get_p(target_rate);
return get_rate_yaw(target_rate);
current_rate = omega.z * DEGX100;
rate_error = target_rate - current_rate;
//Gain Scheduling:
//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
//going to overshoot the yaw target to the left side, so we should
//strengthen the yaw output to slow down the yaw!
#if (FRAME_CONFIG == HELI_FRAME || FRAME_CONFIG == TRI_FRAME)
if ( target_rate > 0 && last_target_rate > target_rate && rate_error < 0 ){
decel_boost = 1;
} else if (target_rate < 0 && last_target_rate < target_rate && rate_error > 0 ){
decel_boost = 1;
} else if (target_rate == 0 && abs(current_rate) > 1000){
decel_boost = 1;
} else {
decel_boost = 0;
}
last_target_rate = target_rate;
#else
decel_boost = 0;
#endif
// separately calculate p, i, d values for logging
// we will use d=0, and hold i at it's last value
// since manual inputs are never steady state
p = g.pid_rate_yaw.get_p(rate_error);
i = g.pid_rate_yaw.get_integrator();
d = 0;
if (decel_boost){
p *= 2;
}
output = p+i+d;
// output control:
// constrain output
output = constrain(output, -4500, 4500);
#if LOGGING_ENABLED == ENABLED
static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash
// log output if PID loggins is on and we are tuning the yaw
if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_YAW_KP || g.radio_tuning == CH6_YAW_RATE_KP) ) {
log_counter++;
if( log_counter >= 10 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10
log_counter = 0;
Log_Write_PID(CH6_YAW_RATE_KP, rate_error, p, i, d, output, tuning_value);
}
}
#endif
return output;
}
static int16_t