gps hold and baro updates

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

ArducopterNG/Arducopter.h

@@ -288,10 +288,12 @@ int err_altitude_old;
 float command_altitude;
 float altitude_I;
 float altitude_D;
+int ch_throttle_altitude_hold;
 
 //Pressure Sensor variables
-long target_baro_altitude;
+long target_baro_altitude;         // Altitude in cm
 long 	press_alt			= 0;
+byte    Baro_new_data                   = 0;
 
 
 #define BATTERY_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*VOLT_DIV_RATIO

ArducopterNG/ArducopterNG.pde

@@ -60,7 +60,7 @@
 //#define IsCAM       // Do we have camera stabilization in use, If you activate, check OUTPUT pins from ArduUser.h
 
 //#define UseAirspeed  // Quads don't use AirSpeed... Legacy, jp 19-10-10
-//#define UseBMP       // Use pressure sensor
+#define UseBMP       // Use pressure sensor
 //#define BATTERY_EVENT 1   // (boolean) 0 = don't read battery, 1 = read battery voltage (only if you have it _wired_ up!)
 
 #define CONFIGURATOR
@@ -314,6 +314,15 @@ void loop()
             }
         }
         #endif
+        #ifdef UseBMP
+        if (Baro_new_data)   // New altitude data?
+          {
+          ch_throttle_altitude_hold = Altitude_control_baro(press_alt,target_baro_altitude);   // Altitude control
+          Baro_new_data=0;
+          Serial.println(ch_throttle_altitude_hold);
+          }
+        ch_throttle = ch_throttle_altitude_hold;
+        #endif
       }
       else   // First time we enter in GPS position hold we capture the target position as the actual position
       {
@@ -322,14 +331,14 @@ void loop()
           target_lattitude = GPS.Lattitude;
           target_longitude = GPS.Longitude;
           target_position=1;
-          //target_sonar_altitude = sonar_value;
-          target_baro_altitude = press_alt;
-          Initial_Throttle = ch_throttle;
-          Reset_I_terms_navigation();  // Reset I terms (in Navigation.pde)
         }
         #endif
         command_gps_roll=0;
         command_gps_pitch=0;
+        target_baro_altitude = press_alt;
+        Initial_Throttle = ch_throttle;
+        ch_throttle_altitude_hold = ch_throttle;
+        Reset_I_terms_navigation();  // Reset I terms (in Navigation.pde)
       }
     }
     else
@@ -384,6 +393,8 @@ void loop()
       if (APM_BMP085.Read()){
         read_baro();
         Baro_new_data = 1;
+        //Serial.print("B ");
+        //Serial.println(press_alt);
       }
 #endif
       break;

ArducopterNG/Navigation.pde

@@ -106,11 +106,11 @@ void Reset_I_terms_navigation()
 /* Altitude control... (based on barometer) */
 int Altitude_control_baro(int altitude, int target_altitude)
 { 
-  #define ALTITUDE_CONTROL_BARO_OUTPUT_MIN 50
-  #define ALTITUDE_CONTROL_BARO_OUTPUT_MAX 90
+  #define ALTITUDE_CONTROL_BARO_OUTPUT_MIN 40
+  #define ALTITUDE_CONTROL_BARO_OUTPUT_MAX 80
   
-  #define KP_BARO_ALTITUDE 0.65
-  #define KD_BARO_ALTITUDE 0.05
+  #define KP_BARO_ALTITUDE 0.25  //0.3
+  #define KD_BARO_ALTITUDE 0.09 //0.09
   #define KI_BARO_ALTITUDE 0.1
   
   int command_altitude;
@@ -119,7 +119,7 @@ int Altitude_control_baro(int altitude, int target_altitude)
   err_altitude = target_altitude - altitude;  
   altitude_D = (float)(err_altitude-err_altitude_old)/0.05;  // 20Hz
   altitude_I += (float)err_altitude*0.05;
-  altitude_I = constrain(altitude_I,-100,100);
+  altitude_I = constrain(altitude_I,-120,120);
   command_altitude = KP_BARO_ALTITUDE*err_altitude + KD_BARO_ALTITUDE*altitude_D + KI_BARO_ALTITUDE*altitude_I;
   command_altitude = Initial_Throttle + constrain(command_altitude,-ALTITUDE_CONTROL_BARO_OUTPUT_MIN,ALTITUDE_CONTROL_BARO_OUTPUT_MAX);
   return command_altitude;

ArducopterNG/Sensors.pde

@@ -96,7 +96,7 @@ void read_baro(void)
   if (press_alt == 0)
     press_alt = (1.0 - tempPresAlt) * 4433000;      // Altitude in cm
   else
-    press_alt = press_alt * 0.9 + ((1.0 - tempPresAlt) * 443300);  // Altitude in cm (filtered)
+    press_alt = press_alt * 0.75 + ((1.0 - tempPresAlt) * 4433000)*0.25;  // Altitude in cm (filtered)
 }
 #endif
-
+