mirror of https://github.com/ArduPilot/ardupilot
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
parent
dfa605b00b
commit
4af9be6e85
|
|
@ -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
|
||||
}
|
||||
|
||||
/* ************************************************************ */
|
||||
// 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
|
||||
// ROLL, PITCH and YAW PID controls...
|
||||
// Input : desired Roll, Pitch and Yaw absolute angles. Output : Motor commands
|
||||
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_pitch_rate;
|
||||
float roll_rate;
|
||||
|
|
@ -374,7 +309,7 @@ void Attitude_control_v2()
|
|||
|
||||
// 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 + 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
|
||||
if (AP_mode==2) // Normal mode => Stabilization mode
|
||||
|
|
@ -395,7 +330,7 @@ void Attitude_control_v2()
|
|||
|
||||
// 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 + 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
|
||||
err_yaw = command_rx_yaw - ToDeg(yaw);
|
||||
|
|
@ -408,7 +343,7 @@ void Attitude_control_v2()
|
|||
|
||||
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]);
|
||||
yaw_D = - ToDeg(Omega[2]);
|
||||
|
||||
// PID control
|
||||
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();
|
||||
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++)
|
||||
AN_OFFSET[y]=aux_float[y];
|
||||
|
|
|
|||
|
|
@ -33,7 +33,7 @@ void readSerialCommand() {
|
|||
KP_QUAD_YAW = readFloatSerial();
|
||||
KI_QUAD_YAW = readFloatSerial();
|
||||
KD_QUAD_YAW = readFloatSerial();
|
||||
KD_QUAD_COMMAND_PART = readFloatSerial();
|
||||
STABLE_MODE_KP_RATE = readFloatSerial();
|
||||
MAGNETOMETER = readFloatSerial();
|
||||
break;
|
||||
case 'C': // Receive GPS PID
|
||||
|
|
@ -96,7 +96,7 @@ void readSerialCommand() {
|
|||
writeEEPROM(KP_QUAD_YAW, KP_QUAD_YAW_ADR);
|
||||
writeEEPROM(KD_QUAD_YAW, KD_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(KD_GPS_ROLL, KD_GPS_ROLL_ADR);
|
||||
writeEEPROM(KI_GPS_ROLL, KI_GPS_ROLL_ADR);
|
||||
|
|
@ -140,7 +140,7 @@ void readSerialCommand() {
|
|||
KP_QUAD_YAW = 3.6;
|
||||
KD_QUAD_YAW = 1.2;
|
||||
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;
|
||||
KD_GPS_ROLL = 0.005;
|
||||
KI_GPS_ROLL = 0.004;
|
||||
|
|
@ -217,7 +217,7 @@ void sendSerialTelemetry() {
|
|||
comma();
|
||||
Serial.print(KD_QUAD_YAW, 3);
|
||||
comma();
|
||||
Serial.print(KD_QUAD_COMMAND_PART, 3);
|
||||
Serial.print(STABLE_MODE_KP_RATE, 3);
|
||||
comma();
|
||||
Serial.println(MAGNETOMETER, 3);
|
||||
queryType = 'X';
|
||||
|
|
|
|||
|
|
@ -30,7 +30,7 @@ float KI_QUAD_PITCH;
|
|||
float KP_QUAD_YAW;
|
||||
float KD_QUAD_YAW;
|
||||
float KI_QUAD_YAW;
|
||||
float KD_QUAD_COMMAND_PART;
|
||||
float STABLE_MODE_KP_RATE;
|
||||
float KP_GPS_ROLL;
|
||||
float KD_GPS_ROLL;
|
||||
float KI_GPS_ROLL;
|
||||
|
|
@ -74,7 +74,7 @@ float xmitFactor;
|
|||
#define KP_QUAD_YAW_ADR 24
|
||||
#define KD_QUAD_YAW_ADR 28
|
||||
#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 KD_GPS_ROLL_ADR 44
|
||||
#define KI_GPS_ROLL_ADR 48
|
||||
|
|
@ -141,7 +141,7 @@ void readUserConfig() {
|
|||
KP_QUAD_YAW = readEEPROM(KP_QUAD_YAW_ADR);
|
||||
KD_QUAD_YAW = readEEPROM(KD_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);
|
||||
KD_GPS_ROLL = readEEPROM(KD_GPS_ROLL_ADR);
|
||||
KI_GPS_ROLL = readEEPROM(KI_GPS_ROLL_ADR);
|
||||
|
|
|
|||
Loading…
Reference in New Issue