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

Update to Stable mode function to limit the maximun output value. This prevents to enter in unstable state if there are a big external perturbation.

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@522 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jjulio1234 2010-09-16 21:50:26 +00:00
parent 6cf2d1e86e
commit d342f7582d
1 changed files with 5 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Arducopter/Arducopter.pde
View File

@ -135,6 +135,7 @@
// Input : desired Roll, Pitch and Yaw absolute angles. Output : Motor commands // Input : desired Roll, Pitch and Yaw absolute angles. Output : Motor commands
void Attitude_control_v3() void Attitude_control_v3()
{ {
#define MAX_CONTROL_OUTPUT 250
float stable_roll,stable_pitch,stable_yaw; float stable_roll,stable_pitch,stable_yaw;
// ROLL CONTROL // ROLL CONTROL
@ -154,6 +155,7 @@ void Attitude_control_v3()
// PD rate control (we use also the bias corrected gyro rates) // PD rate control (we use also the bias corrected gyro rates)
err_roll = stable_roll - ToDeg(Omega[0]); // Omega[] is the raw gyro reading plus Omega_I, so it´s bias corrected err_roll = stable_roll - ToDeg(Omega[0]); // Omega[] is the raw gyro reading plus Omega_I, so it´s bias corrected
control_roll = STABLE_MODE_KP_RATE_ROLL*err_roll; control_roll = STABLE_MODE_KP_RATE_ROLL*err_roll;
control_roll = constrain(control_roll,-MAX_CONTROL_OUTPUT,MAX_CONTROL_OUTPUT);
// PITCH CONTROL // PITCH CONTROL
if (AP_mode==2) // Normal mode => Stabilization mode if (AP_mode==2) // Normal mode => Stabilization mode
@ -172,6 +174,7 @@ void Attitude_control_v3()
// P rate control (we use also the bias corrected gyro rates) // P rate control (we use also the bias corrected gyro rates)
err_pitch = stable_pitch - ToDeg(Omega[1]); err_pitch = stable_pitch - ToDeg(Omega[1]);
control_pitch = STABLE_MODE_KP_RATE_PITCH*err_pitch; control_pitch = STABLE_MODE_KP_RATE_PITCH*err_pitch;
control_pitch = constrain(control_pitch,-MAX_CONTROL_OUTPUT,MAX_CONTROL_OUTPUT);
// YAW CONTROL // YAW CONTROL
err_yaw = command_rx_yaw - ToDeg(yaw); err_yaw = command_rx_yaw - ToDeg(yaw);
@ -189,6 +192,7 @@ void Attitude_control_v3()
// PD rate control (we use also the bias corrected gyro rates) // PD rate control (we use also the bias corrected gyro rates)
err_yaw = stable_yaw - ToDeg(Omega[2]); err_yaw = stable_yaw - ToDeg(Omega[2]);
control_yaw = STABLE_MODE_KP_RATE_YAW*err_yaw; control_yaw = STABLE_MODE_KP_RATE_YAW*err_yaw;
control_yaw = constrain(control_yaw,-MAX_CONTROL_OUTPUT,MAX_CONTROL_OUTPUT);
} }
// ACRO MODE // ACRO MODE