From 13f8f8ef11d19e08ccc42a1dc7a6fc0994ccdfd5 Mon Sep 17 00:00:00 2001 From: jjulio1234 Date: Wed, 18 Aug 2010 19:23:29 +0000 Subject: [PATCH] Corrected bug on GPS position hold (added GPS_Dt for D and I terms of the control). Separated navigation.pde file. git-svn-id: https://arducopter.googlecode.com/svn/trunk@231 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- Arducopter/ArduCopter.h | 8 +++-- Arducopter/Arducopter.pde | 61 ++++----------------------------- Arducopter/Navigation.pde | 71 +++++++++++++++++++++++++++++++++++++++ Arducopter/SerialCom.pde | 16 ++++----- 4 files changed, 91 insertions(+), 65 deletions(-) create mode 100644 Arducopter/Navigation.pde diff --git a/Arducopter/ArduCopter.h b/Arducopter/ArduCopter.h index 6416759cd0..2edd33db48 100644 --- a/Arducopter/ArduCopter.h +++ b/Arducopter/ArduCopter.h @@ -145,8 +145,12 @@ float Temporary_Matrix[3][3]={ float speed_3d=0; int GPS_ground_speed=0; -long timer=0; //general porpuse timer +// Main timers +long timer=0; long timer_old; +long GPS_timer; +long GPS_timer_old; +float GPS_Dt=0.2; // GPS Dt // Attitude control variables float command_rx_roll=0; // User commands @@ -408,4 +412,4 @@ void readUserConfig() { pitch_mid = readEEPROM(CHPITCH_MID); yaw_mid = readEEPROM(CHYAW_MID); -} +} diff --git a/Arducopter/Arducopter.pde b/Arducopter/Arducopter.pde index a3562008aa..c3ba3e3932 100644 --- a/Arducopter/Arducopter.pde +++ b/Arducopter/Arducopter.pde @@ -1,5 +1,5 @@ /* ********************************************************************** */ -/* j ArduCopter Quadcopter code */ +/* ArduCopter Quadcopter code */ /* */ /* Quadcopter code from AeroQuad project and ArduIMU quadcopter project */ /* IMU DCM code from Diydrones.com */ @@ -88,58 +88,6 @@ // Normal users does not need to edit anything below these lines. If you have // need, go and change them in FrameConfig.h -/* ************************************************************ */ -/* Altitude control... (based on sonar) */ -void Altitude_control(int target_sonar_altitude) -{ - err_altitude_old = err_altitude; - err_altitude = target_sonar_altitude - Sonar_value; - altitude_D = (float)(err_altitude - err_altitude_old) / G_Dt; - altitude_I += (float)err_altitude * G_Dt; - altitude_I = constrain(altitude_I, -100, 100); - command_altitude = Initial_Throttle + KP_ALTITUDE * err_altitude + KD_ALTITUDE * altitude_D + KI_ALTITUDE * altitude_I; -} - -/* ************************************************************ */ -/* Position control... */ -void Position_control(long lat_dest, long lon_dest) -{ - long Lon_diff; - long Lat_diff; - float gps_err_roll; - float gps_err_pitch; - - Lon_diff = lon_dest - GPS.Longitude; - Lat_diff = lat_dest - GPS.Lattitude; - - // ROLL - gps_err_roll_old = gps_err_roll; - //Optimization : cos(yaw) = DCM_Matrix[0][0] ; sin(yaw) = DCM_Matrix[1][0] - gps_err_roll = (float)Lon_diff*GEOG_CORRECTION_FACTOR*DCM_Matrix[0][0] - (float)Lat_diff*DCM_Matrix[1][0]; - - gps_roll_D = (gps_err_roll-gps_err_roll_old)/G_Dt; - - gps_roll_I += gps_err_roll*G_Dt; - gps_roll_I = constrain(gps_roll_I,-500,500); - - command_gps_roll = KP_GPS_ROLL*gps_err_roll + KD_GPS_ROLL*gps_roll_D + KI_GPS_ROLL*gps_roll_I; - command_gps_roll = constrain(command_gps_roll,-GPS_MAX_ANGLE,GPS_MAX_ANGLE); // Limit max command - - // PITCH - gps_err_pitch_old = gps_err_pitch; - gps_err_pitch = -(float)Lat_diff*DCM_Matrix[0][0]- (float)Lon_diff*GEOG_CORRECTION_FACTOR*DCM_Matrix[1][0]; - - gps_pitch_D = (gps_err_pitch-gps_err_pitch_old)/G_Dt; - - gps_pitch_I += gps_err_pitch*G_Dt; - gps_pitch_I = constrain(gps_pitch_I,-500,500); - - command_gps_pitch = KP_GPS_PITCH*gps_err_pitch + KD_GPS_PITCH*gps_pitch_D + KI_GPS_PITCH*gps_pitch_I; - command_gps_pitch = constrain(command_gps_pitch,-GPS_MAX_ANGLE,GPS_MAX_ANGLE); // Limit max command -} - - - /* ************************************************************ */ // STABLE MODE // ROLL, PITCH and YAW PID controls... @@ -448,7 +396,7 @@ void loop(){ counter++; timer_old = timer; timer=millis(); - G_Dt = (timer-timer_old)/1000.0; // Real time of loop run + G_Dt = (timer-timer_old)*0.001; // Real time of loop run // IMU DCM Algorithm Read_adc_raw(); @@ -574,6 +522,9 @@ void loop(){ GPS.Read(); if (GPS.NewData) // New GPS data? { + GPS_timer_old=GPS_timer; // Update GPS timer + GPS_timer = timer; + GPS_Dt = (GPS_timer-GPS_timer_old)*0.001; // GPS_Dt GPS.NewData=0; // We Reset the flag... //Output GPS data @@ -727,4 +678,4 @@ void loop(){ } - + diff --git a/Arducopter/Navigation.pde b/Arducopter/Navigation.pde new file mode 100644 index 0000000000..abc1844359 --- /dev/null +++ b/Arducopter/Navigation.pde @@ -0,0 +1,71 @@ +/* + ArduCopter v1.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 . + */ + +/* ************************************************************ */ +/* GPS based Position control */ +void Position_control(long lat_dest, long lon_dest) +{ + long Lon_diff; + long Lat_diff; + float gps_err_roll; + float gps_err_pitch; + + Lon_diff = lon_dest - GPS.Longitude; + Lat_diff = lat_dest - GPS.Lattitude; + + // ROLL + gps_err_roll_old = gps_err_roll; + //Optimization : cos(yaw) = DCM_Matrix[0][0] ; sin(yaw) = DCM_Matrix[1][0] + gps_err_roll = (float)Lon_diff*GEOG_CORRECTION_FACTOR*DCM_Matrix[0][0] - (float)Lat_diff*DCM_Matrix[1][0]; + + gps_roll_D = (gps_err_roll-gps_err_roll_old)/GPS_Dt; + + gps_roll_I += gps_err_roll*GPS_Dt; + gps_roll_I = constrain(gps_roll_I,-800,800); + + command_gps_roll = KP_GPS_ROLL*gps_err_roll + KD_GPS_ROLL*gps_roll_D + KI_GPS_ROLL*gps_roll_I; + command_gps_roll = constrain(command_gps_roll,-GPS_MAX_ANGLE,GPS_MAX_ANGLE); // Limit max command + + // PITCH + gps_err_pitch_old = gps_err_pitch; + gps_err_pitch = -(float)Lat_diff*DCM_Matrix[0][0]- (float)Lon_diff*GEOG_CORRECTION_FACTOR*DCM_Matrix[1][0]; + + gps_pitch_D = (gps_err_pitch-gps_err_pitch_old)/GPS_Dt; + + gps_pitch_I += gps_err_pitch*GPS_Dt; + gps_pitch_I = constrain(gps_pitch_I,-800,800); + + command_gps_pitch = KP_GPS_PITCH*gps_err_pitch + KD_GPS_PITCH*gps_pitch_D + KI_GPS_PITCH*gps_pitch_I; + command_gps_pitch = constrain(command_gps_pitch,-GPS_MAX_ANGLE,GPS_MAX_ANGLE); // Limit max command +} + +/* ************************************************************ */ +/* Altitude control... (based on sonar) // NOT TESTED!!*/ +void Altitude_control(int target_sonar_altitude) +{ + err_altitude_old = err_altitude; + err_altitude = target_sonar_altitude - Sonar_value; + altitude_D = (float)(err_altitude - err_altitude_old) / G_Dt; + altitude_I += (float)err_altitude * G_Dt; + altitude_I = constrain(altitude_I, -100, 100); + command_altitude = Initial_Throttle + KP_ALTITUDE * err_altitude + KD_ALTITUDE * altitude_D + KI_ALTITUDE * altitude_I; +} + + diff --git a/Arducopter/SerialCom.pde b/Arducopter/SerialCom.pde index 0db12441bb..d86e7a1bb6 100644 --- a/Arducopter/SerialCom.pde +++ b/Arducopter/SerialCom.pde @@ -144,13 +144,13 @@ void readSerialCommand() { KD_QUAD_YAW = 1.2; KI_QUAD_YAW = 0.15; STABLE_MODE_KP_RATE = 0.2; // New param for stable mode - KP_GPS_ROLL = 0.012; - KD_GPS_ROLL = 0.005; - KI_GPS_ROLL = 0.004; - KP_GPS_PITCH = 0.012; - KD_GPS_PITCH = 0.005; - KI_GPS_PITCH = 0.004; - GPS_MAX_ANGLE = 10; + KP_GPS_ROLL = 0.02; + KD_GPS_ROLL = 0.006; + KI_GPS_ROLL = 0.008; + KP_GPS_PITCH = 0.02; + KD_GPS_PITCH = 0.006; + KI_GPS_PITCH = 0.008; + GPS_MAX_ANGLE = 18; KP_ALTITUDE = 0.8; KD_ALTITUDE = 0.2; KI_ALTITUDE = 0.2; @@ -441,4 +441,4 @@ void comma() { Serial.print(','); } */ - +