sercom based channel mid calibrations

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

Arducopter/ArduCopter.h

@@ -19,6 +19,7 @@
 */
 
 #include "WProgram.h"
+#include "UserConfig.h"
 
 /*******************************************************************/
 // ArduPilot Mega specific hardware and software settings
@@ -214,6 +215,10 @@ int num_iter;
 float aux_debug;
 
 // Radio definitions
+int roll_mid;
+int pitch_mid;
+int yaw_mid;
+
 int Neutro_yaw;
 int ch_roll;
 int ch_pitch;
@@ -329,6 +334,9 @@ float xmitFactor;
 #define KP_RATEYAW_ADR 156
 #define KI_RATEYAW_ADR 160
 #define KD_RATEYAW_ADR 164
+#define CHROLL_MID 168
+#define CHPITCH_MID 172
+#define CHYAW_MID 176
 
 // Utilities for writing and reading from the EEPROM
 float readEEPROM(int address) {
@@ -396,4 +404,8 @@ void readUserConfig() {
   Ki_RateYaw = readEEPROM(KI_RATEYAW_ADR);
   Kd_RateYaw = readEEPROM(KD_RATEYAW_ADR);
   xmitFactor = readEEPROM(XMITFACTOR_ADR);
+  roll_mid = readEEPROM(CHROLL_MID);
+  pitch_mid = readEEPROM(CHPITCH_MID);
+  yaw_mid = readEEPROM(CHYAW_MID);
+
 }

Arducopter/Arducopter.pde

@@ -161,7 +161,7 @@ void Attitude_control_v2()
 
   // New control term...
   roll_rate = ToDeg(Omega[0]);  // Omega[] is the raw gyro reading plus Omega_I, so it´s bias corrected
-  err_roll_rate = ((ch_roll - ROLL_MID) >> 1) - roll_rate;
+  err_roll_rate = ((ch_roll - roll_mid) >> 1) - roll_rate;
 
   roll_I += err_roll * G_Dt;
   roll_I = constrain(roll_I, -20, 20);
@@ -184,7 +184,7 @@ void Attitude_control_v2()
 
   // New control term...
   pitch_rate = ToDeg(Omega[1]);
-  err_pitch_rate = ((ch_pitch - PITCH_MID) >> 1) - pitch_rate;
+  err_pitch_rate = ((ch_pitch - pitch_mid) >> 1) - pitch_rate;
 
   pitch_I += err_pitch*G_Dt;
   pitch_I = constrain(pitch_I,-20,20);
@@ -221,7 +221,7 @@ void Rate_control()
   // ROLL CONTROL
   currentRollRate = read_adc(0);      // I need a positive sign here
 
-  err_roll = ((ch_roll - ROLL_MID) * xmitFactor) - currentRollRate;
+  err_roll = ((ch_roll - roll_mid) * xmitFactor) - currentRollRate;
 
   roll_I += err_roll * G_Dt;
   roll_I = constrain(roll_I, -20, 20);
@@ -234,7 +234,7 @@ void Rate_control()
 
   // PITCH CONTROL
   currentPitchRate = read_adc(1);
-  err_pitch = ((ch_pitch - PITCH_MID) * xmitFactor) - currentPitchRate;
+  err_pitch = ((ch_pitch - pitch_mid) * xmitFactor) - currentPitchRate;
 
   pitch_I += err_pitch*G_Dt;
   pitch_I = constrain(pitch_I,-20,20);
@@ -247,7 +247,7 @@ void Rate_control()
 
   // YAW CONTROL
   currentYawRate = read_adc(2);
-  err_yaw = ((ch_yaw - YAW_MID) * xmitFactor) - currentYawRate;
+  err_yaw = ((ch_yaw - yaw_mid) * xmitFactor) - currentYawRate;
 
   yaw_I += err_yaw*G_Dt;
   yaw_I = constrain(yaw_I,-20,20);
@@ -316,6 +316,12 @@ void setup()
 #endif
 
   readUserConfig(); // Load user configurable items from EEPROM
+  
+  // Safety measure for Channel mids
+  if(roll_mid < 1400 || roll_mid > 1600) roll_mid = 1500;
+  if(pitch_mid < 1400 || pitch_mid > 1600) pitch_mid = 1500;
+  if(yaw_mid < 1400 || yaw_mid > 1600) yaw_mid = 1500;
+
 
   // RC channels Initialization (Quad motors)  
   APM_RC.OutputCh(0,MIN_THROTTLE);  // Motors stoped
@@ -342,7 +348,6 @@ void setup()
     delay(30000);
   }
 
-
   Read_adc_raw();
   delay(20);
 
@@ -371,7 +376,7 @@ 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
+//  Neutro_yaw = APM_RC.InputCh(3); // Take yaw neutral radio value
 #ifndef CONFIGURATOR
   for(i=0;i<6;i++)
   {
@@ -380,10 +385,11 @@ void setup()
   }
 
   Serial.print("Yaw neutral value:");
-  Serial.println(Neutro_yaw);
+//  Serial.println(Neutro_yaw);
+  Serial.print(yaw_mid);
 #endif
 
-#if (RADIO_TEST_MODE)    // RADIO TEST MODE TO TEST RADIO CHANNELS
+#ifdef RADIO_TEST_MODE    // RADIO TEST MODE TO TEST RADIO CHANNELS
   while(1)
   {
     if (APM_RC.GetState()==1)
@@ -502,7 +508,8 @@ void loop(){
       command_rx_pitch_old = command_rx_pitch;
       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;
+//      aux_float = (ch_yaw-Neutro_yaw) / 180.0;
+      aux_float = (ch_yaw-yaw_mid) / 180.0;
       command_rx_yaw += aux_float;
       command_rx_yaw_diff = aux_float;
       if (command_rx_yaw > 180)         // Normalize yaw to -180,180 degrees

Arducopter/Functions.pde

@@ -0,0 +1,146 @@
+/*
+ ArduCopter 1.3 - Aug 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 <http://www.gnu.org/licenses/>. 
+ */
+
+void RadioCalibration() {
+  long command_timer;
+  int command; 
+  int counter = 5;
+  boolean Cmd_ok; 
+  long roll_new = 0;
+  long pitch_new = 0;
+  long yaw_new = 0;
+
+  Serial.flush();
+  Serial.println("Entering Radio Calibration mode");
+  Serial.println("Current channel MID values are:");
+  Serial.print("ROLL: ");
+  Serial.print(roll_mid);
+  Serial.print(" PITCH: ");
+  Serial.print(pitch_mid);
+  Serial.print(" YAW: ");
+  Serial.print(yaw_mid);
+  Serial.println();
+  Serial.println("Recalibrate Channel MID's [Y/N]?: ");
+  command_timer = millis();
+
+  // Start counter loop and wait serial input. If not input for 5 seconds, return to normal mode
+  while(millis() - command_timer < 5000) {
+    if (Serial.available()) {
+      queryType = Serial.read();
+      if(queryType == 'y' || queryType == 'Y') {  
+        Cmd_ok = TRUE;
+        break;    
+      } 
+      else {
+        Cmd_ok = FALSE;     
+        break;
+      }
+    }
+  }
+  if(Cmd_ok) {
+    // We have a go. Let's do new calibration
+    Serial.println("Starting calibration run in 5 seconds. Place all sticks to their middle including trims");
+    for(counter = 5; counter >= 0; counter --) {
+      command_timer = millis();
+      while(millis() - command_timer < 1000) {
+      }
+      Serial.println(counter);
+    }
+    // Do actual calibration now
+    Serial.println("Measuring average channel values");
+    Serial.println("ROLL, PITCH, YAW");
+
+    counter = 0; // Reset counter for just in case. 
+    command_timer = millis();
+    while(millis() - command_timer < 1000) {
+
+      if (APM_RC.GetState()==1) {  // New radio frame?
+        // Commands from radio Rx... 
+        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);
+
+        Serial.print(ch_roll);
+        comma();
+        Serial.print(ch_pitch);
+        comma();
+        Serial.print(ch_yaw);
+        Serial.println();
+        roll_new += ch_roll;
+        pitch_new += ch_pitch; 
+        yaw_new += ch_yaw;
+        counter++;
+      }
+    }
+    Serial.print("New samples received: ");
+    Serial.println(counter);    
+    roll_new = roll_new / counter;
+    pitch_new = pitch_new / counter;
+    yaw_new = yaw_new / counter;
+    Serial.print("New values as: ");
+    Serial.print("ROLL: ");
+    Serial.print(roll_new);
+    Serial.print(" PITCH: ");
+    Serial.print(pitch_new);
+    Serial.print(" YAW: ");
+    Serial.print(yaw_new);
+    Serial.println();
+    Serial.println("Accept & Save values [Y/N]?: ");
+    Cmd_ok = FALSE;
+    while(millis() - command_timer < 5000) {
+      if (Serial.available()) {
+        queryType = Serial.read();
+        if(queryType == 'y' || queryType == 'Y') { 
+          Cmd_ok = TRUE;
+          roll_mid = roll_new;
+          pitch_mid = pitch_new;
+          yaw_mid = yaw_new;
+          Serial.println("Values accepted, remember to save them to EEPROM with 'W' command");
+          break;    
+        } 
+        else {
+          Cmd_ok = TRUE;
+          break;
+        }
+      }   
+    } 
+  } 
+  if(queryType == 'n' || queryType == 'N') Cmd_ok = TRUE;
+  if(Cmd_ok)
+    Serial.println("Returning normal mode...");
+  else Serial.println("Command timeout, returning normal mode....");
+}
+
+
+void comma() {
+  Serial.print(',');
+}
+
+
+
+
+
+
+
+
+

Arducopter/SerialCom.pde

@@ -129,6 +129,9 @@ void readSerialCommand() {
       writeEEPROM(Ki_RateYaw, KI_RATEYAW_ADR);
       writeEEPROM(Kd_RateYaw, KD_RATEYAW_ADR);
       writeEEPROM(xmitFactor, XMITFACTOR_ADR);
+      writeEEPROM(roll_mid, CHROLL_MID);
+      writeEEPROM(pitch_mid, CHPITCH_MID);
+      writeEEPROM(yaw_mid, CHYAW_MID);
       break;
     case 'Y': // Initialize EEPROM with default values
       KP_QUAD_ROLL = 1.8;
@@ -173,6 +176,9 @@ void readSerialCommand() {
       Ki_RateYaw = 0.3;
       Kd_RateYaw = 0;
       xmitFactor = 0.8;
+      roll_mid = 1500;
+      pitch_mid = 1500;
+      yaw_mid = 1500;
       break;
     }
   }
@@ -289,6 +295,7 @@ void sendSerialTelemetry() {
     queryType = 'X';
     break;
   case 'L': // Spare
+    RadioCalibration();
     queryType = 'X';
     break;
   case 'N': // Send magnetometer config
@@ -385,7 +392,14 @@ void sendSerialTelemetry() {
     comma();
     Serial.print(ch_aux); // AUX1 (Mode)
     comma();
-    Serial.println(ch_aux2); // AUX2  
+    Serial.print(ch_aux2); // AUX2 
+    comma();
+    Serial.print(roll_mid); // Roll MID value
+    comma();
+    Serial.print(pitch_mid); // Pitch MID value
+    comma();
+    Serial.println(yaw_mid); // Yaw MID Value
+
     break;
   case 'V': // Spare
     break;
@@ -422,7 +436,9 @@ float readFloatSerial() {
   return atof(data);
 }
 
+/*
 void comma() {
-  Serial.print(',');
+ Serial.print(',');
-}
+ }
+ */
 

Arducopter/UserConfig.h

@@ -59,10 +59,10 @@ TODO:
 
 //#define RADIO_TEST_MODE
 
-#define ROLL_MID 1500    // Radio Roll channel mid value
+#define ROLL_MID 1478    // Radio Roll channel mid value
-#define PITCH_MID 1500    // Radio Pitch channel mid value
+#define PITCH_MID 1483    // Radio Pitch channel mid value
 #define YAW_MID 1500    // Radio Yaw channel mid value
-#define THROTTLE_MID 1500    // Radio Throttle channel mid value
+#define THROTTLE_MID 1502    // Radio Throttle channel mid value
 #define AUX_MID 1500
 
 #define CHANN_CENTER 1500       // Channel center, legacy