From 7667214987455304f045c7e95b73adbfe73469b9 Mon Sep 17 00:00:00 2001 From: jjulio1234 Date: Mon, 11 Oct 2010 20:43:25 +0000 Subject: [PATCH] Corrected bug on yaw sensor sign git-svn-id: https://arducopter.googlecode.com/svn/trunk@654 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArducopterNG/Arducopter.h | 8 ++++---- ArducopterNG/ArducopterNG.pde | 21 +++++++++++++++++---- ArducopterNG/DCM.pde | 4 ---- 3 files changed, 21 insertions(+), 12 deletions(-) diff --git a/ArducopterNG/Arducopter.h b/ArducopterNG/Arducopter.h index 20c8939699..284a6b0019 100644 --- a/ArducopterNG/Arducopter.h +++ b/ArducopterNG/Arducopter.h @@ -157,7 +157,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 */ @@ -181,8 +181,8 @@ int SENSOR_SIGN[]={ // 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_Y 0.41 //Y axis Gyro gain -#define Gyro_Gain_Z 0.41 //Z axis Gyro gain +#define Gyro_Gain_Y 0.4 //Y axis Gyro gain +#define Gyro_Gain_Z 0.4 //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 @@ -388,4 +388,4 @@ int mainLoop_count = 0; unsigned long elapsedTime = 0; // for doing custom events //unsigned int GPS_timer = 0; - + diff --git a/ArducopterNG/ArducopterNG.pde b/ArducopterNG/ArducopterNG.pde index f3966f5c50..626dbfdce8 100644 --- a/ArducopterNG/ArducopterNG.pde +++ b/ArducopterNG/ArducopterNG.pde @@ -51,6 +51,8 @@ //#define UseBMP //#define BATTERY_EVENT 1 // (boolean) 0 = don't read battery, 1 = read battery voltage (only if you have it wired up!) +#define CONFIGURATOR + // Serial data, do we have FTDI cable or Xbee on Telemetry port as our primary command link #define Ser0 // FTDI/USB Port Either one //#define Ser3 // Telemetry port @@ -197,13 +199,13 @@ void loop() Attitude_control_v3(command_rx_roll,command_rx_pitch,command_rx_yaw); else // Automatic mode : GPS position hold mode Attitude_control_v3(command_rx_roll+command_gps_roll,command_rx_pitch+command_gps_pitch,command_rx_yaw); - } + } else { // ACRO Mode 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); - } + } // Send output commands to motors... motor_output(); @@ -221,7 +223,18 @@ void loop() } #endif #ifdef UseBMP - #endif - } + #endif + // Slow loop (10Hz) + if((millis()-tlmTimer)>=100) { + //#if BATTERY_EVENT == 1 + // read_battery(); // Battery monitor + //#endif + #ifdef CONFIGURATOR + readSerialCommand(); + sendSerialTelemetry(); + #endif + tlmTimer = millis(); + } + } } diff --git a/ArducopterNG/DCM.pde b/ArducopterNG/DCM.pde index cb585ecc13..f3d59a2bde 100644 --- a/ArducopterNG/DCM.pde +++ b/ArducopterNG/DCM.pde @@ -112,10 +112,6 @@ 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 - // Low pass filter on accelerometer data (to filter vibrations) Accel_Vector[0]=Accel_Vector[0]*0.6 + (float)read_adc(3)*0.4; // acc x Accel_Vector[1]=Accel_Vector[1]*0.6 + (float)read_adc(4)*0.4; // acc y