From 122623f64bc8779331a92d794feacc7732b7b442 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Sat, 28 Jan 2012 09:27:16 +0900
Subject: [PATCH] OpticalFlow - added OF_LOITER flightmode

---
 ArduCopter/ArduCopter.pde | 11 +++--------
 ArduCopter/config.h       | 12 ++++++++++++
 ArduCopter/defines.h      |  3 ++-
 ArduCopter/system.pde     | 13 ++++++++++---
 4 files changed, 27 insertions(+), 12 deletions(-)

diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index 193a112b8a..58f240169a 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -292,7 +292,8 @@ static const char* flight_mode_strings[] = {
 	"RTL",
 	"CIRCLE",
 	"POSITION",
-	"LAND"};
+	"LAND",
+	"OF_LOITER"};
 
 /* Radio values
 		Channel assignments
@@ -1440,15 +1441,9 @@ void update_roll_pitch_mode(void)
 			if(do_simple && new_radio_frame){
 				update_simple_mode();
 			}
-
-			// in this mode, nav_roll and nav_pitch = the iterm
-			#if WIND_COMP_STAB == 1
-			g.rc_1.servo_out = get_stabilize_roll(get_of_roll(g.rc_1.control_in + nav_roll));
-			g.rc_2.servo_out = get_stabilize_pitch(get_of_pitch(g.rc_2.control_in + nav_pitch));
-			#else
+			// mix in user control with optical flow
 			g.rc_1.servo_out = get_stabilize_roll(get_of_roll(g.rc_1.control_in));
 			g.rc_2.servo_out = get_stabilize_pitch(get_of_pitch(g.rc_2.control_in));
-			#endif
 			break;
 	}
 
diff --git a/ArduCopter/config.h b/ArduCopter/config.h
index a483e98a93..4d07062990 100644
--- a/ArduCopter/config.h
+++ b/ArduCopter/config.h
@@ -503,6 +503,18 @@
 # define SUPER_SIMPLE		DISABLED
 #endif
 
+// LOITER Mode
+#ifndef OF_LOITER_YAW
+# define OF_LOITER_YAW 		YAW_HOLD
+#endif
+
+#ifndef OF_LOITER_RP
+# define OF_LOITER_RP 			ROLL_PITCH_STABLE_OF
+#endif
+
+#ifndef OF_LOITER_THR
+# define OF_LOITER_THR			THROTTLE_HOLD
+#endif
 
 //////////////////////////////////////////////////////////////////////////////
 // Attitude Control
diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h
index c0121912fb..9ca248da84 100644
--- a/ArduCopter/defines.h
+++ b/ArduCopter/defines.h
@@ -122,7 +122,8 @@
 #define CIRCLE 7			// AUTO control
 #define POSITION 8			// AUTO control
 #define LAND 9				// AUTO control
-#define NUM_MODES 10
+#define OF_LOITER 10			// Hold a single location using optical flow sensor
+#define NUM_MODES 11
 
 #define INITIALISING 9     // in startup routines
 
diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde
index bc599db619..d4ad437754 100644
--- a/ArduCopter/system.pde
+++ b/ArduCopter/system.pde
@@ -408,13 +408,13 @@ static void set_mode(byte mode)
 	// if we don't have GPS lock
 	if(home_is_set == false){
 		// our max mode should be
-		if (mode > ALT_HOLD)
+		if (mode > ALT_HOLD && mode != OF_LOITER)
 			mode = STABILIZE;
 	}
 
-	// nothing but Loiter for OptFlow only
+	// nothing but OF_LOITER for OptFlow only
 	if (g.optflow_enabled && GPS_enabled == false){
-		if (mode > ALT_HOLD && mode != LOITER)
+		if (mode > ALT_HOLD && mode != OF_LOITER)
 			mode = STABILIZE;
 	}
 
@@ -520,6 +520,13 @@ static void set_mode(byte mode)
 			do_RTL();
 			break;
 
+		case OF_LOITER:
+			yaw_mode 		= OF_LOITER_YAW;
+			roll_pitch_mode = OF_LOITER_RP;
+			throttle_mode 	= OF_LOITER_THR;
+			set_next_WP(&current_loc);
+			break;
+
 		default:
 			break;
 	}