cleanup

added more ifdef for optflow

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();

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

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;