Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: allow integrator in stabilize mode when no stick input

Browse Source 
fixes issue #472
This commit is contained in:
Andrew Tridgell 2013-08-02 21:55:34 +10:00
parent a7cbebbeba
commit 890b77447a
1 changed files with 15 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
ArduPlane/Attitude.pde
View File

@ -73,9 +73,13 @@ static void stabilize_roll(float speed_scaler)
if (ahrs.roll_sensor < 0) nav_roll_cd -= 36000; if (ahrs.roll_sensor < 0) nav_roll_cd -= 36000;
} }
bool disable_integrator = false;
if (control_mode == STABILIZE && channel_roll->control_in != 0) {
disable_integrator = true;
}
channel_roll->servo_out = g.rollController.get_servo_out(nav_roll_cd - ahrs.roll_sensor, channel_roll->servo_out = g.rollController.get_servo_out(nav_roll_cd - ahrs.roll_sensor,
speed_scaler, speed_scaler,
control_mode == STABILIZE); disable_integrator);
} }
/* /*
@ -86,9 +90,13 @@ static void stabilize_roll(float speed_scaler)
static void stabilize_pitch(float speed_scaler) static void stabilize_pitch(float speed_scaler)
{ {
int32_t demanded_pitch = nav_pitch_cd + g.pitch_trim_cd + channel_throttle->servo_out * g.kff_throttle_to_pitch; int32_t demanded_pitch = nav_pitch_cd + g.pitch_trim_cd + channel_throttle->servo_out * g.kff_throttle_to_pitch;
bool disable_integrator = false;
if (control_mode == STABILIZE && channel_pitch->control_in != 0) {
disable_integrator = true;
}
channel_pitch->servo_out = g.pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor, channel_pitch->servo_out = g.pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor,
speed_scaler, speed_scaler,
control_mode == STABILIZE); disable_integrator);
} }
/* /*
@ -374,8 +382,11 @@ static void calc_nav_yaw(float speed_scaler, float ch4_inf)
return; return;
} }
channel_rudder->servo_out = g.yawController.get_servo_out(speed_scaler, bool disable_integrator = false;
control_mode == STABILIZE); if (control_mode == STABILIZE && channel_rudder->control_in != 0) {
disable_integrator = true;
}
channel_rudder->servo_out = g.yawController.get_servo_out(speed_scaler, disable_integrator);
// add in rudder mixing from roll // add in rudder mixing from roll
channel_rudder->servo_out += channel_roll->servo_out * g.kff_rudder_mix; channel_rudder->servo_out += channel_roll->servo_out * g.kff_rudder_mix;