From 830fa2b104108e80df9e30942ebeccff1ecdff0d Mon Sep 17 00:00:00 2001 From: jjulio1234 Date: Mon, 21 Jun 2010 20:15:22 +0000 Subject: [PATCH] Adjusted IMU gains and Accelerometers dynamic weighting git-svn-id: https://arducopter.googlecode.com/svn/trunk@19 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopter.pde | 133 +++++++------- DCM.pde | 12 +- libraries/APM_RC/APM_RC.cpp | 167 ++++++++++++++++++ libraries/APM_RC/APM_RC.h | 21 +++ .../APM_RC/examples/APM_radio/APM_radio.pde | 31 ++++ libraries/APM_RC/keywords.txt | 5 + 6 files changed, 297 insertions(+), 72 deletions(-) create mode 100644 libraries/APM_RC/APM_RC.cpp create mode 100644 libraries/APM_RC/APM_RC.h create mode 100644 libraries/APM_RC/examples/APM_radio/APM_radio.pde create mode 100644 libraries/APM_RC/keywords.txt diff --git a/ArduCopter.pde b/ArduCopter.pde index a651418535..497e910c3a 100644 --- a/ArduCopter.pde +++ b/ArduCopter.pde @@ -5,10 +5,11 @@ /* (Original ArduIMU code from Jordi Muñoz and William Premerlani) */ /* Ardupilot core code : from DIYDrones.com development team */ /* Quadcopter code from AeroQuad project and ArduIMU quadcopter project */ -/* Authors : Jose Julio, Ted Carancho, Jordi Muñoz */ -/* Date : 27-05-2010 */ -/* Version : 1.0 beta */ -/* Hardware : ArduPilot Mega + Sensor Shield (codename:FOXTRAP2) */ +/* Authors : Jose Julio, Ted Carancho (aeroquad), Jordi Muñoz, */ +/* Roberto Navoni, ... (Arcucopter team) */ +/* Date : 17-06-2010 */ +/* Version : 1.1 beta */ +/* Hardware : ArduPilot Mega + Sensor Shield (Production versions) */ /* This code use this libraries : */ /* APM_RC_QUAD : Radio library (adapted for quads) */ /* APM_ADC : External ADC library */ @@ -32,7 +33,7 @@ #define LED_Green 37 #define RELE_pin 47 #define SW1_pin 41 -#define SW2_pin 42 +#define SW2_pin 40 /* *** */ /* ***************************************************************************** */ @@ -45,46 +46,46 @@ #define MAGNETOMETER 1 // 0 : No magnetometer, 1: Magnetometer // QuadCopter Attitude control PID GAINS -#define KP_QUAD_ROLL 1.7 // 1.5 //1.75 -#define KD_QUAD_ROLL 0.45 //0.35 // 0.4 //Absolute max:0.85 -#define KI_QUAD_ROLL 0.4 // 0.4 //0.45 -#define KP_QUAD_PITCH 1.7 -#define KD_QUAD_PITCH 0.45 -#define KI_QUAD_PITCH 0.4 -#define KP_QUAD_YAW 3.5 // 3.8 +#define KP_QUAD_ROLL 1.8 // 1.5 //1.75 +#define KD_QUAD_ROLL 0.48 //0.35 // 0.4 //Absolute max:0.85 +#define KI_QUAD_ROLL 0.30 // 0.4 //0.45 +#define KP_QUAD_PITCH 1.8 +#define KD_QUAD_PITCH 0.48 +#define KI_QUAD_PITCH 0.30 //0.4 +#define KP_QUAD_YAW 3.6 // 3.8 #define KD_QUAD_YAW 1.2 // 1.3 #define KI_QUAD_YAW 0.15 // 0.15 -#define KD_QUAD_COMMAND_PART 12.0 // for special KD implementation (in two parts). Higher values makes the quadcopter more responsive to user inputs +#define KD_QUAD_COMMAND_PART 2.0 // for special KD implementation (in two parts). Higher values makes the quadcopter more responsive to user inputs // Position control PID GAINS -#define KP_GPS_ROLL 0.03 -#define KD_GPS_ROLL 0.0 -#define KI_GPS_ROLL 0.01 -#define KP_GPS_PITCH 0.03 -#define KD_GPS_PITCH 0.0 -#define KI_GPS_PITCH 0.01 +#define KP_GPS_ROLL 0.012 +#define KD_GPS_ROLL 0.005 +#define KI_GPS_ROLL 0.004 +#define KP_GPS_PITCH 0.012 +#define KD_GPS_PITCH 0.005 +#define KI_GPS_PITCH 0.004 -#define GPS_MAX_ANGLE 16 // Maximun command roll and pitch angle from position control +#define GPS_MAX_ANGLE 10 // Maximun command roll and pitch angle from position control // Altitude control PID GAINS #define KP_ALTITUDE 0.8 -#define KD_ALTITUDE 0.3 +#define KD_ALTITUDE 0.2 #define KI_ALTITUDE 0.2 // The IMU should be correctly adjusted : Gyro Gains and also initial IMU offsets: // We have to take this values with the IMU flat (0º roll, 0º pitch) -#define acc_offset_x 2039 -#define acc_offset_y 2035 -#define acc_offset_z 2014 // We need to rotate the IMU exactly 90º to take this value -#define gyro_offset_roll 1650 -#define gyro_offset_pitch 1690 -#define gyro_offset_yaw 1690 +#define acc_offset_x 2079 +#define acc_offset_y 2050 +#define acc_offset_z 2008 // We need to rotate the IMU exactly 90º to take this value +#define gyro_offset_roll 1659 //1650 +#define gyro_offset_pitch 1618 //1690 +#define gyro_offset_yaw 1673 // ADC : Voltage reference 3.3v / 12bits(4096 steps) => 0.8mV/ADC step // ADXL335 Sensitivity(from datasheet) => 330mV/g, 0.8mV/ADC step => 330/0.8 = 412 -// Tested value : 414 -#define GRAVITY 414 //this equivalent to 1G in the raw data coming from the accelerometer +// Tested value : 408 +#define GRAVITY 408 //this equivalent to 1G in the raw data coming from the accelerometer #define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square #define ToRad(x) (x*0.01745329252) // *pi/180 @@ -92,19 +93,17 @@ // IDG500 Sensitivity (from datasheet) => 2.0mV/º/s, 0.8mV/ADC step => 0.8/3.33 = 0.4 // Tested values : -#define Gyro_Gain_X 0.4 //X axis Gyro gain +#define Gyro_Gain_X 0.4 //X axis Gyro gain #define Gyro_Gain_Y 0.41 //Y axis Gyro gain #define Gyro_Gain_Z 0.41 //Z axis Gyro gain #define Gyro_Scaled_X(x) x*ToRad(Gyro_Gain_X) //Return the scaled ADC raw data of the gyro in radians for second #define Gyro_Scaled_Y(x) x*ToRad(Gyro_Gain_Y) //Return the scaled ADC raw data of the gyro in radians for second #define Gyro_Scaled_Z(x) x*ToRad(Gyro_Gain_Z) //Return the scaled ADC raw data of the gyro in radians for second -//#define Kp_ROLLPITCH 0.0125 //0.010 // Pitch&Roll Proportional Gain -#define Kp_ROLLPITCH 0.002 //0.003125 -//#define Ki_ROLLPITCH 0.000010 // Pitch&Roll Integrator Gain -#define Ki_ROLLPITCH 0.0000025 +#define Kp_ROLLPITCH 0.0032 //0.002 //0.003125 // Pitch&Roll Proportional Gain +#define Ki_ROLLPITCH 0.000001 //0.000005 //0.0000025 // Pitch&Roll Integrator Gain #define Kp_YAW 1.5 // Yaw Porportional Gain -#define Ki_YAW 0.00005 // Yaw Integrator Gain +#define Ki_YAW 0.00005 //0.00005 // Yaw Integrator Gain /*For debugging purposes*/ #define OUTPUTMODE 1 //If value = 1 will print the corrected data, 0 will print uncorrected data of the gyros (with drift), 2 Accel only data @@ -159,27 +158,11 @@ float Temporary_Matrix[3][3]={ float speed_3d=0; int GPS_ground_speed=0; -//Log variables -int log_roll; -int log_pitch; -int log_yaw; - long timer=0; //general porpuse timer long timer_old; -// Sonar variables -static volatile unsigned long sonar_start_ms; -static volatile unsigned char sonar_start_t0; -static volatile unsigned long sonar_pulse_start_ms; -static volatile unsigned char sonar_pulse_start_t0; -static volatile unsigned long sonar_pulse_end_ms; -static volatile unsigned char sonar_pulse_end_t0; -static volatile byte sonar_status=0; -static volatile byte sonar_new_data=0; -int sonar_value=0; - // Attitude control variables -float command_rx_roll=0; // comandos recibidos rx +float command_rx_roll=0; // User commands float command_rx_roll_old; float command_rx_roll_diff; float command_rx_pitch=0; @@ -187,12 +170,12 @@ float command_rx_pitch_old; float command_rx_pitch_diff; float command_rx_yaw=0; float command_rx_yaw_diff; -int control_roll; // resultados del control +int control_roll; // PID control results int control_pitch; int control_yaw; float K_aux; -// Attitude control +// Attitude PID controls float roll_I=0; float roll_D; float err_roll; @@ -227,6 +210,10 @@ float command_altitude; float altitude_I; float altitude_D; +// Sonar variables +int Sonar_value=0; +#define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters +int Sonar_Counter=0; // AP_mode : 1=> Position hold 2=>Stabilization assist mode (normal mode) byte AP_mode = 2; @@ -249,7 +236,7 @@ int ch_yaw; void Altitude_control(int target_sonar_altitude) { err_altitude_old = err_altitude; - err_altitude = target_sonar_altitude - sonar_value; + err_altitude = target_sonar_altitude - Sonar_value; altitude_D = (float)(err_altitude-err_altitude_old)/G_Dt; altitude_I += (float)err_altitude*G_Dt; altitude_I = constrain(altitude_I,-100,100); @@ -316,7 +303,7 @@ void Attitude_control() roll_D = command_rx_roll_diff*KD_QUAD_COMMAND_PART - ToDeg(Omega[0]); // Take into account Angular velocity of the stick (command) // PID control - control_roll = KP_QUAD_ROLL*err_roll + KD_QUAD_ROLL*roll_D + KI_QUAD_ROLL*roll_I; + 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 @@ -332,7 +319,7 @@ void Attitude_control() pitch_D = command_rx_pitch_diff*KD_QUAD_COMMAND_PART - ToDeg(Omega[1]); // PID control - control_pitch = KP_QUAD_PITCH*err_pitch + KD_QUAD_PITCH*pitch_D + KI_QUAD_PITCH*pitch_I; + control_pitch = K_aux*err_pitch + KD_QUAD_PITCH*pitch_D + KI_QUAD_PITCH*pitch_I; // YAW CONTROL @@ -342,7 +329,7 @@ void Attitude_control() else if(err_yaw < -180) err_yaw += 360; - err_yaw = constrain(err_yaw,-25,25); // to limit max yaw command... + 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); @@ -390,11 +377,14 @@ void setup() pinMode(RELE_pin,OUTPUT); // Rele output digitalWrite(RELE_pin,LOW); + delay(250); + APM_RC_QUAD.Init(); // APM Radio initialization APM_ADC.Init(); // APM ADC library initialization DataFlash.Init(); // DataFlash log initialization GPS.Init(); // GPS Initialization + delay(100); // RC channels Initialization (Quad motors) APM_RC_QUAD.OutputCh(0,MIN_THROTTLE+15); // Motors stoped APM_RC_QUAD.OutputCh(1,MIN_THROTTLE+15); @@ -498,6 +488,11 @@ void loop(){ int aux; int i; float aux_float; + //Log variables + int log_roll; + int log_pitch; + int log_yaw; + if((millis()-timer)>=14) // Main loop 70Hz { @@ -533,8 +528,16 @@ void loop(){ Serial.print(","); Serial.print(log_yaw); + /* + for (int i=0;i<6;i++) + { + Serial.print(AN[i]); + Serial.print(","); + } + */ + // Write Sensor raw data to DataFlash log - //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); // Write attitude to DataFlash log Log_Write_Attitude(log_roll,log_pitch,log_yaw); @@ -547,10 +550,10 @@ void loop(){ ch_throttle = channel_filter(APM_RC_QUAD.InputCh(2),ch_throttle); ch_yaw = channel_filter(APM_RC_QUAD.InputCh(3),ch_yaw); command_rx_roll_old = command_rx_roll; - command_rx_roll = (ch_roll-CHANN_CENTER)/15.0; + 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)/15.0; + 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; command_rx_yaw += aux_float; @@ -562,14 +565,12 @@ void loop(){ // 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 + ((APM_RC_QUAD.InputCh(5)-1500)/300.0 + 0.4)*0.2; + K_aux = K_aux*0.8 + ((APM_RC_QUAD.InputCh(5)-1500)/300.0 + 1.7)*0.2; // /300 + 1.0 if (K_aux < 0) K_aux = 0; - /* - Serial.print(","); - Serial.print(K_aux); - */ + //Serial.print(","); + //Serial.print(K_aux); // We read the Quad Mode from Channel 5 if (APM_RC_QUAD.InputCh(4) < 1200) diff --git a/DCM.pde b/DCM.pde index edd52a181e..a2c546530a 100644 --- a/DCM.pde +++ b/DCM.pde @@ -7,7 +7,7 @@ void Read_adc_raw(void) for (int i=0;i<6;i++) AN[i] = APM_ADC.Ch(sensors[i]); - // Correction for non ratiometric sensor (test) + // Correction for non ratiometric sensor (test code) //temp = APM_ADC.Ch(3); //AN[0] += 1500-temp; //AN[1] += 1500-temp; @@ -67,8 +67,8 @@ void Drift_correction(void) // Calculate the magnitude of the accelerometer vector Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]); Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity. - // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0) - Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1); + // Weight for accelerometer info (<0.75G = 0.0, 1G = 1.0 , >1.25G = 0.0) + Accel_weight = constrain(1 - 4*abs(1 - Accel_magnitude),0,1); Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight); @@ -109,9 +109,9 @@ void Matrix_update(void) //Accel_Vector[2]=read_adc(5); // acc z // Low pass filter on accelerometer data (to filter vibrations) - Accel_Vector[0]=Accel_Vector[0]*0.4 + (float)read_adc(3)*0.6; // acc x - Accel_Vector[1]=Accel_Vector[1]*0.4 + (float)read_adc(4)*0.6; // acc y - Accel_Vector[2]=Accel_Vector[2]*0.4 + (float)read_adc(5)*0.6; // acc z + Accel_Vector[0]=Accel_Vector[0]*0.5 + (float)read_adc(3)*0.5; // acc x + Accel_Vector[1]=Accel_Vector[1]*0.5 + (float)read_adc(4)*0.5; // acc y + Accel_Vector[2]=Accel_Vector[2]*0.5 + (float)read_adc(5)*0.5; // acc z Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]);//adding integrator Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]);//adding proportional diff --git a/libraries/APM_RC/APM_RC.cpp b/libraries/APM_RC/APM_RC.cpp new file mode 100644 index 0000000000..832897fb2a --- /dev/null +++ b/libraries/APM_RC/APM_RC.cpp @@ -0,0 +1,167 @@ +/* + APM_RC.cpp - Radio Control Library for Ardupilot Mega. Arduino + Code by Jordi Muñoz and Jose Julio. DIYDrones.com + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + RC Input : PPM signal on IC4 pin + RC Output : 11 Servo outputs (standard 20ms frame) + + Methods: + Init() : Initialization of interrupts an Timers + OutpuCh(ch,pwm) : Output value to servos (range : 900-2100us) ch=0..10 + InputCh(ch) : Read a channel input value. ch=0..7 + GetState() : Returns the state of the input. 1 => New radio frame to process + Automatically resets when we call InputCh to read channels + +*/ +#include "APM_RC.h" + +#include +#include "WProgram.h" + +// Variable definition for Input Capture interrupt +volatile unsigned int ICR4_old; +volatile unsigned char PPM_Counter=0; +volatile unsigned int PWM_RAW[8] = {2400,2400,2400,2400,2400,2400,2400,2400}; +volatile unsigned char radio_status=0; + +/**************************************************** + Input Capture Interrupt ICP4 => PPM signal read + ****************************************************/ +ISR(TIMER4_CAPT_vect) +{ + unsigned int Pulse; + unsigned int Pulse_Width; + + Pulse=ICR4; + if (Pulse8000) // SYNC pulse? + PPM_Counter=0; + else + { + PPM_Counter &= 0x07; // For safety only (limit PPM_Counter to 7) + PWM_RAW[PPM_Counter++]=Pulse_Width; //Saving pulse. + if (PPM_Counter >= NUM_CHANNELS) + radio_status = 1; + } + ICR4_old = Pulse; +} + + +// Constructors //////////////////////////////////////////////////////////////// + +APM_RC_Class::APM_RC_Class() +{ +} + +// Public Methods ////////////////////////////////////////////////////////////// +void APM_RC_Class::Init(void) +{ + // Init PWM Timer 1 + pinMode(11,OUTPUT); // (PB5/OC1A) + pinMode(12,OUTPUT); //OUT2 (PB6/OC1B) + pinMode(13,OUTPUT); //OUT3 (PB7/OC1C) + + //Remember the registers not declared here remains zero by default... + TCCR1A =((1<>1; // Because timer runs at 0.5us we need to do value/2 + result2 = PWM_RAW[ch]>>1; + if (result != result2) + result = PWM_RAW[ch]>>1; // if the results are different we make a third reading (this should be fine) + + // Limit values to a valid range + result = constrain(result,MIN_PULSEWIDTH,MAX_PULSEWIDTH); + radio_status=0; // Radio channel read + return(result); +} + +unsigned char APM_RC_Class::GetState(void) +{ + return(radio_status); +} + +// make one instance for the user to use +APM_RC_Class APM_RC; \ No newline at end of file diff --git a/libraries/APM_RC/APM_RC.h b/libraries/APM_RC/APM_RC.h new file mode 100644 index 0000000000..c3c50e1867 --- /dev/null +++ b/libraries/APM_RC/APM_RC.h @@ -0,0 +1,21 @@ +#ifndef APM_RC_h +#define APM_RC_h + +#define NUM_CHANNELS 8 +#define MIN_PULSEWIDTH 900 +#define MAX_PULSEWIDTH 2100 + +class APM_RC_Class +{ + private: + public: + APM_RC_Class(); + void Init(); + void OutputCh(unsigned char ch, int pwm); + int InputCh(unsigned char ch); + unsigned char GetState(); +}; + +extern APM_RC_Class APM_RC; + +#endif \ No newline at end of file diff --git a/libraries/APM_RC/examples/APM_radio/APM_radio.pde b/libraries/APM_RC/examples/APM_radio/APM_radio.pde new file mode 100644 index 0000000000..eebc92934d --- /dev/null +++ b/libraries/APM_RC/examples/APM_radio/APM_radio.pde @@ -0,0 +1,31 @@ +/* + Example of APM_RC library. + Code by Jordi Muñoz and Jose Julio. DIYDrones.com + + Print Input values and send Output to the servos + (Works with last PPM_encoder firmware) +*/ + +#include // ArduPilot Mega RC Library + +void setup() +{ + APM_RC.Init(); // APM Radio initialization + Serial.begin(57600); + Serial.println("ArduPilot Mega RC library test"); + delay(1000); +} +void loop() +{ + if (APM_RC.GetState()==1) // New radio frame? (we could use also if((millis()- timer) > 20) + { + Serial.print("CH:"); + for(int i=0;i<8;i++) + { + Serial.print(APM_RC.InputCh(i)); // Print channel values + Serial.print(","); + APM_RC.OutputCh(i,APM_RC.InputCh(i)); // Copy input to Servos + } + Serial.println(); + } +} \ No newline at end of file diff --git a/libraries/APM_RC/keywords.txt b/libraries/APM_RC/keywords.txt new file mode 100644 index 0000000000..71a785f71c --- /dev/null +++ b/libraries/APM_RC/keywords.txt @@ -0,0 +1,5 @@ +APM_RC KEYWORD1 +begin KEYWORD2 +InputCh KEYWORD2 +OutputCh KEYWORD2 +GetState KEYWORD2