From 4a07a3c09ddd17336620c435bb858fb44577721e Mon Sep 17 00:00:00 2001 From: jphelirc Date: Sat, 30 Oct 2010 08:30:46 +0000 Subject: [PATCH] preparing for automatic calibration via CLI git-svn-id: https://arducopter.googlecode.com/svn/trunk@752 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArducopterNG/ArduUser.h | 6 ++++ ArducopterNG/Arducopter.h | 6 ++++ ArducopterNG/ArducopterNG.pde | 3 +- ArducopterNG/CLI.pde | 65 +++++++++++++++++++++++++++++++++-- ArducopterNG/GCS.pde | 3 ++ ArducopterNG/Motors.pde | 55 +++++++++++++++-------------- ArducopterNG/Sensors.pde | 16 +++++---- ArducopterNG/System.pde | 15 ++++---- 8 files changed, 124 insertions(+), 45 deletions(-) diff --git a/ArducopterNG/ArduUser.h b/ArducopterNG/ArduUser.h index ec8324e52a..ae15a0fc7a 100644 --- a/ArducopterNG/ArduUser.h +++ b/ArducopterNG/ArduUser.h @@ -59,6 +59,11 @@ TODO: #define RI_LED AN10 // Mega PH4 pin, OUT5 #define LE_LED AN8 // Mega PH5 pin, OUT4 +#define GPS_AM_PAT1 L\0x00\0x00\0x05 +#define GPS_AM_PAT2 L\0xFF\0xFF\0x05 +#define GPS_AM_PAT3 L\0xF0\0xF0\0x05 + + /* AM PIN Definitions - END */ /*************************************************************/ @@ -86,6 +91,7 @@ TODO: #define MIN_THROTTLE 1040 // Throttle pulse width at minimun... #define CAM_CENT 1500 // Camera center +#define CAM_SMOOTHING 1000 // Camera movement smoothing /*************************************************************/ // General definitions diff --git a/ArducopterNG/Arducopter.h b/ArducopterNG/Arducopter.h index f1a5df3bbd..bdf7990b60 100644 --- a/ArducopterNG/Arducopter.h +++ b/ArducopterNG/Arducopter.h @@ -110,6 +110,7 @@ TODO: //#define SerBau 115200 // Baud setting moved close next to port selection #define SerPri Serial3.print #define SerPrln Serial3.println +#define SerPriln Serial3.println #define SerRea Serial3.read #define SerAva Serial3.available #define SerRea Serial3.read @@ -122,6 +123,7 @@ TODO: //#define SerBau 115200 // Baud setting moved close next to port selection #define SerPri Serial.print #define SerPrln Serial.println +#define SerPriln Serial.println #define SerRea Serial.read #define SerAva Serial.available #define SerRea Serial.read @@ -156,6 +158,10 @@ int SENSOR_SIGN[]={ #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 ROLL_DEF 0 // Level values for roll, used to calculate roll_acc_offset +#define PITCH_DEF 0 // Level values for pitch, used to calculate pitch_acc_offset +#define Z_DEF GRAVITY // Stable level value for Z, used to calculate z_acc_offset, same as GRAVITY + #define ToRad(x) (x*0.01745329252) // *pi/180 #define ToDeg(x) (x*57.2957795131) // *180/pi diff --git a/ArducopterNG/ArducopterNG.pde b/ArducopterNG/ArducopterNG.pde index 8a69b6fa7b..b9cef48dea 100644 --- a/ArducopterNG/ArducopterNG.pde +++ b/ArducopterNG/ArducopterNG.pde @@ -251,7 +251,8 @@ void loop() // Send output commands to motor ESCs... motor_output(); -// camera_output(); + // Do we have cameras stabilization connected and in use? + if(!SW_DIP2) camera_output(); // Autopilot mode functions if (AP_mode == AP_AUTOMATIC_MODE) diff --git a/ArducopterNG/CLI.pde b/ArducopterNG/CLI.pde index 55c6b23e37..b0408db0ef 100644 --- a/ArducopterNG/CLI.pde +++ b/ArducopterNG/CLI.pde @@ -7,8 +7,7 @@ Version : v1.0, Oct 18, 2010 Author(s): ArduCopter Team Jani Hirvinen, Jose Julio, Jordi Muñoz, - Ted Carancho (aeroquad), Ken McEwans, Roberto Navoni, - Sandro Benigno, Chris Anderson, Randy McEvans + Ken McEwans, Roberto Navoni, Sandro Benigno, Chris Anderson, Randy McEvans 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 @@ -40,6 +39,10 @@ boolean ShowMainMenu; // This can be moved later to CLI.pde void RunCLI () { +// APM_RC.Init(); // APM Radio initialization + + readUserConfig(); // Read memory values from EEPROM + ShowMainMenu = TRUE; // We need to initialize Serial again due it was not initialized during startup. SerBeg(SerBau); @@ -61,7 +64,9 @@ void RunCLI () { case 'c': CALIB_CompassOffset(); break; - + case 'i': + CALIB_AccOffset(); + break; } } } // Mainloop ends @@ -73,6 +78,7 @@ void Show_MainMenu() { SerPrln("CLI Menu - Type your command on command prompt"); SerPrln("----------------------------------------------"); SerPrln(" c - Show compass offsets (no return, reboot)"); + SerPrln(" i - Initialize and calibrate Accel offsets"); SerPrln(" "); } @@ -162,6 +168,59 @@ void CALIB_CompassOffset() { } +/* ************************************************************** */ +// Accell calibration +void CALIB_AccOffset() { + + uint8_t loopy; + uint16_t xx = 0, xy = 0, xz = 0; + + APM_ADC.Init(); // APM ADC library initialization +// delay(250); // Giving small moment before starting + + calibrateSensors(); // Calibrate neutral values of gyros (in Sensors.pde) + + SerPrln(); + SerPrln("Sampling 10 samples from Accelerometers, don't move your ArduCopter!"); + SerPrln("Sample:\tAcc-X\tAxx-Y\tAcc-Z"); + + for(loopy = 1; loopy <= 5; loopy++) { + SerPri(" "); + SerPri(loopy); + SerPri(":"); + tab(); +/* SerPri(xx += read_adc(4)); + tab(); + SerPri(xy += -read_adc(3)); + tab(); + SerPrln(xz += read_adc(5)); +*/ + SerPri(xx += APM_ADC.Ch(4)); + tab(); + SerPri(xy += APM_ADC.Ch(5)); + tab(); + SerPrln(xz += APM_ADC.Ch(3)); + } + + xx = xx / (loopy - 1); + xy = xy / (loopy - 1); + xz = xz / (loopy - 1) ; + + SerPriln("Averages as follows"); + SerPri(" "); + tab(); + SerPri(xx); + tab(); + SerPri(xy); + tab(); + SerPri(xz); + SerPriln(); + + + +} + + diff --git a/ArducopterNG/GCS.pde b/ArducopterNG/GCS.pde index 9c8574a6a6..106d94e0b3 100644 --- a/ArducopterNG/GCS.pde +++ b/ArducopterNG/GCS.pde @@ -435,6 +435,9 @@ void comma() { SerPri(','); } +void tab() { + SerPri("\t"); +} // Used to read floating point values from the serial port float readFloatSerial() { diff --git a/ArducopterNG/Motors.pde b/ArducopterNG/Motors.pde index a3033911fc..e7a2068dd2 100644 --- a/ArducopterNG/Motors.pde +++ b/ArducopterNG/Motors.pde @@ -6,9 +6,9 @@ File : Motors.pde Version : v1.0, Aug 27, 2010 Author(s): ArduCopter Team - Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz, - Jani Hirvinen, Ken McEwans, Roberto Navoni, - Sandro Benigno, Chris Anderson + Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz, + Jani Hirvinen, Ken McEwans, Roberto Navoni, + Sandro Benigno, Chris Anderson 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 @@ -22,16 +22,16 @@ You should have received a copy of the GNU General Public License along with this program. If not, see . - -* ************************************************************** * -ChangeLog: - - -* ************************************************************** * -TODO: - - -* ************************************************************** */ + + * ************************************************************** * + ChangeLog: + + + * ************************************************************** * + TODO: + + + * ************************************************************** */ @@ -40,24 +40,24 @@ void motor_output() { if (ch_throttle < (MIN_THROTTLE + 100)) // If throttle is low we disable yaw (neccesary to arm/disarm motors safely) control_yaw = 0; - + // Quadcopter mix - if (motorArmed == 1) - { - #ifdef IsAM + if (motorArmed == 1) { +#ifdef IsAM digitalWrite(FR_LED, HIGH); // AM-Mode - #endif +#endif + // Quadcopter output mix rightMotor = constrain(ch_throttle - control_roll + control_yaw, minThrottle, 2000); leftMotor = constrain(ch_throttle + control_roll + control_yaw, minThrottle, 2000); frontMotor = constrain(ch_throttle + control_pitch - control_yaw, minThrottle, 2000); backMotor = constrain(ch_throttle - control_pitch - control_yaw, minThrottle, 2000); - } - else // MOTORS DISARMED - { - #ifdef IsAM + + } else { // MOTORS DISARMED + +#ifdef IsAM digitalWrite(FR_LED, LOW); // AM-Mode - #endif +#endif digitalWrite(LED_Green,HIGH); // Ready LED on rightMotor = MIN_THROTTLE; @@ -69,22 +69,21 @@ void motor_output() roll_I = 0; // reset I terms of PID controls pitch_I = 0; yaw_I = 0; + // Initialize yaw command to actual yaw when throttle is down... command_rx_yaw = ToDeg(yaw); - } + } // Send commands to motors APM_RC.OutputCh(0, rightMotor); APM_RC.OutputCh(1, leftMotor); APM_RC.OutputCh(2, frontMotor); APM_RC.OutputCh(3, backMotor); - - // Do we have cameras stabilization connected and in use? - if(SW_DIP2) camera_output(); - // InstantPWM => Force inmediate output on PWM signals APM_RC.Force_Out0_Out1(); APM_RC.Force_Out2_Out3(); + } + diff --git a/ArducopterNG/Sensors.pde b/ArducopterNG/Sensors.pde index 16a2e4c981..b09d6564b3 100644 --- a/ArducopterNG/Sensors.pde +++ b/ArducopterNG/Sensors.pde @@ -66,9 +66,11 @@ void calibrateSensors(void) { { Read_adc_raw(); // Read sensors for(gyro=GYROZ; gyro<=GYROY; gyro++) - aux_float[gyro]=aux_float[gyro]*0.8 + AN[gyro]*0.2; // Filtering - Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],0); + aux_float[gyro] = aux_float[gyro] * 0.8 + AN[gyro] * 0.2; // Filtering + Log_Write_Sensor(AN[0], AN[1], AN[2], AN[3], AN[4], AN[5], 0); + delay(5); + RunningLights(j); // (in Functions.pde) // Runnings lights effect to let user know that we are taking mesurements if((i % 5) == 0) j++; @@ -78,8 +80,8 @@ void calibrateSensors(void) { // Switch off all ABC lights LightsOff(); - for(gyro=GYROZ; gyro<=GYROY; gyro++) - AN_OFFSET[gyro]=aux_float[gyro]; // Update sensor OFFSETs from values read + for(gyro = GYROZ; gyro <= GYROY; gyro++) + AN_OFFSET[gyro] = aux_float[gyro]; // Update sensor OFFSETs from values read } #ifdef UseBMP @@ -91,10 +93,10 @@ void read_baro(void) //tempPresAlt = pow(tempPresAlt, 0.190284); //press_alt = (1.0 - tempPresAlt) * 145366.45; tempPresAlt = pow(tempPresAlt, 0.190295); - if (press_alt==0) + if (press_alt == 0) press_alt = (1.0 - tempPresAlt) * 4433000; // Altitude in cm else - press_alt = press_alt*0.9 + ((1.0 - tempPresAlt) * 443300); // Altitude in cm (filtered) + press_alt = press_alt * 0.9 + ((1.0 - tempPresAlt) * 443300); // Altitude in cm (filtered) } #endif - + diff --git a/ArducopterNG/System.pde b/ArducopterNG/System.pde index 1a1f02a5b8..517ad7fe30 100644 --- a/ArducopterNG/System.pde +++ b/ArducopterNG/System.pde @@ -51,11 +51,6 @@ void APM_Init() { APMPinMode(DDRL,6,INPUT); // DIP3, (PL6) APMPinMode(DDRL,7,INPUT); // DIP4, (PL7), Furthest DIP from sliding SW2 switch - // Is CLI mode active or not, if it is fire it up and never return. - if(digitalRead(SW2)) { - RunCLI(); - // Btw.. We never return from this.... - } /* ********************************************************* */ ///////////////////////////////////////////////////////// @@ -93,6 +88,14 @@ void APM_Init() { #endif #endif + + // Is CLI mode active or not, if it is fire it up and never return. + if(digitalRead(SW2)) { + RunCLI(); + // Btw.. We never return from this.... + } + + // Read DIP Switches and other important values. DIP switches needs special functions to // read due they are not defined as normal pins like other GPIO's are. SW_DIP1 = APMPinRead(PINE, 7); @@ -143,7 +146,7 @@ void APM_Init() { // 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)==0) + while (digitalRead(SW1)==0) // LEGACY remove soon by jp, 30-10-10 { Serial.println("Entering Log Read Mode..."); // This will be obsole soon due moving to CLI system Log_Read(1,1000);