Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

New stable mode parameter compatible with configurator 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@70 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jjulio1234 2010-08-10 21:06:01 +00:00
parent dfa605b00b
commit 4af9be6e85
3 changed files with 11 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
Arducopter/Arducopter.pde
View File

@ -277,77 +277,12 @@ void Position_control(long lat_dest, long lon_dest)
command_gps_pitch = constrain(command_gps_pitch,-GPS_MAX_ANGLE,GPS_MAX_ANGLE); // Limit max command command_gps_pitch = constrain(command_gps_pitch,-GPS_MAX_ANGLE,GPS_MAX_ANGLE); // Limit max command
} }
/* ************************************************************ */
// STABLE MODE
// ROLL, PITCH and YAW PID controls...
// Input : desired Roll, Pitch and Yaw absolute angles. Output : Motor commands
void Attitude_control()
{
// ROLL CONTROL
if (AP_mode==2) // Normal Mode => Stabilization mode
err_roll = command_rx_roll - ToDeg(roll);
else
err_roll = (command_rx_roll + command_gps_roll) - ToDeg(roll); // Position control
err_roll = constrain(err_roll,-25,25); // to limit max roll command...
roll_I += err_roll*G_Dt;
roll_I = constrain(roll_I,-20,20);
// D term implementation => two parts: gyro part and command part
// To have a better (faster) response we can use the Gyro reading directly for the Derivative term...
// Omega[] is the raw gyro reading plus Omega_I, so it´s bias corrected
// We also add a part that takes into account the command from user (stick) to make the system more responsive to user inputs
roll_D = command_rx_roll_diff*KD_QUAD_COMMAND_PART - ToDeg(Omega[0]); // Take into account Angular velocity of the stick (command)
// PID control
K_aux = KP_QUAD_ROLL; // Comment this out if you want to use transmitter to adjust gain
control_roll = K_aux*err_roll + KD_QUAD_ROLL*roll_D + KI_QUAD_ROLL*roll_I;
// PITCH CONTROL
if (AP_mode==2) // Normal mode => Stabilization mode
err_pitch = command_rx_pitch - ToDeg(pitch);
else
err_pitch = (command_rx_pitch + command_gps_pitch) - ToDeg(pitch); // Position Control
err_pitch = constrain(err_pitch,-25,25); // to limit max pitch command...
pitch_I += err_pitch*G_Dt;
pitch_I = constrain(pitch_I,-20,20);
// D term
pitch_D = command_rx_pitch_diff*KD_QUAD_COMMAND_PART - ToDeg(Omega[1]);
// PID control
K_aux = KP_QUAD_PITCH; // Comment this out if you want to use transmitter to adjust gain
control_pitch = K_aux*err_pitch + KD_QUAD_PITCH*pitch_D + KI_QUAD_PITCH*pitch_I;
// YAW CONTROL
err_yaw = command_rx_yaw - ToDeg(yaw);
if (err_yaw > 180) // Normalize to -180,180
err_yaw -= 360;
else if(err_yaw < -180)
err_yaw += 360;
err_yaw = constrain(err_yaw,-60,60); // to limit max yaw command...
yaw_I += err_yaw*G_Dt;
yaw_I = constrain(yaw_I,-20,20);
yaw_D = command_rx_yaw_diff*KD_QUAD_COMMAND_PART - ToDeg(Omega[2]);
// PID control
control_yaw = KP_QUAD_YAW*err_yaw + KD_QUAD_YAW*yaw_D + KI_QUAD_YAW*yaw_I;
}
/* ************************************************************ */ /* ************************************************************ */
// STABLE MODE // STABLE MODE
// ROLL, PITCH and YAW PID controls... // ROLL, PITCH and YAW PID controls...
// 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_v2() void Attitude_control_v2()
{ {
#define STABLE_MODE_KP_RATE_ROLL 0.2
#define STABLE_MODE_KP_RATE_PITCH 0.2
float err_roll_rate; float err_roll_rate;
float err_pitch_rate; float err_pitch_rate;
float roll_rate; float roll_rate;
@ -374,7 +309,7 @@ void Attitude_control_v2()
// PID control // PID control
K_aux = KP_QUAD_ROLL; // Comment this out if you want to use transmitter to adjust gain K_aux = KP_QUAD_ROLL; // Comment this out if you want to use transmitter to adjust gain
control_roll = K_aux*err_roll + KD_QUAD_ROLL*roll_D + KI_QUAD_ROLL*roll_I + STABLE_MODE_KP_RATE_ROLL*err_roll_rate; ; control_roll = K_aux*err_roll + KD_QUAD_ROLL*roll_D + KI_QUAD_ROLL*roll_I + STABLE_MODE_KP_RATE*err_roll_rate; ;
// PITCH CONTROL // PITCH CONTROL
if (AP_mode==2) // Normal mode => Stabilization mode if (AP_mode==2) // Normal mode => Stabilization mode
@ -395,7 +330,7 @@ void Attitude_control_v2()
// PID control // PID control
K_aux = KP_QUAD_PITCH; // Comment this out if you want to use transmitter to adjust gain K_aux = KP_QUAD_PITCH; // Comment this out if you want to use transmitter to adjust gain
control_pitch = K_aux*err_pitch + KD_QUAD_PITCH*pitch_D + KI_QUAD_PITCH*pitch_I + STABLE_MODE_KP_RATE_PITCH*err_pitch_rate; control_pitch = K_aux*err_pitch + KD_QUAD_PITCH*pitch_D + KI_QUAD_PITCH*pitch_I + STABLE_MODE_KP_RATE*err_pitch_rate;
// YAW CONTROL // YAW CONTROL
err_yaw = command_rx_yaw - ToDeg(yaw); err_yaw = command_rx_yaw - ToDeg(yaw);
@ -408,7 +343,7 @@ void Attitude_control_v2()
yaw_I += err_yaw*G_Dt; yaw_I += err_yaw*G_Dt;
yaw_I = constrain(yaw_I,-20,20); yaw_I = constrain(yaw_I,-20,20);
yaw_D = command_rx_yaw_diff*KD_QUAD_COMMAND_PART - ToDeg(Omega[2]); yaw_D = - ToDeg(Omega[2]);
// PID control // PID control
control_yaw = KP_QUAD_YAW*err_yaw + KD_QUAD_YAW*yaw_D + KI_QUAD_YAW*yaw_I; control_yaw = KP_QUAD_YAW*err_yaw + KD_QUAD_YAW*yaw_D + KI_QUAD_YAW*yaw_I;
@ -559,7 +494,7 @@ void setup()
} }
//Serial.println(); //Serial.println();
Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],ch_throttle); Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],ch_throttle);
delay(14); delay(10);
} }
for(int y=0; y<=2; y++) for(int y=0; y<=2; y++)
AN_OFFSET[y]=aux_float[y]; AN_OFFSET[y]=aux_float[y];

