mirror of https://github.com/ArduPilot/ardupilot
sercom based channel mid calibrations
git-svn-id: https://arducopter.googlecode.com/svn/trunk@224 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jphelirc
parent
5e976861e8
commit
f0776a962d
|
|
@ -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);
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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(',');
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -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(',');
|
||||
}
|
||||
*/
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue