OpticalFlow - replaced PI controller with PID controller.

Modified optical flow hold to use aggregated position instead of speed.

ArduCopter/ArduCopter.pde

@@ -69,6 +69,7 @@ http://code.google.com/p/ardupilot-mega/downloads/list
 #include <AP_TimerProcess.h>            // TimerProcess is the scheduler for MPU6000 reads.
 #include <AP_DCM.h>         // ArduPilot Mega DCM Library
 #include <APM_PI.h>            	// PI library
+#include <PID.h>            // PID library
 #include <RC_Channel.h>     // RC Channel Library
 #include <AP_RangeFinder.h>	// Range finder library
 #include <AP_OpticalFlow.h> // Optical Flow library
@@ -1989,17 +1990,22 @@ static void tuning(){
 			break;
 
 		case CH6_OPTFLOW_KP:
-			g.rc_6.set_range(0,10000);     // 0 to 10
+			g.rc_6.set_range(0,5000);     // 0 to 5
 			g.pi_optflow_roll.kP(tuning_value);
 			g.pi_optflow_pitch.kP(tuning_value);
 			break;
 
 		case CH6_OPTFLOW_KI:
-			g.rc_6.set_range(0,100);     // 0 to 0.1
+			g.rc_6.set_range(0,10000);     // 0 to 10
 			g.pi_optflow_roll.kI(tuning_value);
 			g.pi_optflow_pitch.kI(tuning_value);
 			break;
 
+		case CH6_OPTFLOW_KD:
+			g.rc_6.set_range(0,200);    // 0 to 0.2
+			g.pi_optflow_roll.kD(tuning_value);
+			g.pi_optflow_pitch.kD(tuning_value);
+			break;
 	}
 }
 

ArduCopter/Attitude.pde

@@ -258,6 +258,8 @@ static void reset_optflow_I(void)
 {
 	g.pi_optflow_roll.reset_I();
 	g.pi_optflow_pitch.reset_I();
+	of_roll = 0;
+	of_pitch = 0;
 }
 
 static void reset_wind_I(void)
@@ -462,34 +464,35 @@ static void init_z_damper()
 }
 #endif
 
