aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: autotune disables rate output from stab controller

Browse Source
This commit is contained in:
Randy Mackay 2013-10-13 11:56:01 +09:00
parent b69f08c03e
commit 6876107a15
4 changed files with 23 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/ArduCopter.pde
View File

@ -396,6 +396,8 @@ static union {
uint8_t CH8_flag : 2; // 13,14 // ch8 aux switch : 0 is low or false, 1 is center or true, 2 is high
uint8_t usb_connected : 1; // 15 // true if APM is powered from USB connection
uint8_t yaw_stopped : 1; // 16 // Used to manage the Yaw hold capabilities
uint8_t disable_stab_rate_limit : 1; // 17 // disables limits rate request from the stability controller
};
uint32_t value;
} ap;

16
ArduCopter/Attitude.pde
View File

@ -31,12 +31,14 @@ get_stabilize_roll(int32_t target_angle)
// angle error
target_angle = wrap_180_cd(target_angle - ahrs.roll_sensor);
// limit the error we're feeding to the PID
target_angle = constrain_int32(target_angle, -g.angle_max, g.angle_max);
// convert to desired rate
int32_t target_rate = g.pi_stabilize_roll.kP() * target_angle;
// constrain the target rate
if (!ap.disable_stab_rate_limit) {
target_rate = constrain_int32(target_rate, -STABILIZE_RATE_LIMIT, STABILIZE_RATE_LIMIT);
}
// set targets for rate controller
set_roll_rate_target(target_rate, EARTH_FRAME);
}
@ -47,12 +49,14 @@ get_stabilize_pitch(int32_t target_angle)
// angle error
target_angle = wrap_180_cd(target_angle - ahrs.pitch_sensor);
// limit the error we're feeding to the PID
target_angle = constrain_int32(target_angle, -g.angle_max, g.angle_max);
// convert to desired rate
int32_t target_rate = g.pi_stabilize_pitch.kP() * target_angle;
// constrain the target rate
if (!ap.disable_stab_rate_limit) {
target_rate = constrain_int32(target_rate, -STABILIZE_RATE_LIMIT, STABILIZE_RATE_LIMIT);
}
// set targets for rate controller
set_pitch_rate_target(target_rate, EARTH_FRAME);
}

8
ArduCopter/auto_tune.pde
View File

@ -108,6 +108,7 @@ void auto_tune_restore_orig_pids()
g.pid_rate_pitch.kI(orig_pitch_ri);
g.pid_rate_pitch.kD(orig_pitch_rd);
g.pi_stabilize_pitch.kP(orig_pitch_sp);
ap.disable_stab_rate_limit = false;
}
// auto_tune_load_tuned_pids - restore pids to their original values
@ -122,6 +123,7 @@ void auto_tune_load_tuned_pids()
g.pid_rate_pitch.kI(tune_pitch_rp*AUTO_TUNE_RP_RATIO_FINAL);
g.pid_rate_pitch.kD(tune_pitch_rd);
g.pi_stabilize_pitch.kP(tune_pitch_sp);
ap.disable_stab_rate_limit = false;
}
}
@ -164,6 +166,7 @@ void auto_tune_stop()
if (auto_tune_state.enabled) {
auto_tune_state.enabled = false;
auto_tune_state.active = false;
ap.disable_stab_rate_limit = false;
if (roll_pitch_mode == ROLL_PITCH_AUTOTUNE) {
set_roll_pitch_mode(ROLL_PITCH_STABLE); // restore roll-pitch mode
rate_targets_frame = EARTH_FRAME; // regular stabilize mode frame
@ -260,6 +263,7 @@ get_autotune_roll_pitch_controller(int32_t pilot_roll_angle, int32_t pilot_pitch
g.pid_rate_pitch.kD(tune_pitch_rd);
g.pi_stabilize_pitch.kP(tune_pitch_sp);
}
ap.disable_stab_rate_limit = true; // disable rate limits
}
break;
@ -376,7 +380,6 @@ get_autotune_roll_pitch_controller(int32_t pilot_roll_angle, int32_t pilot_pitch
case AUTO_TUNE_STEP_UPDATE_GAINS:
// restore pids to their original values
//auto_tune_restore_orig_pids();
g.pid_rate_roll.kP(orig_roll_rp);
g.pid_rate_roll.kI(orig_roll_rp*AUTO_TUNE_PI_RATIO_FOR_TESTING);
g.pid_rate_roll.kD(orig_roll_rd);
@ -386,6 +389,9 @@ get_autotune_roll_pitch_controller(int32_t pilot_roll_angle, int32_t pilot_pitch
g.pid_rate_pitch.kD(orig_pitch_rd);
g.pi_stabilize_pitch.kP(orig_pitch_sp);
// re-enable the rate limits
ap.disable_stab_rate_limit = false;
// logging
if (auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL) {
Log_Write_AutoTune(auto_tune_state.axis, auto_tune_state.tune_type, auto_tune_test_min, auto_tune_test_max, tune_roll_rp, tune_roll_rd, tune_roll_sp);

4
ArduCopter/config.h
View File

@ -785,6 +785,10 @@
# define STABILIZE_PITCH_IMAX 0
#endif
#ifndef STABILIZE_RATE_LIMIT
# define STABILIZE_RATE_LIMIT 18000
#endif
#ifndef STABILIZE_YAW_P
# define STABILIZE_YAW_P 4.5f // increase for more aggressive Yaw Hold, decrease if it's bouncy
#endif