8
Arducopter/SerialCom.pde
View File

@ -33,7 +33,7 @@ void readSerialCommand() {
KP_QUAD_YAW = readFloatSerial(); KP_QUAD_YAW = readFloatSerial();
KI_QUAD_YAW = readFloatSerial(); KI_QUAD_YAW = readFloatSerial();
KD_QUAD_YAW = readFloatSerial(); KD_QUAD_YAW = readFloatSerial();
KD_QUAD_COMMAND_PART = readFloatSerial(); STABLE_MODE_KP_RATE = readFloatSerial();
MAGNETOMETER = readFloatSerial(); MAGNETOMETER = readFloatSerial();
break; break;
case 'C': // Receive GPS PID case 'C': // Receive GPS PID
@ -96,7 +96,7 @@ void readSerialCommand() {
writeEEPROM(KP_QUAD_YAW, KP_QUAD_YAW_ADR); writeEEPROM(KP_QUAD_YAW, KP_QUAD_YAW_ADR);
writeEEPROM(KD_QUAD_YAW, KD_QUAD_YAW_ADR); writeEEPROM(KD_QUAD_YAW, KD_QUAD_YAW_ADR);
writeEEPROM(KI_QUAD_YAW, KI_QUAD_YAW_ADR); writeEEPROM(KI_QUAD_YAW, KI_QUAD_YAW_ADR);
writeEEPROM(KD_QUAD_COMMAND_PART, KD_QUAD_COMMAND_PART_ADR); writeEEPROM(STABLE_MODE_KP_RATE, STABLE_MODE_KP_RATE_ADR);
writeEEPROM(KP_GPS_ROLL, KP_GPS_ROLL_ADR); writeEEPROM(KP_GPS_ROLL, KP_GPS_ROLL_ADR);
writeEEPROM(KD_GPS_ROLL, KD_GPS_ROLL_ADR); writeEEPROM(KD_GPS_ROLL, KD_GPS_ROLL_ADR);
writeEEPROM(KI_GPS_ROLL, KI_GPS_ROLL_ADR); writeEEPROM(KI_GPS_ROLL, KI_GPS_ROLL_ADR);
@ -140,7 +140,7 @@ void readSerialCommand() {
KP_QUAD_YAW = 3.6; KP_QUAD_YAW = 3.6;
KD_QUAD_YAW = 1.2; KD_QUAD_YAW = 1.2;
KI_QUAD_YAW = 0.15; KI_QUAD_YAW = 0.15;
KD_QUAD_COMMAND_PART = 0.0; STABLE_MODE_KP_RATE = 0.2; // New param for stable mode
KP_GPS_ROLL = 0.012; KP_GPS_ROLL = 0.012;
KD_GPS_ROLL = 0.005; KD_GPS_ROLL = 0.005;
KI_GPS_ROLL = 0.004; KI_GPS_ROLL = 0.004;
@ -217,7 +217,7 @@ void sendSerialTelemetry() {
comma(); comma();
Serial.print(KD_QUAD_YAW, 3); Serial.print(KD_QUAD_YAW, 3);
comma(); comma();
Serial.print(KD_QUAD_COMMAND_PART, 3); Serial.print(STABLE_MODE_KP_RATE, 3);
comma(); comma();
Serial.println(MAGNETOMETER, 3); Serial.println(MAGNETOMETER, 3);
queryType = 'X'; queryType = 'X';

6
Arducopter/UserSettings.h
View File

@ -30,7 +30,7 @@ float KI_QUAD_PITCH;
float KP_QUAD_YAW; float KP_QUAD_YAW;
float KD_QUAD_YAW; float KD_QUAD_YAW;
float KI_QUAD_YAW; float KI_QUAD_YAW;
float KD_QUAD_COMMAND_PART; float STABLE_MODE_KP_RATE;
float KP_GPS_ROLL; float KP_GPS_ROLL;
float KD_GPS_ROLL; float KD_GPS_ROLL;
float KI_GPS_ROLL; float KI_GPS_ROLL;
@ -74,7 +74,7 @@ float xmitFactor;
#define KP_QUAD_YAW_ADR 24 #define KP_QUAD_YAW_ADR 24
#define KD_QUAD_YAW_ADR 28 #define KD_QUAD_YAW_ADR 28
#define KI_QUAD_YAW_ADR 32 #define KI_QUAD_YAW_ADR 32
#define KD_QUAD_COMMAND_PART_ADR 36 #define STABLE_MODE_KP_RATE_ADR 36
#define KP_GPS_ROLL_ADR 40 #define KP_GPS_ROLL_ADR 40
#define KD_GPS_ROLL_ADR 44 #define KD_GPS_ROLL_ADR 44
#define KI_GPS_ROLL_ADR 48 #define KI_GPS_ROLL_ADR 48
@ -141,7 +141,7 @@ void readUserConfig() {
KP_QUAD_YAW = readEEPROM(KP_QUAD_YAW_ADR); KP_QUAD_YAW = readEEPROM(KP_QUAD_YAW_ADR);
KD_QUAD_YAW = readEEPROM(KD_QUAD_YAW_ADR); KD_QUAD_YAW = readEEPROM(KD_QUAD_YAW_ADR);
KI_QUAD_YAW = readEEPROM(KI_QUAD_YAW_ADR); KI_QUAD_YAW = readEEPROM(KI_QUAD_YAW_ADR);
KD_QUAD_COMMAND_PART = readEEPROM(KD_QUAD_COMMAND_PART_ADR); STABLE_MODE_KP_RATE = readEEPROM(STABLE_MODE_KP_RATE_ADR);
KP_GPS_ROLL = readEEPROM(KP_GPS_ROLL_ADR); KP_GPS_ROLL = readEEPROM(KP_GPS_ROLL_ADR);
KD_GPS_ROLL = readEEPROM(KD_GPS_ROLL_ADR); KD_GPS_ROLL = readEEPROM(KD_GPS_ROLL_ADR);
KI_GPS_ROLL = readEEPROM(KI_GPS_ROLL_ADR); KI_GPS_ROLL = readEEPROM(KI_GPS_ROLL_ADR);