From 0d31d775b8b37095e87bd7571f6981ea8da6cbcb Mon Sep 17 00:00:00 2001
From: Jason Short <jasonshort@gmail.com>
Date: Thu, 15 Sep 2011 23:41:15 -0700
Subject: [PATCH] cleanup added more ifdef for optflow

---
 ArduCopter/ArduCopter.pde |  4 +++-
 ArduCopter/config.h       |  2 +-
 ArduCopter/navigation.pde | 24 +++++++++++-------------
 3 files changed, 15 insertions(+), 15 deletions(-)

diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index 69e92c4b7b..62b3d06bec 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -204,7 +204,7 @@ static AP_Int8                *flight_modes = &g.flight_mode1;
  	#endif
 #endif
 
-#elif HIL_MODE == HIL_MODE_ATTITUDE
+#if HIL_MODE == HIL_MODE_ATTITUDE
 	#ifdef OPTFLOW_ENABLED
 		AP_OpticalFlow_ADNS3080 optflow(&dcm);
 	#endif
@@ -617,6 +617,7 @@ static void medium_loop()
 
 			medium_loopCounter++;
 
+			#ifdef OPTFLOW_ENABLED
 			if(g.optflow_enabled){
 				optflow.read();
 				optflow.update_position(cos_yaw_x, sin_yaw_y, current_loc.alt);  // updates internal lon and lat with estimation based on optical flow
@@ -626,6 +627,7 @@ static void medium_loop()
 					Log_Write_Optflow();
 				}
 			}
+			#endif
 
 			if(GPS_enabled){
 				update_GPS();
diff --git a/ArduCopter/config.h b/ArduCopter/config.h
index 3f6937a374..79e19c1b05 100644
--- a/ArduCopter/config.h
+++ b/ArduCopter/config.h
@@ -210,7 +210,7 @@
   #define OPTFLOW_ENABLED
 #endif
 
-#define OPTFLOW_ENABLED
+//#define OPTFLOW_ENABLED
 
 #ifndef OPTFLOW					// sets global enabled/disabled flag for optflow (as seen in CLI)
 # define OPTFLOW				DISABLED
diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde
index a11b96e894..855761192f 100644
--- a/ArduCopter/navigation.pde
+++ b/ArduCopter/navigation.pde
@@ -97,14 +97,19 @@ static void calc_nav_rate(int x_error, int y_error, int max_speed, int min_speed
 	// find the rates:
 	float temp		= radians((float)g_gps->ground_course/100.0);
 
-	// calc the cos of the error to tell how fast we are moving towards the target in cm
-	if(g.optflow_enabled && current_loc.alt < 500 &&  g_gps->ground_speed < 150){
-		x_actual_speed 	= optflow.vlon * 10;
-		y_actual_speed 	= optflow.vlat * 10;
-	}else{
+	#ifdef OPTFLOW_ENABLED
+		// calc the cos of the error to tell how fast we are moving towards the target in cm
+		if(g.optflow_enabled && current_loc.alt < 500 &&  g_gps->ground_speed < 150){
+			x_actual_speed 	= optflow.vlon * 10;
+			y_actual_speed 	= optflow.vlat * 10;
+		}else{
+			x_actual_speed 	= (float)g_gps->ground_speed * sin(temp);
+			y_actual_speed 	= (float)g_gps->ground_speed * cos(temp);
+		}
+	#else
 		x_actual_speed 	= (float)g_gps->ground_speed * sin(temp);
 		y_actual_speed 	= (float)g_gps->ground_speed * cos(temp);
-	}
+	#endif
 
 	y_rate_error 	= y_target_speed - y_actual_speed; // 413
 	y_rate_error 	= constrain(y_rate_error, -600, 600);	// added a rate error limit to keep pitching down to a minimum
@@ -128,13 +133,6 @@ static void calc_nav_pitch_roll()
 	nav_pitch = -nav_pitch;
 }
 
-// ------------------------------
-/*static void calc_bearing_error()
-{
-	bearing_error 	= nav_bearing - dcm.yaw_sensor;
-	bearing_error 	= wrap_180(bearing_error);
-}*/
-
 static long get_altitude_error()
 {
 	return next_WP.alt - current_loc.alt;