From 04b893ea4365fbe2586be86f1591875c04d8f150 Mon Sep 17 00:00:00 2001
From: jjulio1234 <jjulio1234@f9c3cf11-9bcb-44bc-f272-b75c42450872>
Date: Sat, 13 Nov 2010 17:51:43 +0000
Subject: [PATCH] Added altitude control function based on sonar (not activated
 yet)

git-svn-id: https://arducopter.googlecode.com/svn/trunk@835 f9c3cf11-9bcb-44bc-f272-b75c42450872
---
 ArducopterNG/Navigation.pde | 20 ++++++++++++++++++++
 1 file changed, 20 insertions(+)

diff --git a/ArducopterNG/Navigation.pde b/ArducopterNG/Navigation.pde
index c1bbf0f96a..e20dc40783 100644
--- a/ArducopterNG/Navigation.pde
+++ b/ArducopterNG/Navigation.pde
@@ -125,5 +125,25 @@ int Altitude_control_baro(int altitude, int target_altitude)
   return command_altitude;
 }
 
+/* Altitude control... (based on sonar) */
+/* CONTROL PARAMETERS FOR SONAR ALTITUDE CONTROL (TEMPORATLY HERE) */
+#define KP_SONAR_ALTITUDE 0.8
+#define KD_SONAR_ALTITUDE 0.7
+#define KI_SONAR_ALTITUDE 0.3
+int Altitude_control_Sonar(int Sonar_altitude, int target_sonar_altitude)
+{
+  #define ALTITUDE_CONTROL_SONAR_OUTPUT_MIN 60
+  #define ALTITUDE_CONTROL_SONAR_OUTPUT_MAX 80
+  
+  int command_altitude;
+   
+  err_altitude_old = err_altitude;
+  err_altitude = target_sonar_altitude - Sonar_altitude;  
+  altitude_D = (float)(err_altitude-err_altitude_old)/0.05;
+  altitude_I += (float)err_altitude*0.05;
+  altitude_I = constrain(altitude_I,-1000,1000);
+  command_altitude = KP_SONAR_ALTITUDE*err_altitude + KD_SONAR_ALTITUDE*altitude_D + KI_SONAR_ALTITUDE*altitude_I;
+  return (Initial_Throttle + constrain(command_altitude,-ALTITUDE_CONTROL_SONAR_OUTPUT_MIN,ALTITUDE_CONTROL_SONAR_OUTPUT_MAX));
+}