diff --git a/ArducopterNG/ArducopterNG.pde b/ArducopterNG/ArducopterNG.pde index 60b64ab60a..61b6d800ac 100644 --- a/ArducopterNG/ArducopterNG.pde +++ b/ArducopterNG/ArducopterNG.pde @@ -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() { } } } - + diff --git a/ArducopterNG/Attitude.pde b/ArducopterNG/Attitude.pde index ba437f479f..5e2712fb52 100644 --- a/ArducopterNG/Attitude.pde +++ b/ArducopterNG/Attitude.pde @@ -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); -} +}