ArduCopter - added ROLL_PITCH_STABLE_OF (i.e. Stabilised Roll/Pitch + adjustments based on optical flow)

Removed optical flow from regular loiter for now until it's tested.

ArduCopter/ArduCopter.pde

@@ -665,6 +665,11 @@ static int16_t	nav_lon;
 static int16_t	nav_lat_p;
 static int16_t	nav_lon_p;
 
+// The Commanded ROll from the autopilot based on optical flow sensor.
+static int32_t	of_roll = 0;
+// The Commanded pitch from the autopilot based on optical flow sensor. negative Pitch means go forward.
+static int32_t	of_pitch = 0;
+
 
 ////////////////////////////////////////////////////////////////////////////////
 // Navigation Throttle control
@@ -926,12 +931,6 @@ static void medium_loop()
 				update_GPS();
 			}
 
-			#ifdef OPTFLOW_ENABLED
-			if(g.optflow_enabled){
-				update_optical_flow();
-			}
-			#endif
-
 			#if HIL_MODE != HIL_MODE_ATTITUDE					// don't execute in HIL mode
 				if(g.compass_enabled){
 					if (compass.read()) {
@@ -975,13 +974,13 @@ static void medium_loop()
 					// If we have optFlow enabled we can grab a more accurate speed
 					// here and override the speed from the GPS
 					// ----------------------------------------
-					#ifdef OPTFLOW_ENABLED
+					//#ifdef OPTFLOW_ENABLED
-					if(g.optflow_enabled && current_loc.alt < 500){
+					//if(g.optflow_enabled && current_loc.alt < 500){
-						// optflow wont be enabled on 1280's
+					//	// optflow wont be enabled on 1280's
-						x_GPS_speed 	= optflow.x_cm;
+					//	x_GPS_speed 	= optflow.x_cm;
-						y_GPS_speed 	= optflow.y_cm;
+					//	y_GPS_speed 	= optflow.y_cm;
-					}
+					//}
-					#endif
+					//#endif
 
 					// control mode specific updates
 					// -----------------------------
@@ -1096,6 +1095,13 @@ static void fifty_hz_loop()
 	}
     #endif
 
+	// syncronise optical flow reads with altitude reads
+	#ifdef OPTFLOW_ENABLED
+	if(g.optflow_enabled){
+		update_optical_flow();
+	}
+	#endif
+
 	// agmatthews - USERHOOKS
 	#ifdef USERHOOK_50HZLOOP
 	  USERHOOK_50HZLOOP
@@ -1222,15 +1228,21 @@ static void super_slow_loop()
 #ifdef OPTFLOW_ENABLED
 static void update_optical_flow(void)
 {
+    static int log_counter = 0;
+
 	optflow.update();
 	optflow.update_position(dcm.roll, dcm.pitch, cos_yaw_x, sin_yaw_y, current_loc.alt);  // updates internal lon and lat with estimation based on optical flow
 
 	// write to log
+	log_counter++;
+	if( log_counter >= 5 ) {
+	    log_counter = 0;
 		if (g.log_bitmask & MASK_LOG_OPTFLOW){
 			Log_Write_Optflow();
 		}
+	}
 
-	if(g.optflow_enabled && current_loc.alt < 500){
+	/*if(g.optflow_enabled && current_loc.alt < 500){
 		if(GPS_enabled){
 			// if we have a GPS, we add some detail to the GPS
 			// XXX this may not ne right
@@ -1247,7 +1259,7 @@ static void update_optical_flow(void)
 		}
 		// OK to run the nav routines
 		nav_ok = true;
-	}
+	}*/
 }
 #endif
 
@@ -1414,6 +1426,22 @@ void update_roll_pitch_mode(void)
 			g.rc_1.servo_out 	= get_stabilize_roll(control_roll);
 			g.rc_2.servo_out 	= get_stabilize_pitch(control_pitch);
 			break;
+
+		case ROLL_PITCH_STABLE_OF:
+			// apply SIMPLE mode transform
+			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
+			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;
 	}
 
 	// clear new radio frame info
@@ -1950,6 +1978,19 @@ static void tuning(){
 			g.rc_6.set_range(0,1000);     // 0 to 1
 			g.pi_alt_hold.kP(tuning_value);
 			break;
+
+		case CH6_OPTFLOW_KP:
+			g.rc_6.set_range(0,10000);     // 0 to 10
+			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.pi_optflow_roll.kI(tuning_value);
+			g.pi_optflow_pitch.kI(tuning_value);
+			break;
+
 	}
 }
 

ArduCopter/Attitude.pde

@@ -258,6 +258,8 @@ static void reset_rate_I()
 	g.pi_rate_pitch.reset_I();
 	g.pi_acro_roll.reset_I();
 	g.pi_acro_pitch.reset_I();
+	g.pi_optflow_roll.reset_I();
+	g.pi_optflow_pitch.reset_I();
 }
 
 
@@ -430,3 +432,69 @@ static void init_z_damper()
 {
 }
 #endif
