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

starting migration of functions 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@507 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
CaranchoEngineering 2010-09-13 09:07:31 +00:00
parent 1f7f814183
commit 813b67cb5e
2 changed files with 256 additions and 104 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

207
ArducopterNG/ArducopterNG.pde
View File

@ -6,7 +6,7 @@
File : Arducopter.pde
Version : v1.0, Aug 27, 2010
Author(s): ArduCopter Team
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
Ted Carancho (AeroQuad), Jose Julio, Jordi Muñoz,
Jani Hirvinen, Ken McEwans, Roberto Navoni,
Sandro Benigno, Chris Anderson
@ -145,7 +145,159 @@ void setup() {
//APM_Init_Serial();
//APM_Init_xx
// Just add this in now and edit later
int i, j;
float aux_float[3];
pinMode(LED_Yellow,OUTPUT); //Yellow LED A (PC1)
pinMode(LED_Red,OUTPUT); //Red LED B (PC2)
pinMode(LED_Green,OUTPUT); //Green LED C (PC0)
pinMode(SW1_pin,INPUT); //Switch SW1 (pin PG0)
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++) {
digitalWrite(LED_Green, HIGH);
digitalWrite(LED_Yellow, HIGH);
digitalWrite(LED_Red, HIGH);
delay(20);
digitalWrite(LED_Green, LOW);
digitalWrite(LED_Yellow, LOW);
digitalWrite(LED_Red, LOW);
delay(20);
}
APM_ADC.Init(); // APM ADC library initialization
DataFlash.Init(); // DataFlash log initialization
#ifdef IsGPS
GPS.Init(); // GPS Initialization
#ifdef IsNEWMTEK
delay(250);
// DIY Drones MTEK GPS needs binary sentences activated if you upgraded to latest firmware.
// If your GPS shows solid blue but LED C (Red) does not go on, your GPS is on NMEA mode
Serial1.print("$PGCMD,16,0,0,0,0,0*6A\r\n");
#endif
#endif
readUserConfig(); // Load user configurable items from EEPROM
// Safety measure for Channel mids
if(roll_mid < 1400 || roll_mid > 1600) roll_mid = 1500;
if(pitch_mid < 1400 || pitch_mid > 1600) pitch_mid = 1500;
if(yaw_mid < 1400 || yaw_mid > 1600) yaw_mid = 1500;
if (MAGNETOMETER == 1)
APM_Compass.Init(); // I2C initialization
DataFlash.StartWrite(1); // Start a write session on page 1
SerBeg(SerBau); // Initialize SerialXX.port, IsXBEE define declares which port
#ifndef CONFIGURATOR
SerPri("ArduCopter Quadcopter v");
SerPriln(VER)
SerPri("Serial ready on port: "); // Printout greeting to selecter serial port
SerPriln(SerPor); // Printout serial port name
#endif
// Check if we enable the DataFlash log Read Mode (switch)
// If we press switch 1 at startup we read the Dataflash eeprom
while (digitalRead(SW1_pin)==0)
{
SerPriln("Entering Log Read Mode...");
Log_Read(1,2000);
delay(30000);
}
Read_adc_raw();
delay(10);
// Offset values for accels and gyros...
AN_OFFSET[3] = acc_offset_x;
AN_OFFSET[4] = acc_offset_y;
AN_OFFSET[5] = acc_offset_z;
aux_float[0] = gyro_offset_roll;
aux_float[1] = gyro_offset_pitch;
aux_float[2] = gyro_offset_yaw;
j = 0;
// Take the gyro offset values
for(i=0;i<300;i++)
{
Read_adc_raw();
for(int y=0; y<=2; y++) // Read initial ADC values for gyro offset.
{
aux_float[y]=aux_float[y]*0.8 + AN[y]*0.2;
//SerPri(AN[y]);
//SerPri(",");
}
//SerPriln();
Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],ch_throttle);
delay(10);
// Runnings lights effect to let user know that we are taking mesurements
if(j == 0) {
digitalWrite(LED_Green, HIGH);
digitalWrite(LED_Yellow, LOW);
digitalWrite(LED_Red, LOW);
}
else if (j == 1) {
digitalWrite(LED_Green, LOW);
digitalWrite(LED_Yellow, HIGH);
digitalWrite(LED_Red, LOW);
}
else {
digitalWrite(LED_Green, LOW);
digitalWrite(LED_Yellow, LOW);
digitalWrite(LED_Red, HIGH);
}
if((i % 5) == 0) j++;
if(j >= 3) j = 0;
}
digitalWrite(LED_Green, LOW);
digitalWrite(LED_Yellow, LOW);
digitalWrite(LED_Red, LOW);
for(int y=0; y<=2; y++)
AN_OFFSET[y]=aux_float[y];
// Neutro_yaw = APM_RC.InputCh(3); // Take yaw neutral radio value
#ifndef CONFIGURATOR
for(i=0;i<6;i++)
{
SerPri("AN[]:");
SerPriln(AN_OFFSET[i]);
}
SerPri("Yaw neutral value:");
// SerPriln(Neutro_yaw);
SerPri(yaw_mid);
#endif
delay(1000);
DataFlash.StartWrite(1); // Start a write session on page 1
timer = millis();
tlmTimer = millis();
Read_adc_raw(); // Initialize ADC readings...
delay(20);
#ifdef IsAM
// Switch Left & Right lights on
digitalWrite(RI_LED, HIGH);
digitalWrite(LE_LED, HIGH);
#endif
motorArmed = 0;
digitalWrite(LED_Green,HIGH); // Ready to go...
}
@ -155,27 +307,38 @@ void setup() {
/* ************** MAIN PROGRAM - MAIN LOOP ******************** */
/* ************************************************************ */
void loop() {
// We want this to execute at 500Hz if possible
// -------------------------------------------
if (millis()-fast_loopTimer > 5) {
deltaMiliSeconds = millis() - fast_loopTimer;
G_Dt = (float)deltaMiliSeconds / 1000.f;
fast_loopTimer = millis();
mainLoop_count++;
// Execute the fast loop
// ---------------------
// fast_loop();
// - PWM Updates
// - Stabilization
// - Altitude correction
// We want this to execute at 500Hz if possible
// -------------------------------------------
if (millis()-fast_loopTimer > 5) {
deltaMiliSeconds = millis() - fast_loopTimer;
G_Dt = (float)deltaMiliSeconds / 1000.0f;
fast_loopTimer = millis();
mainLoop_count++;
// Execute the fast loop
// ---------------------
// fast_loop();
// - PWM Updates
// - Stabilization
// - Altitude correction
if(FL_mode == 0) { // Changed for variable
gled_speed = 1200;
Attitude_control_v3();
}
else {
gled_speed = 400;
Rate_control_v2();
// Reset yaw, so if we change to stable mode we continue with the actual yaw direction
command_rx_yaw = ToDeg(yaw);
}
// Execute the medium loop
// -----------------------
// medium_loop();
// - Radio read
// - GPS read
// - Drift correction
// Execute the medium loop
// -----------------------
// medium_loop();
// - Radio read
// - GPS read
// - Drift correction
// Execute the slow loop
// -----------------------
@ -196,4 +359,4 @@ void loop() {
}
}
}

153
ArducopterNG/Attitude.pde
View File

@ -36,7 +36,7 @@ TODO:
/* ************************************************************ */
//////////////////////////////////////////////////
// Function : Attitude_control_v2()
// Function : Attitude_control_v3()
//
// Stable flight mode main algoritms
//
@ -52,77 +52,70 @@ TODO:
// Radio input, Gyro
//
/* ************************************************************ */
// 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
//////////////////////////////////////////////////
// Function : Rate_control()
@ -143,48 +136,44 @@ void Attitude_control_v2()
// ACRO MODE
void Rate_control()
void Rate_control_v2()
{
static float previousRollRate, previousPitchRate, previousYawRate;
float currentRollRate, currentPitchRate, currentYawRate;
// ROLL CONTROL
currentRollRate = read_adc(0); // I need a positive sign here
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;
previousRollRate = currentRollRate;
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;
control_roll = Kp_RateRoll*err_roll + Kd_RateRoll*roll_D + Ki_RateRoll*roll_I;
// PITCH CONTROL
currentPitchRate = read_adc(1);
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;
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 = read_adc(2);
err_yaw = ((ch_yaw - yaw_mid) * xmitFactor) - currentYawRate;
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_I = constrain(yaw_I,-20,20);
yaw_D = currentYawRate - previousYawRate;
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;
@ -200,14 +189,14 @@ 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);
}
}