mirror of https://github.com/ArduPilot/ardupilot
New Stable mode implementation. Fixed yaw issue. GPS position hold update. Acrobatic mode update to use bias corrected gyro rates.
git-svn-id: https://arducopter.googlecode.com/svn/trunk@376 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jjulio1234
parent
e0be94a03f
commit
d6fc9650a4
|
|
@ -51,7 +51,7 @@ uint8_t sensors[6] = {1, 2, 0, 4, 5, 6}; // For ArduPilot Mega Sensor Shield Ha
|
|||
|
||||
// Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ, MAGX, MAGY, MAGZ
|
||||
int SENSOR_SIGN[]={
|
||||
1, -1, 1, -1, 1, 1, -1, -1, -1};
|
||||
1, -1, -1, -1, 1, 1, -1, -1, -1};
|
||||
//{-1,1,-1,1,-1,1,-1,-1,-1};
|
||||
/* APM Hardware definitions, END */
|
||||
|
||||
|
|
@ -139,13 +139,8 @@ float GPS_Dt=0.2; // GPS Dt
|
|||
|
||||
// Attitude control variables
|
||||
float command_rx_roll=0; // User commands
|
||||
float command_rx_roll_old;
|
||||
float command_rx_roll_diff;
|
||||
float command_rx_pitch=0;
|
||||
float command_rx_pitch_old;
|
||||
float command_rx_pitch_diff;
|
||||
float command_rx_yaw=0;
|
||||
float command_rx_yaw_diff;
|
||||
int control_roll; // PID control results
|
||||
int control_pitch;
|
||||
int control_yaw;
|
||||
|
|
|
|||
|
|
@ -49,7 +49,7 @@
|
|||
/* User definable modules */
|
||||
|
||||
// Comment out with // modules that you are not using
|
||||
//#define IsGPS // Do we have a GPS connected
|
||||
#define IsGPS // Do we have a GPS connected
|
||||
//#define IsNEWMTEK// Do we have MTEK with new firmware
|
||||
#define IsMAG // Do we have a Magnetometer connected, if have remember to activate it from Configurator
|
||||
//#define IsTEL // Do we have a telemetry connected, eg. XBee connected on Telemetry port
|
||||
|
|
@ -87,8 +87,8 @@
|
|||
#include <APM_BMP085.h>
|
||||
#endif
|
||||
|
||||
//#include <GPS_NMEA.h> // General NMEA GPS
|
||||
#include <GPS_MTK.h> // MediaTEK DIY Drones GPS.
|
||||
#include <GPS_NMEA.h> // General NMEA GPS
|
||||
//#include <GPS_MTK.h> // MediaTEK DIY Drones GPS.
|
||||
//#include <GPS_UBLOX.h> // uBlox GPS
|
||||
|
||||
// EEPROM storage for user configurable values
|
||||
|
|
@ -109,74 +109,64 @@
|
|||
|
||||
/* ************************************************************ */
|
||||
// STABLE MODE
|
||||
// ROLL, PITCH and YAW PID controls...
|
||||
// PI absolute angle control driving a P rate control
|
||||
// Input : desired Roll, Pitch and Yaw absolute angles. Output : Motor commands
|
||||
void Attitude_control_v2()
|
||||
void Attitude_control_v3()
|
||||
{
|
||||
float err_roll_rate;
|
||||
float err_pitch_rate;
|
||||
float roll_rate;
|
||||
float pitch_rate;
|
||||
|
||||
float stable_roll,stable_pitch,stable_yaw;
|
||||
|
||||
// ROLL CONTROL
|
||||
if (AP_mode == 2) // Normal Mode => Stabilization mode
|
||||
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 = (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);
|
||||
|
||||
err_roll = constrain(err_roll, -25, 25); // to limit max roll command...
|
||||
|
||||
// New control term...
|
||||
roll_rate = ToDeg(Omega[0]); // Omega[] is the raw gyro reading plus Omega_I, so it´s bias corrected
|
||||
err_roll_rate = ((ch_roll - roll_mid) >> 1) - roll_rate;
|
||||
|
||||
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...
|
||||
// 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 = - roll_rate; // Take into account Angular velocity of the stick (command)
|
||||
|
||||
// PID control
|
||||
// PID absolute angle 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 * err_roll_rate;
|
||||
|
||||
stable_roll = K_aux*err_roll + KI_QUAD_ROLL*roll_I;
|
||||
|
||||
// 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
|
||||
control_roll = STABLE_MODE_KP_RATE_ROLL*err_roll;
|
||||
|
||||
// PITCH CONTROL
|
||||
if (AP_mode==2) // Normal mode => Stabilization mode
|
||||
err_pitch = command_rx_pitch - ToDeg(pitch);
|
||||
else
|
||||
else // GPS Position hold
|
||||
err_pitch = (command_rx_pitch + command_gps_pitch) - ToDeg(pitch); // Position Control
|
||||
|
||||
err_pitch = constrain(err_pitch, -25, 25); // to limit max pitch command...
|
||||
|
||||
// New control term...
|
||||
pitch_rate = ToDeg(Omega[1]);
|
||||
err_pitch_rate = ((ch_pitch - pitch_mid) >> 1) - pitch_rate;
|
||||
|
||||
pitch_I += err_pitch * G_Dt;
|
||||
pitch_I = constrain(pitch_I, -20, 20);
|
||||
// D term
|
||||
pitch_D = - pitch_rate;
|
||||
|
||||
// PID 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);
|
||||
|
||||
// PID absolute angle 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 * err_pitch_rate;
|
||||
|
||||
stable_pitch = K_aux*err_pitch + KI_QUAD_PITCH*pitch_I;
|
||||
|
||||
// P rate control (we use also the bias corrected gyro rates)
|
||||
err_pitch = stable_pitch - ToDeg(Omega[1]);
|
||||
control_pitch = STABLE_MODE_KP_RATE_PITCH*err_pitch;
|
||||
|
||||
// YAW CONTROL
|
||||
err_yaw = command_rx_yaw - ToDeg(yaw);
|
||||
if (err_yaw > 180) // Normalize to -180,180
|
||||
err_yaw -= 360;
|
||||
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 = - ToDeg(Omega[2]);
|
||||
|
||||
// PID control
|
||||
control_yaw = KP_QUAD_YAW * err_yaw + KD_QUAD_YAW * yaw_D + KI_QUAD_YAW * yaw_I;
|
||||
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);
|
||||
|
||||
// PID absoulte angle control
|
||||
stable_yaw = KP_QUAD_YAW*err_yaw + KI_QUAD_YAW*yaw_I;
|
||||
// PD rate control (we use also the bias corrected gyro rates)
|
||||
err_yaw = stable_yaw - ToDeg(Omega[2]);
|
||||
control_yaw = STABLE_MODE_KP_RATE_YAW*err_yaw;
|
||||
}
|
||||
|
||||
// ACRO MODE
|
||||
|
|
@ -227,6 +217,55 @@ void Rate_control()
|
|||
control_yaw = Kp_RateYaw*err_yaw + Kd_RateYaw*yaw_D + Ki_RateYaw*yaw_I;
|
||||
}
|
||||
|
||||
// RATE CONTROL MODE
|
||||
// Using Omega vector (bias corrected gyro rate)
|
||||
void Rate_control_v2()
|
||||
{
|
||||
static float previousRollRate, previousPitchRate, previousYawRate;
|
||||
float currentRollRate, currentPitchRate, currentYawRate;
|
||||
|
||||
// ROLL CONTROL
|
||||
currentRollRate = ToDeg(Omega[0]); // Omega[] is the raw gyro reading plus Omega_I, so it´s bias corrected
|
||||
|
||||
err_roll = ((ch_roll- roll_mid) * xmitFactor) - currentRollRate;
|
||||
|
||||
roll_I += err_roll*G_Dt;
|
||||
roll_I = constrain(roll_I,-20,20);
|
||||
|
||||
roll_D = (currentRollRate - previousRollRate)/G_Dt;
|
||||
previousRollRate = currentRollRate;
|
||||
|
||||
// PID control
|
||||
control_roll = Kp_RateRoll*err_roll + Kd_RateRoll*roll_D + Ki_RateRoll*roll_I;
|
||||
|
||||
// PITCH CONTROL
|
||||
currentPitchRate = ToDeg(Omega[1]); // Omega[] is the raw gyro reading plus Omega_I, so it´s bias corrected
|
||||
err_pitch = ((ch_pitch - pitch_mid) * xmitFactor) - currentPitchRate;
|
||||
|
||||
pitch_I += err_pitch*G_Dt;
|
||||
pitch_I = constrain(pitch_I,-20,20);
|
||||
|
||||
pitch_D = (currentPitchRate - previousPitchRate)/G_Dt;
|
||||
previousPitchRate = currentPitchRate;
|
||||
|
||||
// PID control
|
||||
control_pitch = Kp_RatePitch*err_pitch + Kd_RatePitch*pitch_D + Ki_RatePitch*pitch_I;
|
||||
|
||||
// YAW CONTROL
|
||||
currentYawRate = ToDeg(Omega[2]); // Omega[] is the raw gyro reading plus Omega_I, so it´s bias corrected;
|
||||
err_yaw = ((ch_yaw - yaw_mid)* xmitFactor) - currentYawRate;
|
||||
|
||||
yaw_I += err_yaw*G_Dt;
|
||||
yaw_I = constrain(yaw_I,-20,20);
|
||||
|
||||
yaw_D = (currentYawRate - previousYawRate)/G_Dt;
|
||||
previousYawRate = currentYawRate;
|
||||
|
||||
// PID control
|
||||
K_aux = KP_QUAD_YAW; // Comment this out if you want to use transmitter to adjust gain
|
||||
control_yaw = Kp_RateYaw*err_yaw + Kd_RateYaw*yaw_D + Ki_RateYaw*yaw_I;
|
||||
}
|
||||
|
||||
// Maximun slope filter for radio inputs... (limit max differences between readings)
|
||||
int channel_filter(int ch, int ch_old)
|
||||
{
|
||||
|
|
@ -237,13 +276,13 @@ int channel_filter(int ch, int ch_old)
|
|||
diff_ch_old = ch - ch_old; // Difference with old reading
|
||||
if (diff_ch_old < 0)
|
||||
{
|
||||
if (diff_ch_old <- 40)
|
||||
return(ch_old - 40); // We limit the max difference between readings
|
||||
if (diff_ch_old <- 60)
|
||||
return(ch_old - 60); // We limit the max difference between readings
|
||||
}
|
||||
else
|
||||
{
|
||||
if (diff_ch_old > 40)
|
||||
return(ch_old + 40);
|
||||
if (diff_ch_old > 60)
|
||||
return(ch_old + 60);
|
||||
}
|
||||
return((ch + ch_old) >> 1); // Small filtering
|
||||
//return(ch);
|
||||
|
|
@ -265,6 +304,13 @@ void setup()
|
|||
|
||||
pinMode(RELE_pin,OUTPUT); // Rele output
|
||||
digitalWrite(RELE_pin,LOW);
|
||||
|
||||
APM_RC.Init(); // APM Radio initialization
|
||||
// RC channels Initialization (Quad motors)
|
||||
APM_RC.OutputCh(0,MIN_THROTTLE); // Motors stoped
|
||||
APM_RC.OutputCh(1,MIN_THROTTLE);
|
||||
APM_RC.OutputCh(2,MIN_THROTTLE);
|
||||
APM_RC.OutputCh(3,MIN_THROTTLE);
|
||||
|
||||
// delay(1000); // Wait until frame is not moving after initial power cord has connected
|
||||
for(i = 0; i <= 50; i++) {
|
||||
|
|
@ -278,7 +324,6 @@ void setup()
|
|||
delay(20);
|
||||
}
|
||||
|
||||
APM_RC.Init(); // APM Radio initialization
|
||||
APM_ADC.Init(); // APM ADC library initialization
|
||||
DataFlash.Init(); // DataFlash log initialization
|
||||
|
||||
|
|
@ -299,12 +344,6 @@ void setup()
|
|||
if(pitch_mid < 1400 || pitch_mid > 1600) pitch_mid = 1500;
|
||||
if(yaw_mid < 1400 || yaw_mid > 1600) yaw_mid = 1500;
|
||||
|
||||
// RC channels Initialization (Quad motors)
|
||||
APM_RC.OutputCh(0,MIN_THROTTLE); // Motors stoped
|
||||
APM_RC.OutputCh(1,MIN_THROTTLE);
|
||||
APM_RC.OutputCh(2,MIN_THROTTLE);
|
||||
APM_RC.OutputCh(3,MIN_THROTTLE);
|
||||
|
||||
if (MAGNETOMETER == 1)
|
||||
APM_Compass.Init(); // I2C initialization
|
||||
|
||||
|
|
@ -320,12 +359,12 @@ void setup()
|
|||
while (digitalRead(SW1_pin)==0)
|
||||
{
|
||||
Serial.println("Entering Log Read Mode...");
|
||||
Log_Read(1,1000);
|
||||
Log_Read(1,2000);
|
||||
delay(30000);
|
||||
}
|
||||
|
||||
Read_adc_raw();
|
||||
delay(20);
|
||||
delay(10);
|
||||
|
||||
// Offset values for accels and gyros...
|
||||
AN_OFFSET[3] = acc_offset_x;
|
||||
|
|
@ -387,30 +426,6 @@ void setup()
|
|||
// Serial.println(Neutro_yaw);
|
||||
Serial.print(yaw_mid);
|
||||
#endif
|
||||
|
||||
#ifdef RADIO_TEST_MODE // RADIO TEST MODE TO TEST RADIO CHANNELS
|
||||
while(1)
|
||||
{
|
||||
if (APM_RC.GetState()==1)
|
||||
{
|
||||
Serial.print("AIL:");
|
||||
Serial.print(APM_RC.InputCh(0));
|
||||
Serial.print("ELE:");
|
||||
Serial.print(APM_RC.InputCh(1));
|
||||
Serial.print("THR:");
|
||||
Serial.print(APM_RC.InputCh(2));
|
||||
Serial.print("YAW:");
|
||||
Serial.print(APM_RC.InputCh(3));
|
||||
Serial.print("AUX(mode):");
|
||||
Serial.print(APM_RC.InputCh(4));
|
||||
Serial.print("AUX2:");
|
||||
Serial.print(APM_RC.InputCh(5));
|
||||
Serial.println();
|
||||
delay(200);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
delay(1000);
|
||||
|
||||
DataFlash.StartWrite(1); // Start a write session on page 1
|
||||
|
|
@ -427,6 +442,7 @@ void setup()
|
|||
|
||||
motorArmed = 0;
|
||||
digitalWrite(LED_Green,HIGH); // Ready to go...
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -482,11 +498,11 @@ void loop(){
|
|||
Serial.print(",");
|
||||
Serial.print(log_yaw);
|
||||
|
||||
for (int i = 0; i < 6; i++)
|
||||
{
|
||||
Serial.print(AN[i]);
|
||||
Serial.print(",");
|
||||
}
|
||||
//for (int i = 0; i < 6; i++)
|
||||
//{
|
||||
// Serial.print(AN[i]);
|
||||
// Serial.print(",");
|
||||
//}
|
||||
#endif
|
||||
|
||||
// Write Sensor raw data to DataFlash log
|
||||
|
|
@ -500,33 +516,30 @@ void loop(){
|
|||
// Stick position defines the desired angle in roll, pitch and yaw
|
||||
ch_roll = channel_filter(APM_RC.InputCh(0) * ch_roll_slope + ch_roll_offset, ch_roll);
|
||||
ch_pitch = channel_filter(APM_RC.InputCh(1) * ch_pitch_slope + ch_pitch_offset, ch_pitch);
|
||||
ch_throttle = channel_filter(APM_RC.InputCh(2) * ch_throttle_slope + ch_throttle_offset, ch_throttle);
|
||||
//ch_throttle = channel_filter(APM_RC.InputCh(2) * ch_throttle_slope + ch_throttle_offset, ch_throttle);
|
||||
ch_throttle = channel_filter(APM_RC.InputCh(2), ch_throttle); // Transmiter calibration not used on throttle
|
||||
ch_yaw = channel_filter(APM_RC.InputCh(3) * ch_yaw_slope + ch_yaw_offset, ch_yaw);
|
||||
ch_aux = APM_RC.InputCh(4) * ch_aux_slope + ch_aux_offset;
|
||||
ch_aux2 = APM_RC.InputCh(5) * ch_aux2_slope + ch_aux2_offset;
|
||||
command_rx_roll_old = command_rx_roll;
|
||||
command_rx_roll = (ch_roll-CHANN_CENTER) / 12.0;
|
||||
command_rx_roll_diff = command_rx_roll - command_rx_roll_old;
|
||||
command_rx_pitch_old = command_rx_pitch;
|
||||
command_rx_pitch = (ch_pitch-CHANN_CENTER) / 12.0;
|
||||
command_rx_pitch_diff = command_rx_pitch - command_rx_pitch_old;
|
||||
// aux_float = (ch_yaw-Neutro_yaw) / 180.0;
|
||||
aux_float = (ch_yaw-yaw_mid) / 180.0;
|
||||
|
||||
command_rx_roll = (ch_roll-roll_mid) / 12.0;
|
||||
command_rx_pitch = (ch_pitch-pitch_mid) / 12.0;
|
||||
//aux_float = (ch_yaw-Neutro_yaw) / 180.0;
|
||||
if (abs(ch_yaw-yaw_mid)<12) // Take into account a bit of "dead zone" on yaw
|
||||
aux_float = 0.0;
|
||||
else
|
||||
aux_float = (ch_yaw-yaw_mid) / 180.0;
|
||||
command_rx_yaw += aux_float;
|
||||
command_rx_yaw_diff = aux_float;
|
||||
if (command_rx_yaw > 180) // Normalize yaw to -180,180 degrees
|
||||
command_rx_yaw -= 360.0;
|
||||
else if (command_rx_yaw < -180)
|
||||
command_rx_yaw += 360.0;
|
||||
|
||||
|
||||
// Read through comments in Attitude_control() if you wish to use transmitter to adjust P gains
|
||||
// I use K_aux (channel 6) to adjust gains linked to a knob in the radio... [not used now]
|
||||
//K_aux = K_aux*0.8 + ((ch_aux-1500)/100.0 + 0.6)*0.2;
|
||||
K_aux = K_aux * 0.8 + ((ch_aux2-AUX_MID) / 300.0 + 1.7) * 0.2; // /300 + 1.0
|
||||
|
||||
if (K_aux < 0)
|
||||
K_aux = 0;
|
||||
|
||||
if (K_aux < 0) K_aux = 0;
|
||||
//Serial.print(",");
|
||||
//Serial.print(K_aux);
|
||||
|
||||
|
|
@ -590,7 +603,8 @@ void loop(){
|
|||
// Write GPS data to DataFlash log
|
||||
Log_Write_GPS(GPS.Time, GPS.Lattitude,GPS.Longitude,GPS.Altitude, GPS.Ground_Speed, GPS.Ground_Course, GPS.Fix, GPS.NumSats);
|
||||
|
||||
if (GPS.Fix >= 2)
|
||||
//if (GPS.Fix >= 2)
|
||||
if (GPS.Fix)
|
||||
digitalWrite(LED_Red,HIGH); // GPS Fix => Blue LED
|
||||
else
|
||||
digitalWrite(LED_Red,LOW);
|
||||
|
|
@ -613,27 +627,25 @@ void loop(){
|
|||
// Control methodology selected using AUX2
|
||||
if (ch_aux2 < 1200) {
|
||||
gled_speed = 1200;
|
||||
Attitude_control_v2();
|
||||
Attitude_control_v3();
|
||||
}
|
||||
else
|
||||
{
|
||||
gled_speed = 400;
|
||||
Rate_control();
|
||||
Rate_control_v2();
|
||||
// Reset yaw, so if we change to stable mode we continue with the actual yaw direction
|
||||
command_rx_yaw = ToDeg(yaw);
|
||||
command_rx_yaw_diff = 0;
|
||||
}
|
||||
|
||||
// Arm motor output : Throttle down and full yaw right for more than 2 seconds
|
||||
if (ch_throttle < 1200) {
|
||||
if (ch_throttle < (MIN_THROTTLE + 100)) {
|
||||
control_yaw = 0;
|
||||
command_rx_yaw = ToDeg(yaw);
|
||||
command_rx_yaw_diff = 0;
|
||||
if (ch_yaw < 1200) {
|
||||
if (ch_yaw > 1850) {
|
||||
if (Arming_counter > ARM_DELAY){
|
||||
if(ch_throttle > 800) {
|
||||
motorArmed = 1;
|
||||
minThrottle = 1100;
|
||||
minThrottle = MIN_THROTTLE+60; // A minimun value for mantain a bit if throttle
|
||||
}
|
||||
}
|
||||
else
|
||||
|
|
@ -642,7 +654,7 @@ void loop(){
|
|||
else
|
||||
Arming_counter=0;
|
||||
// To Disarm motor output : Throttle down and full yaw left for more than 2 seconds
|
||||
if (ch_yaw > 1800) {
|
||||
if (ch_yaw < 1150) {
|
||||
if (Disarming_counter > DISARM_DELAY){
|
||||
motorArmed = 0;
|
||||
minThrottle = MIN_THROTTLE;
|
||||
|
|
@ -664,16 +676,16 @@ void loop(){
|
|||
digitalWrite(FR_LED, HIGH); // AM-Mode
|
||||
#endif
|
||||
#ifdef FLIGHT_MODE_+
|
||||
rightMotor = constrain(ch_throttle - control_roll - control_yaw, minThrottle, 2000);
|
||||
leftMotor = constrain(ch_throttle + control_roll - control_yaw, minThrottle, 2000);
|
||||
frontMotor = constrain(ch_throttle + control_pitch + control_yaw, minThrottle, 2000);
|
||||
backMotor = constrain(ch_throttle - control_pitch + control_yaw, minThrottle, 2000);
|
||||
rightMotor = constrain(ch_throttle - control_roll + control_yaw, minThrottle, 2000);
|
||||
leftMotor = constrain(ch_throttle + control_roll + control_yaw, minThrottle, 2000);
|
||||
frontMotor = constrain(ch_throttle + control_pitch - control_yaw, minThrottle, 2000);
|
||||
backMotor = constrain(ch_throttle - control_pitch - control_yaw, minThrottle, 2000);
|
||||
#endif
|
||||
#ifdef FLIGHT_MODE_X
|
||||
rightMotor = constrain(ch_throttle - control_roll + control_pitch - control_yaw, minThrottle, 2000); // front right motor
|
||||
leftMotor = constrain(ch_throttle + control_roll - control_pitch - control_yaw, minThrottle, 2000); // rear left motor
|
||||
frontMotor = constrain(ch_throttle + control_roll + control_pitch + control_yaw, minThrottle, 2000); // front left motor
|
||||
backMotor = constrain(ch_throttle - control_roll - control_pitch + control_yaw, minThrottle, 2000); // rear right motor
|
||||
rightMotor = constrain(ch_throttle - control_roll + control_pitch + control_yaw, minThrottle, 2000); // front right motor
|
||||
leftMotor = constrain(ch_throttle + control_roll - control_pitch + control_yaw, minThrottle, 2000); // rear left motor
|
||||
frontMotor = constrain(ch_throttle + control_roll + control_pitch - control_yaw, minThrottle, 2000); // front left motor
|
||||
backMotor = constrain(ch_throttle - control_roll - control_pitch - control_yaw, minThrottle, 2000); // rear right motor
|
||||
#endif
|
||||
}
|
||||
if (motorArmed == 0) {
|
||||
|
|
@ -691,7 +703,6 @@ void loop(){
|
|||
yaw_I = 0;
|
||||
// Initialize yaw command to actual yaw when throttle is down...
|
||||
command_rx_yaw = ToDeg(yaw);
|
||||
command_rx_yaw_diff = 0;
|
||||
}
|
||||
APM_RC.OutputCh(0, rightMotor); // Right motor
|
||||
APM_RC.OutputCh(1, leftMotor); // Left motor
|
||||
|
|
@ -735,8 +746,7 @@ void loop(){
|
|||
|
||||
} // End of void loop()
|
||||
|
||||
|
||||
// END of Arducopter.pde
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -9,6 +9,7 @@
|
|||
#define LOG_GPS_MSG 0x02
|
||||
#define LOG_RADIO_MSG 0x03
|
||||
#define LOG_SENSOR_MSG 0x04
|
||||
#define LOG_PID_MSG 0x05
|
||||
|
||||
// Write a Sensor Raw data packet
|
||||
void Log_Write_Sensor(int s1, int s2, int s3,int s4, int s5, int s6, int s7)
|
||||
|
|
@ -38,6 +39,20 @@ void Log_Write_Attitude(int log_roll, int log_pitch, int log_yaw)
|
|||
DataFlash.WriteByte(END_BYTE);
|
||||
}
|
||||
|
||||
// Write a PID control info
|
||||
void Log_Write_PID(byte num_PID, int P, int I,int D, int output)
|
||||
{
|
||||
DataFlash.WriteByte(HEAD_BYTE1);
|
||||
DataFlash.WriteByte(HEAD_BYTE2);
|
||||
DataFlash.WriteByte(LOG_PID_MSG);
|
||||
DataFlash.WriteByte(num_PID);
|
||||
DataFlash.WriteInt(P);
|
||||
DataFlash.WriteInt(I);
|
||||
DataFlash.WriteInt(D);
|
||||
DataFlash.WriteInt(output);
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
}
|
||||
|
||||
// Write an GPS packet. Total length : 30 bytes
|
||||
void Log_Write_GPS(long log_Time, long log_Lattitude, long log_Longitude, long log_Altitude,
|
||||
long log_Ground_Speed, long log_Ground_Course, byte log_Fix, byte log_NumSats)
|
||||
|
|
@ -112,6 +127,22 @@ void Log_Read_Attitude()
|
|||
Serial.println();
|
||||
}
|
||||
|
||||
// Read a Sensor raw data packet
|
||||
void Log_Read_PID()
|
||||
{
|
||||
Serial.print("PID:");
|
||||
Serial.print((int)DataFlash.ReadByte()); // NUM_PID
|
||||
Serial.print(",");
|
||||
Serial.print(DataFlash.ReadInt()); // P
|
||||
Serial.print(",");
|
||||
Serial.print(DataFlash.ReadInt()); // I
|
||||
Serial.print(",");
|
||||
Serial.print(DataFlash.ReadInt()); // D
|
||||
Serial.print(",");
|
||||
Serial.print(DataFlash.ReadInt()); // output
|
||||
Serial.println();
|
||||
}
|
||||
|
||||
// Read a GPS packet
|
||||
void Log_Read_GPS()
|
||||
{
|
||||
|
|
@ -176,7 +207,7 @@ void Log_Read(int start_page, int end_page)
|
|||
{
|
||||
byte data;
|
||||
byte log_step=0;
|
||||
int packet_count=0;
|
||||
long packet_count=0;
|
||||
|
||||
DataFlash.StartRead(start_page);
|
||||
while (DataFlash.GetPage() < end_page)
|
||||
|
|
@ -211,6 +242,10 @@ void Log_Read(int start_page, int end_page)
|
|||
Log_Read_Sensor();
|
||||
log_step++;
|
||||
break;
|
||||
case LOG_PID_MSG:
|
||||
Log_Read_PID();
|
||||
log_step++;
|
||||
break;
|
||||
default:
|
||||
Serial.print("Error Reading Packet: ");
|
||||
Serial.print(packet_count);
|
||||
|
|
|
|||
|
|
@ -24,36 +24,38 @@ void Position_control(long lat_dest, long lon_dest)
|
|||
{
|
||||
long Lon_diff;
|
||||
long Lat_diff;
|
||||
float gps_err_roll;
|
||||
float gps_err_pitch;
|
||||
|
||||
Lon_diff = lon_dest - GPS.Longitude;
|
||||
Lat_diff = lat_dest - GPS.Lattitude;
|
||||
|
||||
// ROLL
|
||||
gps_err_roll_old = gps_err_roll;
|
||||
//Optimization : cos(yaw) = DCM_Matrix[0][0] ; sin(yaw) = DCM_Matrix[1][0]
|
||||
gps_err_roll = (float)Lon_diff * GEOG_CORRECTION_FACTOR * DCM_Matrix[0][0] - (float)Lat_diff * DCM_Matrix[1][0];
|
||||
|
||||
gps_roll_D = (gps_err_roll-gps_err_roll_old) / GPS_Dt;
|
||||
|
||||
gps_err_roll_old = gps_err_roll;
|
||||
|
||||
gps_roll_I += gps_err_roll * GPS_Dt;
|
||||
gps_roll_I = constrain(gps_roll_I, -800, 800);
|
||||
|
||||
command_gps_roll = KP_GPS_ROLL * gps_err_roll + KD_GPS_ROLL * gps_roll_D + KI_GPS_ROLL * gps_roll_I;
|
||||
command_gps_roll = constrain(command_gps_roll, -GPS_MAX_ANGLE, GPS_MAX_ANGLE); // Limit max command
|
||||
|
||||
//Log_Write_PID(1,KP_GPS_ROLL*gps_err_roll*10,KI_GPS_ROLL*gps_roll_I*10,KD_GPS_ROLL*gps_roll_D*10,command_gps_roll*10);
|
||||
|
||||
// PITCH
|
||||
gps_err_pitch_old = gps_err_pitch;
|
||||
gps_err_pitch = -(float)Lat_diff * DCM_Matrix[0][0] - (float)Lon_diff * GEOG_CORRECTION_FACTOR * DCM_Matrix[1][0];
|
||||
|
||||
gps_pitch_D = (gps_err_pitch - gps_err_pitch_old) / GPS_Dt;
|
||||
gps_err_pitch_old = gps_err_pitch;
|
||||
|
||||
gps_pitch_I += gps_err_pitch * GPS_Dt;
|
||||
gps_pitch_I = constrain(gps_pitch_I, -800, 800);
|
||||
|
||||
command_gps_pitch = KP_GPS_PITCH * gps_err_pitch + KD_GPS_PITCH * gps_pitch_D + KI_GPS_PITCH * gps_pitch_I;
|
||||
command_gps_pitch = constrain(command_gps_pitch, -GPS_MAX_ANGLE, GPS_MAX_ANGLE); // Limit max command
|
||||
|
||||
//Log_Write_PID(2,KP_GPS_PITCH*gps_err_pitch*10,KI_GPS_PITCH*gps_pitch_I*10,KD_GPS_PITCH*gps_pitch_D*10,command_gps_pitch*10);
|
||||
}
|
||||
|
||||
/* ************************************************************ */
|
||||
|
|
@ -68,4 +70,4 @@ void Altitude_control(int target_sonar_altitude)
|
|||
command_altitude = Initial_Throttle + KP_ALTITUDE * err_altitude + KD_ALTITUDE * altitude_D + KI_ALTITUDE * altitude_I;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -26,14 +26,14 @@ void readSerialCommand() {
|
|||
case 'A': // Stable PID
|
||||
KP_QUAD_ROLL = readFloatSerial();
|
||||
KI_QUAD_ROLL = readFloatSerial();
|
||||
KD_QUAD_ROLL = readFloatSerial();
|
||||
STABLE_MODE_KP_RATE_ROLL = readFloatSerial();
|
||||
KP_QUAD_PITCH = readFloatSerial();
|
||||
KI_QUAD_PITCH = readFloatSerial();
|
||||
KD_QUAD_PITCH = readFloatSerial();
|
||||
STABLE_MODE_KP_RATE_PITCH = readFloatSerial();
|
||||
KP_QUAD_YAW = readFloatSerial();
|
||||
KI_QUAD_YAW = readFloatSerial();
|
||||
KD_QUAD_YAW = readFloatSerial();
|
||||
STABLE_MODE_KP_RATE = readFloatSerial();
|
||||
STABLE_MODE_KP_RATE_YAW = readFloatSerial();
|
||||
STABLE_MODE_KP_RATE = readFloatSerial(); // NOT USED NOW
|
||||
MAGNETOMETER = readFloatSerial();
|
||||
break;
|
||||
case 'C': // Receive GPS PID
|
||||
|
|
@ -155,21 +155,21 @@ void sendSerialTelemetry() {
|
|||
comma();
|
||||
Serial.print(KI_QUAD_ROLL, 3);
|
||||
comma();
|
||||
Serial.print(KD_QUAD_ROLL, 3);
|
||||
Serial.print(STABLE_MODE_KP_RATE_ROLL, 3);
|
||||
comma();
|
||||
Serial.print(KP_QUAD_PITCH, 3);
|
||||
comma();
|
||||
Serial.print(KI_QUAD_PITCH, 3);
|
||||
comma();
|
||||
Serial.print(KD_QUAD_PITCH, 3);
|
||||
Serial.print(STABLE_MODE_KP_RATE_PITCH, 3);
|
||||
comma();
|
||||
Serial.print(KP_QUAD_YAW, 3);
|
||||
comma();
|
||||
Serial.print(KI_QUAD_YAW, 3);
|
||||
comma();
|
||||
Serial.print(KD_QUAD_YAW, 3);
|
||||
Serial.print(STABLE_MODE_KP_RATE_YAW, 3);
|
||||
comma();
|
||||
Serial.print(STABLE_MODE_KP_RATE, 3);
|
||||
Serial.print(STABLE_MODE_KP_RATE, 3); // NOT USED NOW
|
||||
comma();
|
||||
Serial.println(MAGNETOMETER, 3);
|
||||
queryType = 'X';
|
||||
|
|
|
|||
|
|
@ -81,14 +81,14 @@ TODO:
|
|||
// Following variables stored in EEPROM
|
||||
float KP_QUAD_ROLL;
|
||||
float KI_QUAD_ROLL;
|
||||
float KD_QUAD_ROLL;
|
||||
float STABLE_MODE_KP_RATE_ROLL;
|
||||
float KP_QUAD_PITCH;
|
||||
float KI_QUAD_PITCH;
|
||||
float KD_QUAD_PITCH;
|
||||
float STABLE_MODE_KP_RATE_PITCH;
|
||||
float KP_QUAD_YAW;
|
||||
float KI_QUAD_YAW;
|
||||
float KD_QUAD_YAW;
|
||||
float STABLE_MODE_KP_RATE;
|
||||
float STABLE_MODE_KP_RATE_YAW;
|
||||
float STABLE_MODE_KP_RATE; // NOT USED NOW
|
||||
float KP_GPS_ROLL;
|
||||
float KI_GPS_ROLL;
|
||||
float KD_GPS_ROLL;
|
||||
|
|
@ -137,23 +137,23 @@ float ch_aux2_offset = 0;
|
|||
// This function call contains the default values that are set to the ArduCopter
|
||||
// when a "Default EEPROM Value" command is sent through serial interface
|
||||
void defaultUserConfig() {
|
||||
KP_QUAD_ROLL = 1.8;
|
||||
KI_QUAD_ROLL = 0.30; //0.4
|
||||
KD_QUAD_ROLL = 0.4; //0.48
|
||||
KP_QUAD_PITCH = 1.8;
|
||||
KI_QUAD_PITCH = 0.30; //0.4
|
||||
KD_QUAD_PITCH = 0.4; //0.48
|
||||
KP_QUAD_YAW = 3.6;
|
||||
KP_QUAD_ROLL = 4.0;
|
||||
KI_QUAD_ROLL = 0.15;
|
||||
STABLE_MODE_KP_RATE_ROLL = 1.2;
|
||||
KP_QUAD_PITCH = 4.0;
|
||||
KI_QUAD_PITCH = 0.15;
|
||||
STABLE_MODE_KP_RATE_PITCH = 1.2;
|
||||
KP_QUAD_YAW = 3.0;
|
||||
KI_QUAD_YAW = 0.15;
|
||||
KD_QUAD_YAW = 1.2;
|
||||
STABLE_MODE_KP_RATE = 0.2; // New param for stable mode
|
||||
KP_GPS_ROLL = 0.02;
|
||||
KI_GPS_ROLL = 0.008;
|
||||
KD_GPS_ROLL = 0.006;
|
||||
KP_GPS_PITCH = 0.02;
|
||||
KI_GPS_PITCH = 0.008;
|
||||
KD_GPS_PITCH = 0.006;
|
||||
GPS_MAX_ANGLE = 18;
|
||||
STABLE_MODE_KP_RATE_YAW = 2.4;
|
||||
STABLE_MODE_KP_RATE = 0.2; // NOT USED NOW
|
||||
KP_GPS_ROLL = 0.015;
|
||||
KI_GPS_ROLL = 0.005;
|
||||
KD_GPS_ROLL = 0.01;
|
||||
KP_GPS_PITCH = 0.015;
|
||||
KI_GPS_PITCH = 0.005;
|
||||
KD_GPS_PITCH = 0.01;
|
||||
GPS_MAX_ANGLE = 22;
|
||||
KP_ALTITUDE = 0.8;
|
||||
KI_ALTITUDE = 0.2;
|
||||
KD_ALTITUDE = 0.2;
|
||||
|
|
@ -169,16 +169,16 @@ void defaultUserConfig() {
|
|||
Ki_YAW = 0.00005;
|
||||
GEOG_CORRECTION_FACTOR = 0.87;
|
||||
MAGNETOMETER = 0;
|
||||
Kp_RateRoll = 0.6;
|
||||
Ki_RateRoll = 0.1;
|
||||
Kd_RateRoll = -0.8;
|
||||
Kp_RatePitch = 0.6;
|
||||
Ki_RatePitch = 0.1;
|
||||
Kd_RatePitch = -0.8;
|
||||
Kp_RateYaw = 1.6;
|
||||
Ki_RateYaw = 0.3;
|
||||
Kd_RateYaw = 0;
|
||||
xmitFactor = 0.8;
|
||||
Kp_RateRoll = 1.95;
|
||||
Ki_RateRoll = 0.0;
|
||||
Kd_RateRoll = 0.0;
|
||||
Kp_RatePitch = 1.95;
|
||||
Ki_RatePitch = 0.0;
|
||||
Kd_RatePitch = 0.0;
|
||||
Kp_RateYaw = 3.2;
|
||||
Ki_RateYaw = 0.0;
|
||||
Kd_RateYaw = 0.0;
|
||||
xmitFactor = 0.32;
|
||||
roll_mid = 1500;
|
||||
pitch_mid = 1500;
|
||||
yaw_mid = 1500;
|
||||
|
|
@ -199,14 +199,14 @@ void defaultUserConfig() {
|
|||
// EEPROM storage addresses
|
||||
#define KP_QUAD_ROLL_ADR 0
|
||||
#define KI_QUAD_ROLL_ADR 8
|
||||
#define KD_QUAD_ROLL_ADR 4
|
||||
#define STABLE_MODE_KP_RATE_ROLL_ADR 4
|
||||
#define KP_QUAD_PITCH_ADR 12
|
||||
#define KI_QUAD_PITCH_ADR 20
|
||||
#define KD_QUAD_PITCH_ADR 16
|
||||
#define STABLE_MODE_KP_RATE_PITCH_ADR 16
|
||||
#define KP_QUAD_YAW_ADR 24
|
||||
#define KI_QUAD_YAW_ADR 32
|
||||
#define KD_QUAD_YAW_ADR 28
|
||||
#define STABLE_MODE_KP_RATE_ADR 36
|
||||
#define STABLE_MODE_KP_RATE_YAW_ADR 28
|
||||
#define STABLE_MODE_KP_RATE_ADR 36 // NOT USED NOW
|
||||
#define KP_GPS_ROLL_ADR 40
|
||||
#define KI_GPS_ROLL_ADR 48
|
||||
#define KD_GPS_ROLL_ADR 44
|
||||
|
|
@ -281,14 +281,14 @@ void writeEEPROM(float value, int address) {
|
|||
void readUserConfig() {
|
||||
KP_QUAD_ROLL = readEEPROM(KP_QUAD_ROLL_ADR);
|
||||
KI_QUAD_ROLL = readEEPROM(KI_QUAD_ROLL_ADR);
|
||||
KD_QUAD_ROLL = readEEPROM(KD_QUAD_ROLL_ADR);
|
||||
STABLE_MODE_KP_RATE_ROLL = readEEPROM(STABLE_MODE_KP_RATE_ROLL_ADR);
|
||||
KP_QUAD_PITCH = readEEPROM(KP_QUAD_PITCH_ADR);
|
||||
KI_QUAD_PITCH = readEEPROM(KI_QUAD_PITCH_ADR);
|
||||
KD_QUAD_PITCH = readEEPROM(KD_QUAD_PITCH_ADR);
|
||||
STABLE_MODE_KP_RATE_PITCH = readEEPROM(STABLE_MODE_KP_RATE_PITCH_ADR);
|
||||
KP_QUAD_YAW = readEEPROM(KP_QUAD_YAW_ADR);
|
||||
KI_QUAD_YAW = readEEPROM(KI_QUAD_YAW_ADR);
|
||||
KD_QUAD_YAW = readEEPROM(KD_QUAD_YAW_ADR);
|
||||
STABLE_MODE_KP_RATE = readEEPROM(STABLE_MODE_KP_RATE_ADR);
|
||||
STABLE_MODE_KP_RATE_YAW = readEEPROM(STABLE_MODE_KP_RATE_YAW_ADR);
|
||||
STABLE_MODE_KP_RATE = readEEPROM(STABLE_MODE_KP_RATE_ADR); // NOT USED NOW
|
||||
KP_GPS_ROLL = readEEPROM(KP_GPS_ROLL_ADR);
|
||||
KI_GPS_ROLL = readEEPROM(KI_GPS_ROLL_ADR);
|
||||
KD_GPS_ROLL = readEEPROM(KD_GPS_ROLL_ADR);
|
||||
|
|
@ -340,15 +340,15 @@ void readUserConfig() {
|
|||
|
||||
void writeUserConfig() {
|
||||
writeEEPROM(KP_QUAD_ROLL, KP_QUAD_ROLL_ADR);
|
||||
writeEEPROM(KD_QUAD_ROLL, KD_QUAD_ROLL_ADR);
|
||||
writeEEPROM(KI_QUAD_ROLL, KI_QUAD_ROLL_ADR);
|
||||
writeEEPROM(STABLE_MODE_KP_RATE_ROLL, STABLE_MODE_KP_RATE_ROLL_ADR);
|
||||
writeEEPROM(KP_QUAD_PITCH, KP_QUAD_PITCH_ADR);
|
||||
writeEEPROM(KD_QUAD_PITCH, KD_QUAD_PITCH_ADR);
|
||||
writeEEPROM(KI_QUAD_PITCH, KI_QUAD_PITCH_ADR);
|
||||
writeEEPROM(STABLE_MODE_KP_RATE_PITCH, STABLE_MODE_KP_RATE_PITCH_ADR);
|
||||
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(STABLE_MODE_KP_RATE, STABLE_MODE_KP_RATE_ADR);
|
||||
writeEEPROM(STABLE_MODE_KP_RATE_YAW, STABLE_MODE_KP_RATE_YAW_ADR);
|
||||
writeEEPROM(STABLE_MODE_KP_RATE, STABLE_MODE_KP_RATE_ADR); // NOT USED NOW
|
||||
writeEEPROM(KP_GPS_ROLL, KP_GPS_ROLL_ADR);
|
||||
writeEEPROM(KD_GPS_ROLL, KD_GPS_ROLL_ADR);
|
||||
writeEEPROM(KI_GPS_ROLL, KI_GPS_ROLL_ADR);
|
||||
|
|
@ -396,4 +396,4 @@ void writeUserConfig() {
|
|||
writeEEPROM(ch_yaw_offset, ch_yaw_offset_ADR);
|
||||
writeEEPROM(ch_aux_offset, ch_aux_offset_ADR);
|
||||
writeEEPROM(ch_aux2_offset, ch_aux2_offset_ADR);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue