From f0776a962d1c19a12ea65a7bd5fc916487a46d12 Mon Sep 17 00:00:00 2001 From: jphelirc Date: Wed, 18 Aug 2010 01:27:35 +0000 Subject: [PATCH] sercom based channel mid calibrations git-svn-id: https://arducopter.googlecode.com/svn/trunk@224 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- Arducopter/ArduCopter.h | 12 ++++ Arducopter/Arducopter.pde | 27 ++++--- Arducopter/Functions.pde | 146 ++++++++++++++++++++++++++++++++++++++ Arducopter/SerialCom.pde | 22 +++++- Arducopter/UserConfig.h | 6 +- 5 files changed, 197 insertions(+), 16 deletions(-) create mode 100644 Arducopter/Functions.pde diff --git a/Arducopter/ArduCopter.h b/Arducopter/ArduCopter.h index 9e483a6a11..6416759cd0 100644 --- a/Arducopter/ArduCopter.h +++ b/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); + } diff --git a/Arducopter/Arducopter.pde b/Arducopter/Arducopter.pde index 8ff478a7a5..a3562008aa 100644 --- a/Arducopter/Arducopter.pde +++ b/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 diff --git a/Arducopter/Functions.pde b/Arducopter/Functions.pde new file mode 100644 index 0000000000..76a715e1be --- /dev/null +++ b/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 . + */ + +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(','); +} + + + + + + + + + diff --git a/Arducopter/SerialCom.pde b/Arducopter/SerialCom.pde index 2739898381..0db12441bb 100644 --- a/Arducopter/SerialCom.pde +++ b/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(','); + } + */ diff --git a/Arducopter/UserConfig.h b/Arducopter/UserConfig.h index f52a50b337..a04b9132e3 100644 --- a/Arducopter/UserConfig.h +++ b/Arducopter/UserConfig.h @@ -59,10 +59,10 @@ TODO: //#define RADIO_TEST_MODE -#define ROLL_MID 1500 // Radio Roll channel mid value -#define PITCH_MID 1500 // Radio Pitch channel mid value +#define ROLL_MID 1478 // Radio Roll 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