+
+// calculate modified roll/pitch depending upon optical flow values
+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 unsigned long last_of_roll_update = 0;
+	static float prev_value = 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
+		x_cm = (optflow.x_cm + prev_value) / 2.0 * 50.0;
+
+		// 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
+		}else{
+		    g.pi_optflow_roll.reset_I();
+			prev_value = 0;
+		}
+	}
+	// limit maximum angle
+	of_roll	= constrain(of_roll, -1000, 1000);
+
+    return control_roll+of_roll;
+#else
+    return control_roll;
+#endif
+}
+
+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 unsigned long last_of_pitch_update = 0;
+	static float prev_value = 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
+		y_cm = (optflow.y_cm + prev_value) / 2.0 * 50.0;
+
+		// only stop roll if caller isn't modifying roll
+		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
+		}else{
+		    g.pi_optflow_pitch.reset_I();
+			prev_value = 0;
+		}
+	}
+	// limit maximum angle
+	of_pitch = constrain(of_pitch, -1000, 1000);
+    return control_pitch+of_pitch;
+#else
+    return control_pitch;
+#endif
+}

ArduCopter/Log.pde

@@ -482,8 +482,12 @@ static void Log_Write_Optflow()
 	DataFlash.WriteInt((int)optflow.dx);
 	DataFlash.WriteInt((int)optflow.dy);
 	DataFlash.WriteInt((int)optflow.surface_quality);
+	DataFlash.WriteInt((int)optflow.x_cm);
+	DataFlash.WriteInt((int)optflow.y_cm);
 	DataFlash.WriteLong(optflow.vlat);//optflow_offset.lat + optflow.lat);
 	DataFlash.WriteLong(optflow.vlon);//optflow_offset.lng + optflow.lng);
+	DataFlash.WriteLong(of_roll);
+	DataFlash.WriteLong(of_pitch);
 	DataFlash.WriteByte(END_BYTE);
 	#endif
 }
@@ -495,15 +499,23 @@ static void Log_Read_Optflow()
 	int16_t temp1 	= DataFlash.ReadInt();			// 1
 	int16_t temp2 	= DataFlash.ReadInt();			// 2
 	int16_t temp3 	= DataFlash.ReadInt();			// 3
-	float temp4 	= DataFlash.ReadLong();			// 4
+	int16_t temp4 	= DataFlash.ReadInt();			// 4
-	float temp5 	= DataFlash.ReadLong();			// 5
+	int16_t temp5 	= DataFlash.ReadInt();			// 5
+	float temp6 	= DataFlash.ReadLong();			// 6
+	float temp7 	= DataFlash.ReadLong();			// 7
+	int32_t temp8 	= DataFlash.ReadLong();			// 8
+	int32_t temp9 	= DataFlash.ReadLong();			// 9
 
-	Serial.printf_P(PSTR("OF, %d, %d, %d, %4.7f, %4.7f\n"),
+	Serial.printf_P(PSTR("OF, %d, %d, %d, %d, %d, %4.7f, %4.7f, %d, %d\n"),
 			temp1,
 			temp2,
 			temp3,
 			temp4,
-			temp5);
+			temp5,
+			temp6,
+			temp7,
+			temp8,
+			temp9);
 	#endif
 }
 

ArduCopter/Parameters.h

@@ -178,6 +178,8 @@ public:
 	k_param_pi_throttle,
 	k_param_pi_acro_roll,
 	k_param_pi_acro_pitch,
+	k_param_pi_optflow_roll,
+	k_param_pi_optflow_pitch,  // 250
 
 
     // 254,255: reserved
@@ -298,6 +300,9 @@ public:
 	APM_PI		pi_acro_roll;
 	APM_PI		pi_acro_pitch;
 
+	APM_PI		pi_optflow_roll;
+	APM_PI		pi_optflow_pitch;
+
 	uint8_t		junk;
 
 	// Note: keep initializers here in the same order as they are declared above.
@@ -417,6 +422,9 @@ 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_pitch	(k_param_pi_optflow_pitch,		PSTR("OF_PIT_"),	OPTFLOW_PITCH_P,	OPTFLOW_PITCH_I,	OPTFLOW_IMAX * 100),
+
 	junk(0)		// XXX just so that we can add things without worrying about the trailing comma
 	{
 	}

ArduCopter/config.h

@@ -310,6 +310,23 @@
 #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
+#endif
+#ifndef OPTFLOW_ROLL_I
+  #define OPTFLOW_ROLL_I 0.068
+#endif
+#ifndef OPTFLOW_PITCH_P
+  #define OPTFLOW_PITCH_P 6.4
+#endif
+#ifndef OPTFLOW_PITCH_I
+  #define OPTFLOW_PITCH_I 0.068
+#endif
+#ifndef OPTFLOW_IMAX
+  #define OPTFLOW_IMAX 4
+#endif
+
 
 //////////////////////////////////////////////////////////////////////////////
 // RADIO CONFIGURATION

ArduCopter/defines.h

@@ -18,6 +18,7 @@
 #define ROLL_PITCH_STABLE 	0
 #define ROLL_PITCH_ACRO 	1
 #define ROLL_PITCH_AUTO		2
+#define ROLL_PITCH_STABLE_OF	3
 
 #define THROTTLE_MANUAL 	0
 #define THROTTLE_HOLD 		1
@@ -155,6 +156,10 @@
 #define CH6_Z_GAIN 15
 #define CH6_DAMP 16
 
+// optical flow controller
+#define CH6_OPTFLOW_KP 17
+#define CH6_OPTFLOW_KI 18
+
 
 // nav byte mask
 // -------------

ArduCopter/system.pde

@@ -380,6 +380,8 @@ static void reset_I_all(void)
 	g.pi_throttle.reset_I();
 	g.pi_acro_roll.reset_I();
 	g.pi_acro_pitch.reset_I();
+	g.pi_optflow_roll.reset_I();
+	g.pi_optflow_pitch.reset_I();
 }