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

Copter: simplify roll-pitch stabilize controller

Browse Source 
remove check of whether to freeze i term when attitude is less than 5
degrees from horizontal.  I terms have been moved to the rate
controllers.
This commit is contained in:
Randy Mackay 2013-04-22 18:47:56 +09:00
parent 82c604dd67
commit 0b29754920
1 changed files with 6 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
ArduCopter/Attitude.pde
View File

@ -9,19 +9,11 @@ get_stabilize_roll(int32_t target_angle)
// limit the error we're feeding to the PID
target_angle = constrain_int32(target_angle, -4500, 4500);
// convert to desired Rate:
int32_t target_rate = g.pi_stabilize_roll.get_p(target_angle);
int16_t i_stab;
if(labs(ahrs.roll_sensor) < 500) {
target_angle = constrain_int32(target_angle, -500, 500);
i_stab = g.pi_stabilize_roll.get_i(target_angle, G_Dt);
}else{
i_stab = g.pi_stabilize_roll.get_integrator();
}
// convert to desired rate
int32_t target_rate = g.pi_stabilize_roll.get_pi(target_angle, G_Dt);
// set targets for rate controller
set_roll_rate_target(target_rate+i_stab, EARTH_FRAME);
set_roll_rate_target(target_rate, EARTH_FRAME);
}
static void
@ -33,19 +25,11 @@ get_stabilize_pitch(int32_t target_angle)
// limit the error we're feeding to the PID
target_angle = constrain_int32(target_angle, -4500, 4500);
// convert to desired Rate:
int32_t target_rate = g.pi_stabilize_pitch.get_p(target_angle);
int16_t i_stab;
if(labs(ahrs.pitch_sensor) < 500) {
target_angle = constrain_int32(target_angle, -500, 500);
i_stab = g.pi_stabilize_pitch.get_i(target_angle, G_Dt);
}else{
i_stab = g.pi_stabilize_pitch.get_integrator();
}
// convert to desired rate
int32_t target_rate = g.pi_stabilize_pitch.get_pi(target_angle, G_Dt);
// set targets for rate controller
set_pitch_rate_target(target_rate + i_stab, EARTH_FRAME);
set_pitch_rate_target(target_rate, EARTH_FRAME);
}
static void