diff --git a/ArduCopter.pde b/Arducopter/Arducopter.pde similarity index 71% rename from ArduCopter.pde rename to Arducopter/Arducopter.pde index 497e910c3a..6d4d9f80b4 100644 --- a/ArduCopter.pde +++ b/Arducopter/Arducopter.pde @@ -1,17 +1,20 @@ /* ********************************************************************** */ /* ArduCopter Quadcopter code */ /* */ -/* Code based on ArduIMU DCM code from Diydrones.com */ +/* Quadcopter code from AeroQuad project and ArduIMU quadcopter project */ +/* IMU DCM code from Diydrones.com */ /* (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 (aeroquad), Jordi Muñoz, */ -/* Roberto Navoni, ... (Arcucopter team) */ -/* Date : 17-06-2010 */ -/* Version : 1.1 beta */ +/* Authors : Arducopter development team */ +/* Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz, */ +/* Jani Hirvinen, Ken McEwans, Roberto Navoni, */ +/* Sandro Benigno, Chris Anderson */ +/* Date : 04-07-2010 */ +/* Version : 1.3 beta */ /* Hardware : ArduPilot Mega + Sensor Shield (Production versions) */ +/* Mounting position : RC connectors pointing backwards */ /* This code use this libraries : */ -/* APM_RC_QUAD : Radio library (adapted for quads) */ +/* APM_RC : Radio library (with InstantPWM) */ /* APM_ADC : External ADC library */ /* DataFlash : DataFlash log library */ /* APM_BMP085 : BMP085 barometer library */ @@ -21,12 +24,16 @@ #include #include -#include +#include #include #include // Put your GPS library here: #include // MTK GPS +// EEPROM storage for user configurable values +#include +#include "UserSettings.h" + /* APM Hardware definitions */ #define LED_Yellow 36 #define LED_Red 35 @@ -39,48 +46,6 @@ /* ***************************************************************************** */ /* CONFIGURATION PART */ /* ***************************************************************************** */ -//Adjust this parameter for your lattitude -#define GEOG_CORRECTION_FACTOR 0.87 // cos(lattitude) - -#define RADIO_TEST_MODE 0 // 0:Normal 1:Radio Test mode (to test radio channels) -#define MAGNETOMETER 1 // 0 : No magnetometer, 1: Magnetometer - -// QuadCopter Attitude control PID GAINS -#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 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.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 10 // Maximun command roll and pitch angle from position control - -// Altitude control PID GAINS -#define KP_ALTITUDE 0.8 -#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 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 @@ -100,11 +65,6 @@ #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.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 //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 @@ -112,7 +72,7 @@ uint8_t sensors[6] = {1,2,0,4,5,6}; // For ArduPilot Mega Sensor Shield Hardware //Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ -int SENSOR_SIGN[]={-1,1,-1,1,-1,1,-1,-1,-1}; //{1,-1,-1,1,-1,1,-1,-1,-1} +int SENSOR_SIGN[]={1,-1,-1,-1,1,1,-1,-1,-1}; //{-1,1,-1,1,-1,1,-1,-1,-1}; int AN[6]; //array that store the 6 ADC channels int AN_OFFSET[6]; //Array that store the Offset of the gyros and accelerometers @@ -228,9 +188,25 @@ int ch_roll; int ch_pitch; int ch_throttle; int ch_yaw; +int ch_aux; +int ch_aux2; #define CHANN_CENTER 1500 #define MIN_THROTTLE 1040 // Throttle pulse width at minimun... +// Motor variables +#define FLIGHT_MODE_+ +//#define FLIGHT_MODE_X +int frontMotor; +int backMotor; +int leftMotor; +int rightMotor; +byte motorArmed = 0; + +// Serial communication +#define CONFIGURATOR +char queryType; +long tlmTimer = 0; + /* ************************************************************ */ /* Altitude control... (based on sonar) */ void Altitude_control(int target_sonar_altitude) @@ -303,6 +279,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 + 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 @@ -319,6 +296,7 @@ void Attitude_control() 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 @@ -339,6 +317,60 @@ void Attitude_control() control_yaw = KP_QUAD_YAW*err_yaw + KD_QUAD_YAW*yaw_D + KI_QUAD_YAW*yaw_I; } +void Rate_control() +{ + static float previousRollRate, previousPitchRate, previousYawRate; + float currentRollRate, currentPitchRate, currentYawRate; + + // ROLL CONTROL + + // NOTE: We need to test THIS !! Plaese CHECK + #ifdef FLIGHT_MODE_+ + currentRollRate = read_adc(0); // I need a positive sign here + #endif + #ifdef FLIGHT_MODE_X + currentRollRate = -read_adc(0); // Original from Ted + #endif + + err_roll = ((ch_roll-1500) * xmitFactor) - currentRollRate; + + roll_I += err_roll*G_Dt; + roll_I = constrain(roll_I,-20,20); + + roll_D = currentRollRate - previousRollRate; + previousRollRate = currentRollRate; + + // PID control + control_roll = Kp_RateRoll*err_roll + Kd_RateRoll*roll_D + Ki_RateRoll*roll_I; + + // PITCH CONTROL + currentPitchRate = read_adc(1); + err_pitch = ((ch_pitch-1500) * xmitFactor) - currentPitchRate; + + pitch_I += err_pitch*G_Dt; + pitch_I = constrain(pitch_I,-20,20); + + pitch_D = currentPitchRate - previousPitchRate; + 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-1500)* xmitFactor) - currentYawRate; + + yaw_I += err_yaw*G_Dt; + yaw_I = constrain(yaw_I,-20,20); + + yaw_D = currentYawRate - previousYawRate; + 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) { @@ -379,27 +411,28 @@ void setup() delay(250); - APM_RC_QUAD.Init(); // APM Radio initialization + APM_RC.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); - APM_RC_QUAD.OutputCh(2,MIN_THROTTLE+15); - APM_RC_QUAD.OutputCh(3,MIN_THROTTLE+15); - #if (MAGNETOMETER) + readUserConfig(); // Load user configurable items from EEPROM + + // 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 - #endif DataFlash.StartWrite(1); // Start a write session on page 1 - Serial.begin(57600); - Serial.println(); - Serial.println("ArduCopter Quadcopter v1.0"); + //Serial.begin(57600); + Serial.begin(115200); + //Serial.println(); + //Serial.println("ArduCopter Quadcopter v1.0"); // Check if we enable the DataFlash log Read Mode (switch) // If we press switch 1 at startup we read the Dataflash eeprom @@ -410,7 +443,7 @@ void setup() delay(30000); } - delay(3000); + //delay(3000); Read_adc_raw(); delay(20); @@ -424,7 +457,7 @@ void setup() aux_float[2] = gyro_offset_yaw; // Take the gyro offset values - for(i=0;i<250;i++) + for(i=0;i<300;i++) { Read_adc_raw(); for(int y=0; y<=2; y++) // Read initial ADC values for gyro offset. @@ -439,34 +472,36 @@ void setup() } 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++) { Serial.print("AN[]:"); Serial.println(AN_OFFSET[i]); } - Neutro_yaw = APM_RC_QUAD.InputCh(3); // Take yaw neutral radio value Serial.print("Yaw neutral value:"); Serial.println(Neutro_yaw); + #endif #if (RADIO_TEST_MODE) // RADIO TEST MODE TO TEST RADIO CHANNELS while(1) { - if (APM_RC_QUAD.GetState()==1) + if (APM_RC.GetState()==1) { Serial.print("AIL:"); - Serial.print(APM_RC_QUAD.InputCh(0)); + Serial.print(APM_RC.InputCh(0)); Serial.print("ELE:"); - Serial.print(APM_RC_QUAD.InputCh(1)); + Serial.print(APM_RC.InputCh(1)); Serial.print("THR:"); - Serial.print(APM_RC_QUAD.InputCh(2)); + Serial.print(APM_RC.InputCh(2)); Serial.print("YAW:"); - Serial.print(APM_RC_QUAD.InputCh(3)); + Serial.print(APM_RC.InputCh(3)); Serial.print("AUX(mode):"); - Serial.print(APM_RC_QUAD.InputCh(4)); + Serial.print(APM_RC.InputCh(4)); Serial.print("AUX2:"); - Serial.print(APM_RC_QUAD.InputCh(5)); + Serial.print(APM_RC.InputCh(5)); Serial.println(); delay(200); } @@ -477,8 +512,10 @@ void setup() DataFlash.StartWrite(1); // Start a write session on page 1 timer = millis(); + tlmTimer = millis(); Read_adc_raw(); // Initialize ADC readings... delay(20); + motorArmed = 0; digitalWrite(LED_Green,HIGH); // Ready to go... } @@ -494,7 +531,7 @@ void loop(){ int log_yaw; - if((millis()-timer)>=14) // Main loop 70Hz + if((millis()-timer)>=10) // Main loop 100Hz { counter++; timer_old = timer; @@ -503,14 +540,14 @@ void loop(){ // IMU DCM Algorithm Read_adc_raw(); - #if (MAGNETOMETER) - if (counter > 8) // Read compass data at 10Hz... (7 loop runs) - { - counter=0; - APM_Compass.Read(); // Read magnetometer - APM_Compass.Calculate(roll,pitch); // Calculate heading - } - #endif + if (MAGNETOMETER == 1) { + if (counter > 10) // Read compass data at 10Hz... (10 loop runs) + { + counter=0; + APM_Compass.Read(); // Read magnetometer + APM_Compass.Calculate(roll,pitch); // Calculate heading + } + } Matrix_update(); Normalize(); Drift_correction(); @@ -521,34 +558,36 @@ void loop(){ log_roll = ToDeg(roll)*10; log_pitch = ToDeg(pitch)*10; log_yaw = ToDeg(yaw)*10; - + + #ifndef CONFIGURATOR Serial.print(log_roll); Serial.print(","); Serial.print(log_pitch); Serial.print(","); Serial.print(log_yaw); - /* for (int i=0;i<6;i++) { Serial.print(AN[i]); Serial.print(","); } - */ + #endif // Write Sensor raw data to DataFlash log 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); - if (APM_RC_QUAD.GetState()==1) // New radio frame? + if (APM_RC.GetState()==1) // New radio frame? { // Commands from radio Rx... // Stick position defines the desired angle in roll, pitch and yaw - ch_roll = channel_filter(APM_RC_QUAD.InputCh(0),ch_roll); - ch_pitch = channel_filter(APM_RC_QUAD.InputCh(1),ch_pitch); - ch_throttle = channel_filter(APM_RC_QUAD.InputCh(2),ch_throttle); - ch_yaw = channel_filter(APM_RC_QUAD.InputCh(3),ch_yaw); + ch_roll = channel_filter(APM_RC.InputCh(0),ch_roll); + ch_pitch = channel_filter(APM_RC.InputCh(1),ch_pitch); + ch_throttle = channel_filter(APM_RC.InputCh(2),ch_throttle); + ch_yaw = channel_filter(APM_RC.InputCh(3),ch_yaw); + ch_aux = APM_RC.InputCh(4); + ch_aux2 = APM_RC.InputCh(5); 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; @@ -562,18 +601,20 @@ void loop(){ 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 + ((APM_RC_QUAD.InputCh(5)-1500)/300.0 + 1.7)*0.2; // /300 + 1.0 + K_aux = K_aux*0.8 + ((ch_aux2-1500)/300.0 + 1.7)*0.2; // /300 + 1.0 + if (K_aux < 0) K_aux = 0; //Serial.print(","); //Serial.print(K_aux); - + // We read the Quad Mode from Channel 5 - if (APM_RC_QUAD.InputCh(4) < 1200) + if (ch_aux < 1200) { AP_mode = 1; // Position hold mode (GPS position control) digitalWrite(LED_Yellow,HIGH); // Yellow LED On @@ -593,11 +634,13 @@ void loop(){ { target_lattitude = GPS.Lattitude; target_longitude = GPS.Longitude; + #ifndef CONFIGURATOR Serial.println(); Serial.print("* Target:"); Serial.print(target_longitude); Serial.print(","); Serial.println(target_lattitude); + #endif target_position=1; //target_sonar_altitude = sonar_value; //Initial_Throttle = ch3; @@ -635,13 +678,6 @@ void loop(){ if ((target_position==1)&&(GPS.Fix)) { Position_control(target_lattitude,target_longitude); // Call position hold routine - /* - Serial.print("PC:"); - Serial.print(command_gps_roll); - Serial.print(","); - Serial.print(command_gps_pitch); - Serial.println(); - */ } else { @@ -652,30 +688,83 @@ void loop(){ } } - // Attitude control (Roll, Pitch, yaw) - Attitude_control(); + // Control methodology selected using AUX2 + if (ch_aux2 < 1200) + Attitude_control(); + else + { + Rate_control(); + // 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 + if (ch_throttle < 1100) { + control_yaw = 0; + if (ch_yaw > 1800) + motorArmed = 1; + if (ch_yaw < 1200) + motorArmed = 0; + } // Quadcopter mix + // Ask Jose if we still need this IF statement, and if we want to do an ESC calibration if (ch_throttle > (MIN_THROTTLE+20)) // Minimun throttle to start control { - APM_RC_QUAD.OutputCh(0,ch_throttle - control_roll - control_yaw); // Right motor - APM_RC_QUAD.OutputCh(1,ch_throttle + control_roll - control_yaw); // Left motor - APM_RC_QUAD.OutputCh(2,ch_throttle + control_pitch + control_yaw); // Front motor - APM_RC_QUAD.OutputCh(3,ch_throttle - control_pitch + control_yaw); // Back motor + if (motorArmed == 1) { + #ifdef FLIGHT_MODE_+ + rightMotor = ch_throttle - control_roll - control_yaw; + leftMotor = ch_throttle + control_roll - control_yaw; + frontMotor = ch_throttle + control_pitch + control_yaw; + backMotor = ch_throttle - control_pitch + control_yaw; + #endif + #ifdef FLIGHT_MODE_X + frontMotor = ch_throttle + control_roll - control_pitch - control_yaw; // front left motor + rightMotor = ch_throttle - control_roll - control_pitch + control_yaw; // front right motor + leftMotor = ch_throttle + control_roll + control_pitch + control_yaw; // rear left motor + backMotor = ch_throttle - control_roll + control_pitch - control_yaw; // rear right motor + #endif + } + if (motorArmed == 0) { + rightMotor = MIN_THROTTLE; + leftMotor = MIN_THROTTLE; + frontMotor = MIN_THROTTLE; + backMotor = MIN_THROTTLE; + } + APM_RC.OutputCh(0, rightMotor); // Right motor + APM_RC.OutputCh(1, leftMotor); // Left motor + APM_RC.OutputCh(2, frontMotor); // Front motor + APM_RC.OutputCh(3, backMotor); // Back motor + // InstantPWM + APM_RC.Force_Out0_Out1(); + APM_RC.Force_Out2_Out3(); } else { roll_I = 0; // reset I terms of PID controls pitch_I = 0; yaw_I = 0; - APM_RC_QUAD.OutputCh(0,MIN_THROTTLE); // Motors stoped - APM_RC_QUAD.OutputCh(1,MIN_THROTTLE); - APM_RC_QUAD.OutputCh(2,MIN_THROTTLE); - APM_RC_QUAD.OutputCh(3,MIN_THROTTLE); - // Initialize yaw command to actual yaw + 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); + // InstantPWM + APM_RC.Force_Out0_Out1(); + APM_RC.Force_Out2_Out3(); + + // Initialize yaw command to actual yaw when throttle is down... command_rx_yaw = ToDeg(yaw); command_rx_yaw_diff = 0; } + #ifndef CONFIGURATOR Serial.println(); // Line END - } + #endif + } + #ifdef CONFIGURATOR + if((millis()-tlmTimer)>=100) { + readSerialCommand(); + sendSerialTelemetry(); + tlmTimer = millis(); + } + #endif } diff --git a/DCM.pde b/Arducopter/DCM.pde similarity index 86% rename from DCM.pde rename to Arducopter/DCM.pde index a2c546530a..cd73a922b5 100644 --- a/DCM.pde +++ b/Arducopter/DCM.pde @@ -15,7 +15,7 @@ void Read_adc_raw(void) } // Returns an analog value with the offset -float read_adc(int select) +int read_adc(int select) { if (SENSOR_SIGN[select]<0) return (AN_OFFSET[select]-AN[select]); @@ -65,10 +65,13 @@ void Drift_correction(void) //*****Roll and Pitch*************** // 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. + //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.75G = 0.0, 1G = 1.0 , >1.25G = 0.0) - Accel_weight = constrain(1 - 4*abs(1 - Accel_magnitude),0,1); + // Accel_weight = constrain(1 - 4*abs(1 - Accel_magnitude),0,1); + // 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); + Accel_weight = 1.0; 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); @@ -78,7 +81,7 @@ void Drift_correction(void) //*****YAW*************** // We make the gyro YAW drift correction based on compass magnetic heading - #if (MAGNETOMETER) + if (MAGNETOMETER == 1) { errorCourse= (DCM_Matrix[0][0]*APM_Compass.Heading_Y) - (DCM_Matrix[1][0]*APM_Compass.Heading_X); //Calculating YAW error Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position. @@ -87,7 +90,7 @@ void Drift_correction(void) Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW); Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I - #endif + } } /**************************************************/ @@ -104,14 +107,14 @@ void Matrix_update(void) Gyro_Vector[1]=Gyro_Scaled_Y(read_adc(1)); //gyro y pitch Gyro_Vector[2]=Gyro_Scaled_Z(read_adc(2)); //gyro Z yaw - //Accel_Vector[0]=read_adc(3); // acc x - //Accel_Vector[1]=read_adc(4); // acc y - //Accel_Vector[2]=read_adc(5); // acc z + Accel_Vector[0]=read_adc(3); // acc x + Accel_Vector[1]=read_adc(4); // acc y + Accel_Vector[2]=read_adc(5); // acc z // Low pass filter on accelerometer data (to filter vibrations) - 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 + //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/Log.pde b/Arducopter/Log.pde similarity index 100% rename from Log.pde rename to Arducopter/Log.pde diff --git a/Arducopter/SerialCom.pde b/Arducopter/SerialCom.pde new file mode 100644 index 0000000000..7cec5cd78a --- /dev/null +++ b/Arducopter/SerialCom.pde @@ -0,0 +1,410 @@ +/* + ArduCopter v1.2 - June 2010 + www.ArduCopter.com + Copyright (c) 2010. All rights reserved. + An Open Source Arduino based multicopter. + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +void readSerialCommand() { + // Check for serial message + if (Serial.available()) { + queryType = Serial.read(); + switch (queryType) { + case 'A': // Stable PID + KP_QUAD_ROLL = readFloatSerial(); + KI_QUAD_ROLL = readFloatSerial(); + KD_QUAD_ROLL = readFloatSerial(); + KP_QUAD_PITCH = readFloatSerial(); + KI_QUAD_PITCH = readFloatSerial(); + KD_QUAD_PITCH = readFloatSerial(); + KP_QUAD_YAW = readFloatSerial(); + KI_QUAD_YAW = readFloatSerial(); + KD_QUAD_YAW = readFloatSerial(); + KD_QUAD_COMMAND_PART = readFloatSerial(); + MAGNETOMETER = readFloatSerial(); + break; + case 'C': // Receive GPS PID + KP_GPS_ROLL = readFloatSerial(); + KI_GPS_ROLL = readFloatSerial(); + KD_GPS_ROLL = readFloatSerial(); + KP_GPS_PITCH = readFloatSerial(); + KI_GPS_PITCH = readFloatSerial(); + KD_GPS_PITCH = readFloatSerial(); + GPS_MAX_ANGLE = readFloatSerial(); + GEOG_CORRECTION_FACTOR = readFloatSerial(); + break; + case 'E': // Receive altitude PID + KP_ALTITUDE = readFloatSerial(); + KD_ALTITUDE = readFloatSerial(); + KI_ALTITUDE = readFloatSerial(); + break; + case 'G': // Receive drift correction PID + Kp_ROLLPITCH = readFloatSerial(); + Ki_ROLLPITCH = readFloatSerial(); + Kp_YAW = readFloatSerial(); + Ki_YAW = readFloatSerial(); + break; + case 'I': // Receive sensor offset + gyro_offset_roll = readFloatSerial(); + gyro_offset_pitch = readFloatSerial(); + gyro_offset_yaw = readFloatSerial(); + acc_offset_x = readFloatSerial(); + acc_offset_y = readFloatSerial(); + acc_offset_z = readFloatSerial(); + break; + case 'K': // Spare + break; + case 'M': // Receive debug motor commands + frontMotor = readFloatSerial(); + backMotor = readFloatSerial(); + rightMotor = readFloatSerial(); + leftMotor = readFloatSerial(); + motorArmed = readFloatSerial(); + break; + case 'O': // Rate Control PID + Kp_RateRoll = readFloatSerial(); + Ki_RateRoll = readFloatSerial(); + Kd_RateRoll = readFloatSerial(); + Kp_RatePitch = readFloatSerial(); + Ki_RatePitch = readFloatSerial(); + Kd_RatePitch = readFloatSerial(); + Kp_RateYaw = readFloatSerial(); + Ki_RateYaw = readFloatSerial(); + Kd_RateYaw = readFloatSerial(); + xmitFactor = readFloatSerial(); + break; + case 'W': // Write all user configurable values to EEPROM + 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(KP_QUAD_PITCH, KP_QUAD_PITCH_ADR); + writeEEPROM(KD_QUAD_PITCH, KD_QUAD_PITCH_ADR); + writeEEPROM(KI_QUAD_PITCH, KI_QUAD_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(KD_QUAD_COMMAND_PART, KD_QUAD_COMMAND_PART_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); + writeEEPROM(KP_GPS_PITCH, KP_GPS_PITCH_ADR); + writeEEPROM(KD_GPS_PITCH, KD_GPS_PITCH_ADR); + writeEEPROM(KI_GPS_PITCH, KI_GPS_PITCH_ADR); + writeEEPROM(GPS_MAX_ANGLE, GPS_MAX_ANGLE_ADR); + writeEEPROM(KP_ALTITUDE, KP_ALTITUDE_ADR); + writeEEPROM(KD_ALTITUDE, KD_ALTITUDE_ADR); + writeEEPROM(KI_ALTITUDE, KI_ALTITUDE_ADR); + writeEEPROM(acc_offset_x, acc_offset_x_ADR); + writeEEPROM(acc_offset_y, acc_offset_y_ADR); + writeEEPROM(acc_offset_z, acc_offset_z_ADR); + writeEEPROM(gyro_offset_roll, gyro_offset_roll_ADR); + writeEEPROM(gyro_offset_pitch, gyro_offset_pitch_ADR); + writeEEPROM(gyro_offset_yaw, gyro_offset_yaw_ADR); + writeEEPROM(Kp_ROLLPITCH, Kp_ROLLPITCH_ADR); + writeEEPROM(Ki_ROLLPITCH, Ki_ROLLPITCH_ADR); + writeEEPROM(Kp_YAW, Kp_YAW_ADR); + writeEEPROM(Ki_YAW, Ki_YAW_ADR); + writeEEPROM(GEOG_CORRECTION_FACTOR, GEOG_CORRECTION_FACTOR_ADR); + writeEEPROM(MAGNETOMETER, MAGNETOMETER_ADR); + writeEEPROM(Kp_RateRoll, KP_RATEROLL_ADR); + writeEEPROM(Ki_RateRoll, KI_RATEROLL_ADR); + writeEEPROM(Kd_RateRoll, KD_RATEROLL_ADR); + writeEEPROM(Kp_RatePitch, KP_RATEPITCH_ADR); + writeEEPROM(Ki_RatePitch, KI_RATEPITCH_ADR); + writeEEPROM(Kd_RatePitch, KD_RATEPITCH_ADR); + writeEEPROM(Kp_RateYaw, KP_RATEYAW_ADR); + writeEEPROM(Ki_RateYaw, KI_RATEYAW_ADR); + writeEEPROM(Kd_RateYaw, KD_RATEYAW_ADR); + writeEEPROM(xmitFactor, XMITFACTOR_ADR); + break; + case 'Y': // Initialize EEPROM with default values + KP_QUAD_ROLL = 1.8; + KD_QUAD_ROLL = 0.48; + KI_QUAD_ROLL = 0.40; + KP_QUAD_PITCH = 1.8; + KD_QUAD_PITCH = 0.48; + KI_QUAD_PITCH = 0.40; + KP_QUAD_YAW = 3.6; + KD_QUAD_YAW = 1.2; + KI_QUAD_YAW = 0.15; + KD_QUAD_COMMAND_PART = 4.0; + KP_GPS_ROLL = 0.012; + KD_GPS_ROLL = 0.005; + KI_GPS_ROLL = 0.004; + KP_GPS_PITCH = 0.012; + KD_GPS_PITCH = 0.005; + KI_GPS_PITCH = 0.004; + GPS_MAX_ANGLE = 10; + KP_ALTITUDE = 0.8; + KD_ALTITUDE = 0.2; + KI_ALTITUDE = 0.2; + acc_offset_x = 2073; + acc_offset_y = 2056; + acc_offset_z = 2010; + gyro_offset_roll = 1659; + gyro_offset_pitch = 1618; + gyro_offset_yaw = 1673; + Kp_ROLLPITCH = 0.002; + Ki_ROLLPITCH = 0.0000005; + Kp_YAW = 1.5; + 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; + break; + } + } +} + +void sendSerialTelemetry() { + float aux_float[3]; // used for sensor calibration + switch (queryType) { + case '=': // Reserved debug command to view any variable from Serial Monitor + Serial.print("throttle =");Serial.println(ch_throttle); + Serial.print("control roll =");Serial.println(control_roll-CHANN_CENTER); + Serial.print("control pitch =");Serial.println(control_pitch-CHANN_CENTER); + Serial.print("control yaw =");Serial.println(control_yaw-CHANN_CENTER); + Serial.print("front left yaw =");Serial.println(frontMotor); + Serial.print("front right yaw =");Serial.println(rightMotor); + Serial.print("rear left yaw =");Serial.println(leftMotor); + Serial.print("rear right motor =");Serial.println(backMotor);Serial.println(); + + Serial.print("current roll rate =");Serial.println(read_adc(0)); + Serial.print("current pitch rate =");Serial.println(read_adc(1)); + Serial.print("current yaw rate =");Serial.println(read_adc(2)); + Serial.print("command rx yaw =");Serial.println(command_rx_yaw); + Serial.println(); + queryType = 'X'; + break; + case 'B': // Send roll, pitch and yaw PID values + Serial.print(KP_QUAD_ROLL, 3); + comma(); + Serial.print(KI_QUAD_ROLL, 3); + comma(); + Serial.print(KD_QUAD_ROLL, 3); + comma(); + Serial.print(KP_QUAD_PITCH, 3); + comma(); + Serial.print(KI_QUAD_PITCH, 3); + comma(); + Serial.print(KD_QUAD_PITCH, 3); + comma(); + Serial.print(KP_QUAD_YAW, 3); + comma(); + Serial.print(KI_QUAD_YAW, 3); + comma(); + Serial.print(KD_QUAD_YAW, 3); + comma(); + Serial.print(KD_QUAD_COMMAND_PART, 3); + comma(); + Serial.println(MAGNETOMETER, 3); + queryType = 'X'; + break; + case 'D': // Send GPS PID + Serial.print(KP_GPS_ROLL, 3); + comma(); + Serial.print(KI_GPS_ROLL, 3); + comma(); + Serial.print(KD_GPS_ROLL, 3); + comma(); + Serial.print(KP_GPS_PITCH, 3); + comma(); + Serial.print(KI_GPS_PITCH, 3); + comma(); + Serial.print(KD_GPS_PITCH, 3); + comma(); + Serial.print(GPS_MAX_ANGLE, 3); + comma(); + Serial.println(GEOG_CORRECTION_FACTOR, 3); + queryType = 'X'; + break; + case 'F': // Send altitude PID + Serial.print(KP_ALTITUDE, 3); + comma(); + Serial.print(KI_ALTITUDE, 3); + comma(); + Serial.println(KD_ALTITUDE, 3); + queryType = 'X'; + break; + case 'H': // Send drift correction PID + Serial.print(Kp_ROLLPITCH, 4); + comma(); + Serial.print(Ki_ROLLPITCH, 7); + comma(); + Serial.print(Kp_YAW, 4); + comma(); + Serial.println(Ki_YAW, 6); + queryType = 'X'; + break; + case 'J': // Send sensor offset + Serial.print(gyro_offset_roll); + comma(); + Serial.print(gyro_offset_pitch); + comma(); + Serial.print(gyro_offset_yaw); + comma(); + Serial.print(acc_offset_x); + comma(); + Serial.print(acc_offset_y); + comma(); + Serial.println(acc_offset_z); + AN_OFFSET[3] = acc_offset_x; + AN_OFFSET[4] = acc_offset_y; + AN_OFFSET[5] = acc_offset_z; + queryType = 'X'; + break; + case 'L': // Spare + queryType = 'X'; + break; + case 'N': // Send magnetometer config + queryType = 'X'; + break; + case 'P': // Send rate control PID + Serial.print(Kp_RateRoll, 3); + comma(); + Serial.print(Ki_RateRoll, 3); + comma(); + Serial.print(Kd_RateRoll, 3); + comma(); + Serial.print(Kp_RatePitch, 3); + comma(); + Serial.print(Ki_RatePitch, 3); + comma(); + Serial.print(Kd_RatePitch, 3); + comma(); + Serial.print(Kp_RateYaw, 3); + comma(); + Serial.print(Ki_RateYaw, 3); + comma(); + Serial.print(Kd_RateYaw, 3); + comma(); + Serial.println(xmitFactor, 3); + queryType = 'X'; + break; + case 'Q': // Send sensor data + Serial.print(read_adc(0)); + comma(); + Serial.print(read_adc(1)); + comma(); + Serial.print(read_adc(2)); + comma(); + Serial.print(read_adc(4)); + comma(); + Serial.print(read_adc(3)); + comma(); + Serial.print(read_adc(5)); + comma(); + Serial.print(err_roll); + comma(); + Serial.print(err_pitch); + comma(); + Serial.print(degrees(-roll)); + comma(); + Serial.print(degrees(-pitch)); + comma(); + Serial.println(degrees(yaw)); + break; + case 'R': // Send raw sensor data + break; + case 'S': // Send all flight data + Serial.print(timer-timer_old); + comma(); + Serial.print(read_adc(0)); + comma(); + Serial.print(read_adc(1)); + comma(); + Serial.print(read_adc(2)); + comma(); + Serial.print(ch_throttle); + comma(); + Serial.print(control_roll); + comma(); + Serial.print(control_pitch); + comma(); + Serial.print(control_yaw); + comma(); + Serial.print(frontMotor); // Front Motor + comma(); + Serial.print(backMotor); // Back Motor + comma(); + Serial.print(rightMotor); // Right Motor + comma(); + Serial.print(leftMotor); // Left Motor + comma(); + Serial.print(read_adc(4)); + comma(); + Serial.print(read_adc(3)); + comma(); + Serial.println(read_adc(5)); + break; + case 'T': // Spare + break; + case 'U': // Send receiver values + Serial.print(ch_roll); // Aileron + comma(); + Serial.print(ch_pitch); // Elevator + comma(); + Serial.print(ch_yaw); // Yaw + comma(); + Serial.print(ch_throttle); // Throttle + comma(); + Serial.print(ch_aux); // AUX1 (Mode) + comma(); + Serial.println(ch_aux2); // AUX2 + break; + case 'V': // Spare + break; + case 'X': // Stop sending messages + break; + case '!': // Send flight software version + Serial.println("1.2"); + queryType = 'X'; + break; + } +} + +// Used to read floating point values from the serial port +float readFloatSerial() { + byte index = 0; + byte timeout = 0; + char data[128] = ""; + + do { + if (Serial.available() == 0) { + delay(10); + timeout++; + } + else { + data[index] = Serial.read(); + timeout = 0; + index++; + } + } while ((data[constrain(index-1, 0, 128)] != ';') && (timeout < 5) && (index < 128)); + return atof(data); +} + +void comma() { + Serial.print(','); +} diff --git a/Arducopter/UserSettings.h b/Arducopter/UserSettings.h new file mode 100644 index 0000000000..d1aa89c175 --- /dev/null +++ b/Arducopter/UserSettings.h @@ -0,0 +1,177 @@ +/* + ArduCopter v1.2 + www.ArduCopter.com + Copyright (c) 2010. All rights reserved. + An Open Source Arduino based multicopter. + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +#include "WProgram.h" + +// Following variables stored in EEPROM +float KP_QUAD_ROLL; +float KD_QUAD_ROLL; +float KI_QUAD_ROLL; +float KP_QUAD_PITCH; +float KD_QUAD_PITCH; +float KI_QUAD_PITCH; +float KP_QUAD_YAW; +float KD_QUAD_YAW; +float KI_QUAD_YAW; +float KD_QUAD_COMMAND_PART; +float KP_GPS_ROLL; +float KD_GPS_ROLL; +float KI_GPS_ROLL; +float KP_GPS_PITCH; +float KD_GPS_PITCH; +float KI_GPS_PITCH; +float GPS_MAX_ANGLE; +float KP_ALTITUDE; +float KD_ALTITUDE; +float KI_ALTITUDE; +int acc_offset_x; +int acc_offset_y; +int acc_offset_z; +int gyro_offset_roll; +int gyro_offset_pitch; +int gyro_offset_yaw; +float Kp_ROLLPITCH; +float Ki_ROLLPITCH; +float Kp_YAW; +float Ki_YAW; +float GEOG_CORRECTION_FACTOR; +int MAGNETOMETER; +float Kp_RateRoll; +float Ki_RateRoll; +float Kd_RateRoll; +float Kp_RatePitch; +float Ki_RatePitch; +float Kd_RatePitch; +float Kp_RateYaw; +float Ki_RateYaw; +float Kd_RateYaw; +float xmitFactor; + +// EEPROM storage addresses +#define KP_QUAD_ROLL_ADR 0 +#define KD_QUAD_ROLL_ADR 4 +#define KI_QUAD_ROLL_ADR 8 +#define KP_QUAD_PITCH_ADR 12 +#define KD_QUAD_PITCH_ADR 16 +#define KI_QUAD_PITCH_ADR 20 +#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 KP_GPS_ROLL_ADR 40 +#define KD_GPS_ROLL_ADR 44 +#define KI_GPS_ROLL_ADR 48 +#define KP_GPS_PITCH_ADR 52 +#define KD_GPS_PITCH_ADR 56 +#define KI_GPS_PITCH_ADR 60 +#define GPS_MAX_ANGLE_ADR 64 +#define KP_ALTITUDE_ADR 68 +#define KD_ALTITUDE_ADR 72 +#define KI_ALTITUDE_ADR 76 +#define acc_offset_x_ADR 80 +#define acc_offset_y_ADR 84 +#define acc_offset_z_ADR 88 +#define gyro_offset_roll_ADR 92 +#define gyro_offset_pitch_ADR 96 +#define gyro_offset_yaw_ADR 100 +#define Kp_ROLLPITCH_ADR 104 +#define Ki_ROLLPITCH_ADR 108 +#define Kp_YAW_ADR 112 +#define Ki_YAW_ADR 116 +#define GEOG_CORRECTION_FACTOR_ADR 120 +#define MAGNETOMETER_ADR 124 +#define XMITFACTOR_ADR 128 +#define KP_RATEROLL_ADR 132 +#define KI_RATEROLL_ADR 136 +#define KD_RATEROLL_ADR 140 +#define KP_RATEPITCH_ADR 144 +#define KI_RATEPITCH_ADR 148 +#define KD_RATEPITCH_ADR 152 +#define KP_RATEYAW_ADR 156 +#define KI_RATEYAW_ADR 160 +#define KD_RATEYAW_ADR 164 + +// Utilities for writing and reading from the EEPROM +float readEEPROM(int address) { + union floatStore { + byte floatByte[4]; + float floatVal; + } floatOut; + + for (int i = 0; i < 4; i++) + floatOut.floatByte[i] = EEPROM.read(address + i); + return floatOut.floatVal; +} + +void writeEEPROM(float value, int address) { + union floatStore { + byte floatByte[4]; + float floatVal; + } floatIn; + + floatIn.floatVal = value; + for (int i = 0; i < 4; i++) + EEPROM.write(address + i, floatIn.floatByte[i]); +} + +void readUserConfig() { + KP_QUAD_ROLL = readEEPROM(KP_QUAD_ROLL_ADR); + KD_QUAD_ROLL = readEEPROM(KD_QUAD_ROLL_ADR); + KI_QUAD_ROLL = readEEPROM(KI_QUAD_ROLL_ADR); + KP_QUAD_PITCH = readEEPROM(KP_QUAD_PITCH_ADR); + KD_QUAD_PITCH = readEEPROM(KD_QUAD_PITCH_ADR); + KI_QUAD_PITCH = readEEPROM(KI_QUAD_PITCH_ADR); + 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); + KP_GPS_ROLL = readEEPROM(KP_GPS_ROLL_ADR); + KD_GPS_ROLL = readEEPROM(KD_GPS_ROLL_ADR); + KI_GPS_ROLL = readEEPROM(KI_GPS_ROLL_ADR); + KP_GPS_PITCH = readEEPROM(KP_GPS_PITCH_ADR); + KD_GPS_PITCH = readEEPROM(KD_GPS_PITCH_ADR); + KI_GPS_PITCH = readEEPROM(KI_GPS_PITCH_ADR); + GPS_MAX_ANGLE = readEEPROM(GPS_MAX_ANGLE_ADR); + KP_ALTITUDE = readEEPROM(KP_ALTITUDE_ADR); + KD_ALTITUDE = readEEPROM(KD_ALTITUDE_ADR); + KI_ALTITUDE = readEEPROM(KI_ALTITUDE_ADR); + acc_offset_x = readEEPROM(acc_offset_x_ADR); + acc_offset_y = readEEPROM(acc_offset_y_ADR); + acc_offset_z = readEEPROM(acc_offset_z_ADR); + gyro_offset_roll = readEEPROM(gyro_offset_roll_ADR); + gyro_offset_pitch = readEEPROM(gyro_offset_pitch_ADR); + gyro_offset_yaw = readEEPROM(gyro_offset_yaw_ADR); + Kp_ROLLPITCH = readEEPROM(Kp_ROLLPITCH_ADR); + Ki_ROLLPITCH = readEEPROM(Ki_ROLLPITCH_ADR); + Kp_YAW = readEEPROM(Kp_YAW_ADR); + Ki_YAW = readEEPROM(Ki_YAW_ADR); + GEOG_CORRECTION_FACTOR = readEEPROM(GEOG_CORRECTION_FACTOR_ADR); + MAGNETOMETER = readEEPROM(MAGNETOMETER_ADR); + Kp_RateRoll = readEEPROM(KP_RATEROLL_ADR); + Ki_RateRoll = readEEPROM(KI_RATEROLL_ADR); + Kd_RateRoll = readEEPROM(KD_RATEROLL_ADR); + Kp_RatePitch = readEEPROM(KP_RATEPITCH_ADR); + Ki_RatePitch = readEEPROM(KI_RATEPITCH_ADR); + Kd_RatePitch = readEEPROM(KD_RATEPITCH_ADR); + Kp_RateYaw = readEEPROM(KP_RATEYAW_ADR); + Ki_RateYaw = readEEPROM(KI_RATEYAW_ADR); + Kd_RateYaw = readEEPROM(KD_RATEYAW_ADR); + xmitFactor = readEEPROM(XMITFACTOR_ADR); +} diff --git a/Vector.pde b/Arducopter/Vector.pde similarity index 100% rename from Vector.pde rename to Arducopter/Vector.pde diff --git a/Install.txt b/Install.txt index be714fada9..ae71ab63fb 100644 --- a/Install.txt +++ b/Install.txt @@ -3,9 +3,7 @@ Install notes ------------- To install the libraries: - - copy Library Directories to your \arduino\hardware\lirbaries\ directory + - copy Library Directories to your \arduino\hardware\libraries\ or arduino\libraries directory - Restart arduino IDE Each library comes with a simple example. You can find the examples in menu File->Examples - -This code works with Ardupilot Mega Hardware and Beta sensor shield (FOXTRAP2). \ No newline at end of file diff --git a/libraries/APM_Compass/APM_Compass.cpp b/libraries/APM_Compass/APM_Compass.cpp index 4b893d758e..4cbe5d4eba 100644 --- a/libraries/APM_Compass/APM_Compass.cpp +++ b/libraries/APM_Compass/APM_Compass.cpp @@ -23,7 +23,8 @@ Read() : Read Sensor data To do : Calibration of the sensor, code optimization - Mount position : Big capacitor pointing forward, connector backward + Mount position : UPDATED + Big capacitor pointing backward, connector forward */ extern "C" { @@ -74,8 +75,8 @@ void APM_Compass_Class::Read() if (i==6) // All bytes received? { // MSB byte first, then LSB, X,Y,Z - Mag_X = ((((int)buff[0]) << 8) | buff[1]); // X axis - Mag_Y = -((((int)buff[2]) << 8) | buff[3]); // Y axis + Mag_X = -((((int)buff[0]) << 8) | buff[1]); // X axis + Mag_Y = ((((int)buff[2]) << 8) | buff[3]); // Y axis Mag_Z = -((((int)buff[4]) << 8) | buff[5]); // Z axis } } @@ -105,6 +106,5 @@ void APM_Compass_Class::Calculate(float roll, float pitch) } - // make one instance for the user to use APM_Compass_Class APM_Compass; \ No newline at end of file diff --git a/libraries/APM_RC/APM_RC.cpp b/libraries/APM_RC/APM_RC.cpp index 832897fb2a..9a85b33425 100644 --- a/libraries/APM_RC/APM_RC.cpp +++ b/libraries/APM_RC/APM_RC.cpp @@ -163,5 +163,25 @@ unsigned char APM_RC_Class::GetState(void) return(radio_status); } +// InstantPWM implementation +// This function forces the PWM output (reset PWM) on Out0 and Out1 (Timer5). For quadcopters use +void APM_RC_Class::Force_Out0_Out1(void) +{ + if (TCNT5>5000) // We take care that there are not a pulse in the output + TCNT5=39990; // This forces the PWM output to reset in 5us (10 counts of 0.5us). The counter resets at 40000 +} +// This function forces the PWM output (reset PWM) on Out2 and Out3 (Timer1). For quadcopters use +void APM_RC_Class::Force_Out2_Out3(void) +{ + if (TCNT1>5000) + TCNT1=39990; +} +// This function forces the PWM output (reset PWM) on Out6 and Out7 (Timer3). For quadcopters use +void APM_RC_Class::Force_Out6_Out7(void) +{ + if (TCNT3>5000) + TCNT3=39990; +} + // 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 index c3c50e1867..b3fa06da44 100644 --- a/libraries/APM_RC/APM_RC.h +++ b/libraries/APM_RC/APM_RC.h @@ -14,6 +14,9 @@ class APM_RC_Class void OutputCh(unsigned char ch, int pwm); int InputCh(unsigned char ch); unsigned char GetState(); + void Force_Out0_Out1(void); + void Force_Out2_Out3(void); + void Force_Out6_Out7(void); }; extern APM_RC_Class APM_RC; diff --git a/libraries/APM_RC/keywords.txt b/libraries/APM_RC/keywords.txt index 71a785f71c..3efcc0fa52 100644 --- a/libraries/APM_RC/keywords.txt +++ b/libraries/APM_RC/keywords.txt @@ -3,3 +3,6 @@ begin KEYWORD2 InputCh KEYWORD2 OutputCh KEYWORD2 GetState KEYWORD2 +Force_Out0_Out1 KEYWORD2 +Force_Out2_Out3 KEYWORD2 +Force_Out6_Out7 KEYWORD2