preparing for automatic calibration via CLI

git-svn-id: https://arducopter.googlecode.com/svn/trunk@752 f9c3cf11-9bcb-44bc-f272-b75c42450872

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

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
 

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)

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,          
+ Ken McEwans, Roberto Navoni, Sandro Benigno, Chris Anderson, Randy McEvans
- 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();
+
+
+
+}
+
+

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() {

ArducopterNG/Motors.pde

@@ -23,15 +23,15 @@
  You should have received a copy of the GNU General Public License
  along with this program. If not, see <http://www.gnu.org/licenses/>.
  
-* ************************************************************** *
+ * ************************************************************** *
-ChangeLog:
+ ChangeLog:
  
  
-* ************************************************************** *
+ * ************************************************************** *
-TODO:
+ TODO:
  
  
-* ************************************************************** */
+ * ************************************************************** */
 
 
 
@@ -42,22 +42,22 @@ void motor_output()
     control_yaw = 0;
 
   // Quadcopter mix
-  if (motorArmed == 1) 
+  if (motorArmed == 1) {   
-    {   
+#ifdef IsAM
-  #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
+  } else {    // MOTORS DISARMED
-    {    
+
-  #ifdef IsAM
+#ifdef IsAM
     digitalWrite(FR_LED, LOW);    // AM-Mode
-  #endif
+#endif
     digitalWrite(LED_Green,HIGH); // Ready LED on
 
     rightMotor = MIN_THROTTLE;
@@ -69,6 +69,7 @@ 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);
   }
@@ -79,12 +80,10 @@ void motor_output()
   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();
+  
 }
 
+

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  
+      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);
+    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++)  
+  for(gyro = GYROZ; gyro <= GYROY; gyro++)  
-    AN_OFFSET[gyro]=aux_float[gyro];    // Update sensor OFFSETs from values read
+    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
 

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);