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

@@ -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);
 
-}
+}

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

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 <http://www.gnu.org/licenses/>.
+ */
+
+/* ************************************************************ */
+/* 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;
+}
+
+

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(',');
  }
  */
-
+