-// calculate modified roll/pitch depending upon optical flow values
+// calculate modified roll/pitch depending upon optical flow calculated position
 static int32_t
 get_of_roll(int32_t control_roll)
 {
 #ifdef OPTFLOW_ENABLED
-	//static int32_t of_roll = 0;  // we use global variable to make logging easier
+	static float tot_x_cm = 0;  // total distance from target
     static unsigned long last_of_roll_update = 0;
-	static float prev_value = 0;
+	int32_t new_roll = 0;
-	float x_cm;
 
 	// check if new optflow data available
 	if( optflow.last_update != last_of_roll_update) {
 	    last_of_roll_update = optflow.last_update;
 
-		// filter movement
+		// add new distance moved
-		x_cm = (optflow.x_cm + prev_value) / 2.0 * 50.0;
+		tot_x_cm += optflow.x_cm;
 
 		// only stop roll if caller isn't modifying roll
 		if( control_roll == 0 && current_loc.alt < 1500) {
-			of_roll = g.pi_optflow_roll.get_pi(-x_cm, 1.0);  // we could use the last update time to calculate the time change
+			new_roll = g.pi_optflow_roll.get_pid(-tot_x_cm, 1.0, 1.0);  // we could use the last update time to calculate the time change
 		}else{
 		    g.pi_optflow_roll.reset_I();
-			prev_value = 0;
+			tot_x_cm = 0;
 		}
+		// limit amount of change and maximum angle
+		of_roll = constrain(new_roll, (of_roll-20), (of_roll+20));
 	}
-	// limit maximum angle
-	of_roll	= constrain(of_roll, -1000, 1000);
 
+	// limit max angle
+    of_roll = constrain(of_roll, -1000, 1000);
     return control_roll+of_roll;
 #else
     return control_roll;
@@ -500,27 +503,30 @@ static int32_t
 get_of_pitch(int32_t control_pitch)
 {
 #ifdef OPTFLOW_ENABLED
-    //static int32_t of_pitch = 0;  // we use global variable to make logging easier
+    static float tot_y_cm = 0;  // total distance from target
     static unsigned long last_of_pitch_update = 0;
-	static float prev_value = 0;
+	int32_t new_pitch = 0;
-	float y_cm;
 
 	// check if new optflow data available
 	if( optflow.last_update != last_of_pitch_update ) {
 	    last_of_pitch_update = optflow.last_update;
 
-		// filter movement
+		// add new distance moved
-		y_cm = (optflow.y_cm + prev_value) / 2.0 * 50.0;
+		tot_y_cm += optflow.y_cm;
 
-		// only stop roll if caller isn't modifying roll
+		// only stop roll if caller isn't modifying pitch
 		if( control_pitch == 0 && current_loc.alt < 1500 ) {
-			of_pitch = g.pi_optflow_pitch.get_pi(y_cm, 1.0);  // we could use the last update time to calculate the time change
+			new_pitch = g.pi_optflow_pitch.get_pid(tot_y_cm, 1.0, 1.0);  // we could use the last update time to calculate the time change
 		}else{
+		    tot_y_cm = 0;
 		    g.pi_optflow_pitch.reset_I();
-			prev_value = 0;
 		}
+
+		// limit amount of change
+		of_pitch = constrain(new_pitch, (of_pitch-20), (of_pitch+20));
 	}
-	// limit maximum angle
+
+	// limit max angle
 	of_pitch = constrain(of_pitch, -1000, 1000);
     return control_pitch+of_pitch;
 #else

ArduCopter/Parameters.h

@@ -309,8 +309,8 @@ public:
 	APM_PI		pi_acro_roll;
 	APM_PI		pi_acro_pitch;
 
-	APM_PI		pi_optflow_roll;
+	PID			pi_optflow_roll;
-	APM_PI		pi_optflow_pitch;
+	PID			pi_optflow_pitch;
 
 	uint8_t		junk;
 
@@ -435,8 +435,8 @@ public:
 	pi_acro_roll		(k_param_pi_acro_roll,			PSTR("ACRO_RLL_"),	ACRO_ROLL_P,		ACRO_ROLL_I,		ACRO_ROLL_IMAX * 100),
 	pi_acro_pitch		(k_param_pi_acro_pitch,			PSTR("ACRO_PIT_"),	ACRO_PITCH_P,		ACRO_PITCH_I,		ACRO_PITCH_IMAX * 100),
 
-	pi_optflow_roll		(k_param_pi_optflow_roll,		PSTR("OF_RLL_"),	OPTFLOW_ROLL_P,		OPTFLOW_ROLL_I,		OPTFLOW_IMAX * 100),
+	pi_optflow_roll		(k_param_pi_optflow_roll,		PSTR("OF_RLL_"),	OPTFLOW_ROLL_P,		OPTFLOW_ROLL_I,		OPTFLOW_ROLL_D,		OPTFLOW_IMAX * 100),
-	pi_optflow_pitch	(k_param_pi_optflow_pitch,		PSTR("OF_PIT_"),	OPTFLOW_PITCH_P,	OPTFLOW_PITCH_I,	OPTFLOW_IMAX * 100),
+	pi_optflow_pitch	(k_param_pi_optflow_pitch,		PSTR("OF_PIT_"),	OPTFLOW_PITCH_P,	OPTFLOW_PITCH_I,	OPTFLOW_PITCH_D,	OPTFLOW_IMAX * 100),
 
 	junk(0)		// XXX just so that we can add things without worrying about the trailing comma
 	{

ArduCopter/config.h

@@ -313,21 +313,30 @@
 #ifndef OPTFLOW_ORIENTATION
 # define OPTFLOW_ORIENTATION 	AP_OPTICALFLOW_ADNS3080_PINS_FORWARD
 #endif
+#ifndef OPTFLOW_RESOLUTION
+# define OPTFLOW_RESOLUTION 	ADNS3080_RESOLUTION_1600
+#endif
 #ifndef OPTFLOW_FOV
 # define OPTFLOW_FOV 			AP_OPTICALFLOW_ADNS3080_08_FOV
 #endif
 // optical flow based loiter PI values
 #ifndef OPTFLOW_ROLL_P
-  #define OPTFLOW_ROLL_P 6.4
+  #define OPTFLOW_ROLL_P 2.5
 #endif
 #ifndef OPTFLOW_ROLL_I
-  #define OPTFLOW_ROLL_I 0.068
+  #define OPTFLOW_ROLL_I 6.2
+#endif
+#ifndef OPTFLOW_ROLL_D
+  #define OPTFLOW_ROLL_D 0.12
 #endif
 #ifndef OPTFLOW_PITCH_P
-  #define OPTFLOW_PITCH_P 6.4
+  #define OPTFLOW_PITCH_P 2.5
 #endif
 #ifndef OPTFLOW_PITCH_I
-  #define OPTFLOW_PITCH_I 0.068
+  #define OPTFLOW_PITCH_I 6.2
+#endif
+#ifndef OPTFLOW_PITCH_D
+  #define OPTFLOW_PITCH_D 0.12
 #endif
 #ifndef OPTFLOW_IMAX
   #define OPTFLOW_IMAX 4

ArduCopter/defines.h

@@ -163,6 +163,7 @@
 // optical flow controller
 #define CH6_OPTFLOW_KP 17
 #define CH6_OPTFLOW_KI 18
+#define CH6_OPTFLOW_KD 19
 
 
 // nav byte mask