ACM - partial integration of optical flow sensor

git-svn-id: https://arducopter.googlecode.com/svn/trunk@2934 f9c3cf11-9bcb-44bc-f272-b75c42450872

ArduCopterMega/ArduCopterMega.pde

@@ -61,6 +61,7 @@ And much more so PLEASE PM me on DIYDRONES to add your contribution to the List
 #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
 
 #define MAVLINK_COMM_NUM_BUFFERS 2
 #include <GCS_MAVLink.h>    // MAVLink GCS definitions
@@ -132,6 +133,9 @@ static AP_Int8                *flight_modes = &g.flight_mode1;
   #else
     #error Unrecognised MAG_PROTOCOL setting.
   #endif
+  #ifdef OPTFLOW_ENABLED
+    AP_OpticalFlow_ADNS3080 optflow;
+  #endif
 
 	// real GPS selection
 	#if   GPS_PROTOCOL == GPS_PROTOCOL_AUTO
@@ -452,9 +456,11 @@ static struct 	Location next_WP;					// next waypoint
 static struct 	Location target_WP;					// where do we want to you towards?
 static struct 	Location simple_WP;					//
 static struct 	Location next_command;				// command preloaded
-static struct  Location guided_WP;					// guided mode waypoint
+static struct   Location guided_WP;					// guided mode waypoint
 static long 	target_altitude;					// used for
 static boolean	home_is_set; 						// Flag for if we have g_gps lock and have set the home location
+static struct	Location optflow_offset;			// optical flow base position
+static boolean  new_location;						// flag to tell us if location has been updated 
 
 
 // IMU variables
@@ -1003,7 +1009,88 @@ static void update_GPS(void)
 	}
 }
 
-static void update_current_flight_mode(void)
+#ifdef OPTFLOW_ENABLED
+// blend gps and optical flow location
+void update_location(void)
+{
+    //static int count = 0;
+    // get GPS position
+	if(GPS_enabled){
+		update_GPS();
+	}
+
+	if( g.optflow_enabled ) {
+	    int32_t	temp_lat, temp_lng, diff_lat, diff_lng;
+		
+		// get optical flow position
+		optflow.read();
+		optflow.get_position(dcm.roll, dcm.pitch, dcm.yaw, current_loc.alt-home.alt);
+		
+		// write to log
+		if (g.log_bitmask & MASK_LOG_OPTFLOW){
+			Log_Write_Optflow();
+		}
+		
+		temp_lat = optflow_offset.lat + optflow.lat;
+		temp_lng = optflow_offset.lng + optflow.lng;
+		
+		// if we have good GPS values, don't let optical flow position stray too far
+		if( GPS_enabled && g_gps->fix ) {
+			// ensure current location is within 3m of gps location
+			diff_lat = g_gps->latitude - temp_lat;
+			diff_lng = g_gps->longitude - temp_lng;
+			if( diff_lat > 300 ) {
+				optflow_offset.lat += diff_lat - 300;
+				//Serial.println("lat inc!");
+			}
+			if( diff_lat < -300 ) {
+				optflow_offset.lat += diff_lat + 300;
+				//Serial.println("lat dec!");
+			}
+			if( diff_lng > 300 ) {
+				optflow_offset.lng += diff_lng - 300;
+				//Serial.println("lng inc!");
+			}
+			if( diff_lng < -300 ) {
+				optflow_offset.lng += diff_lng + 300;
+				//Serial.println("lng dec!");
+			}
+		}
+		
+		// update the current position
+		current_loc.lat = optflow_offset.lat + optflow.lat;
+		current_loc.lng = optflow_offset.lng + optflow.lng;
+		
+		/*count++;
+		if( count >= 20 ) {
+		    count = 0;
+			Serial.println();
+			Serial.print("lat:");
+			Serial.print(current_loc.lat);
+			Serial.print("\tlng:");
+			Serial.print(current_loc.lng);
+			Serial.print("\tr:");
+			Serial.print(nav_roll);
+			Serial.print("\tp:");
+			Serial.print(nav_pitch);
+			Serial.println();
+		}*/
+		
+		// indicate we have a new position for nav functions
+		new_location = true;
+
+	}else{
+	    // get current position from gps
+		current_loc.lng = g_gps->longitude;	// Lon * 10 * *7
+		current_loc.lat = g_gps->latitude;	// Lat * 10 * *7
+		
+		new_location = g_gps->new_data;
+	}
+}
+#endif
+
+
+void update_current_flight_mode(void)
 {
 	if(control_mode == AUTO){
 

ArduCopterMega/Log.pde

@@ -71,6 +71,7 @@ print_log_menu(void)
 		if (g.log_bitmask & MASK_LOG_CMD)			Serial.printf_P(PSTR(" CMD"));
 		if (g.log_bitmask & MASK_LOG_CURRENT)		Serial.printf_P(PSTR(" CURRENT"));
 		if (g.log_bitmask & MASK_LOG_MOTORS)		Serial.printf_P(PSTR(" MOTORS"));
+		if (g.log_bitmask & MASK_LOG_OPTFLOW)		Serial.printf_P(PSTR(" OPTFLOW"));
 	}
 
 	Serial.println();
@@ -181,6 +182,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
 		TARG(CMD);
 		TARG(CURRENT);
 		TARG(MOTORS);
+		TARG(OPTFLOW);
 		#undef TARG
 	}
 
@@ -494,6 +496,34 @@ static void Log_Read_Motors()
 			DataFlash.ReadInt());
 }
 
+#ifdef OPTFLOW_ENABLED
+// Write an optical flow packet. Total length : 18 bytes
+static void Log_Write_Optflow()
+{
+	DataFlash.WriteByte(HEAD_BYTE1);
+	DataFlash.WriteByte(HEAD_BYTE2);
+	DataFlash.WriteByte(LOG_OPTFLOW_MSG);
+	DataFlash.WriteInt((int)optflow.dx);
+	DataFlash.WriteInt((int)optflow.dy);
+	DataFlash.WriteInt((int)optflow.surface_quality);
+	DataFlash.WriteLong(optflow.lat);//optflow_offset.lat + optflow.lat);
+	DataFlash.WriteLong(optflow.lng);//optflow_offset.lng + optflow.lng);
+	DataFlash.WriteByte(END_BYTE);
+}
+#endif
+
+// Read an attitude packet
+static void Log_Read_Optflow()
+{
+	Serial.printf_P(PSTR("OF, %d, %d, %d, %4.7f, %4.7f\n"),
+			DataFlash.ReadInt(),
+			DataFlash.ReadInt(),
+			DataFlash.ReadInt(),
+			(float)DataFlash.ReadLong(),// / t7,
+			(float)DataFlash.ReadLong() // / t7
+			);
+}
+
 static void Log_Write_Nav_Tuning()
 {
 	Matrix3f tempmat = dcm.get_dcm_matrix();
@@ -820,6 +850,10 @@ static void Log_Read(int start_page, int end_page)
 						Log_Read_Motors();
 						break;
 
+					case LOG_OPTFLOW_MSG:
+						Log_Read_Optflow();
+						break;
+
 					case LOG_GPS_MSG:
 						Log_Read_GPS();
 						break;
@@ -842,6 +876,9 @@ static void Log_Write_Raw() {}
 static void Log_Write_GPS() {}
 static void Log_Write_Current() {}
 static void Log_Write_Attitude() {}
+#ifdef OPTFLOW_ENABLED
+static void Log_Write_Optflow() {}
+#endif
 static void Log_Write_Nav_Tuning() {}
 static void Log_Write_Control_Tuning() {}
 static void Log_Write_Motors() {}

ArduCopterMega/Parameters.h

@@ -17,7 +17,7 @@ public:
 	// The increment will prevent old parameters from being used incorrectly
 	// by newer code.
 	//
-	static const uint16_t k_format_version = 104;
+	static const uint16_t k_format_version = 105;
 
 	// The parameter software_type is set up solely for ground station use
 	// and identifies the software type (eg ArduPilotMega versus ArduCopterMega)
@@ -75,6 +75,7 @@ public:
 	k_param_sonar,
 	k_param_frame_orientation,
 	k_param_top_bottom_ratio,
+	k_param_optflow_enabled,
 
 	//
 	// 160: Navigation parameters
@@ -233,6 +234,7 @@ public:
 	AP_Int8		esc_calibrate;
 	AP_Int8		frame_orientation;
 	AP_Float	top_bottom_ratio;
+    AP_Int8		optflow_enabled;
 
 	#if FRAME_CONFIG ==	HELI_FRAME
 	// Heli
@@ -288,6 +290,7 @@ public:
 	battery_monitoring 		(DISABLED,					k_param_battery_monitoring,				PSTR("BATT_MONITOR")),
 	pack_capacity			(HIGH_DISCHARGE,			k_param_pack_capacity,					PSTR("BATT_CAPACITY")),
 	compass_enabled			(MAGNETOMETER,				k_param_compass_enabled,				PSTR("MAG_ENABLE")),
+	optflow_enabled			(OPTFLOW,					k_param_optflow_enabled,				PSTR("FLOW_ENABLE")),
 
 	waypoint_mode			(0,							k_param_waypoint_mode,					PSTR("WP_MODE")),
 	waypoint_total			(0,							k_param_waypoint_total,					PSTR("WP_TOTAL")),

ArduCopterMega/config.h

@@ -200,6 +200,21 @@
 
 
 //////////////////////////////////////////////////////////////////////////////
+//  OPTICAL_FLOW
+#if defined( __AVR_ATmega2560__ )  // determines if optical flow code is included
+  #define OPTFLOW_ENABLED
+#endif
+
+#ifndef OPTFLOW					// sets global enabled/disabled flag for optflow (as seen in CLI)
+# define OPTFLOW				DISABLED
+#endif
+#ifndef OPTFLOW_ORIENTATION
+# define OPTFLOW_ORIENTATION 	AP_OPTICALFLOW_ADNS3080_PINS_FORWARD
+#endif
+#ifndef OPTFLOW_FOV
+# define OPTFLOW_FOV 			AP_OPTICALFLOW_ADNS3080_12_FOV
+#endif
+
 //////////////////////////////////////////////////////////////////////////////
 // RADIO CONFIGURATION
 //////////////////////////////////////////////////////////////////////////////

ArduCopterMega/defines.h

@@ -261,6 +261,7 @@
 #define LOG_CURRENT_MSG 		0x09
 #define LOG_STARTUP_MSG 		0x0A
 #define LOG_MOTORS_MSG 			0x0B
+#define LOG_OPTFLOW_MSG 		0x0C
 #define LOG_INDEX_MSG			0xF0
 #define MAX_NUM_LOGS			50
 
@@ -275,6 +276,7 @@
 #define MASK_LOG_CMD			(1<<8)
 #define MASK_LOG_CURRENT		(1<<9)
 #define MASK_LOG_MOTORS			(1<<10)
+#define MASK_LOG_OPTFLOW		(1<<11)
 
 // Waypoint Modes
 // ----------------

ArduCopterMega/setup.pde

@@ -16,6 +16,9 @@ static int8_t	setup_compass			(uint8_t argc, const Menu::arg *argv);
 //static int8_t	setup_mag_offset		(uint8_t argc, const Menu::arg *argv);
 static int8_t	setup_declination		(uint8_t argc, const Menu::arg *argv);
 static int8_t	setup_esc				(uint8_t argc, const Menu::arg *argv);
+#ifdef OPTFLOW_ENABLED
+static int8_t	setup_optflow			(uint8_t argc, const Menu::arg *argv);
+#endif
 static int8_t	setup_show				(uint8_t argc, const Menu::arg *argv);
 
 #if FRAME_CONFIG == HELI_FRAME
@@ -40,6 +43,9 @@ const struct Menu::command setup_menu_commands[] PROGMEM = {
 	{"compass",			setup_compass},
 //	{"offsets",			setup_mag_offset},
 	{"declination",		setup_declination},
+#ifdef OPTFLOW_ENABLED
+	{"optflow",			setup_optflow},
+#endif
 #if FRAME_CONFIG == HELI_FRAME
 	{"heli",			setup_heli},
 	{"gyro",			setup_gyro},
@@ -93,6 +99,9 @@ setup_show(uint8_t argc, const Menu::arg *argv)
 	report_flight_modes();
 	report_imu();
 	report_compass();
+#ifdef OPTFLOW_ENABLED
+	report_optflow();
+#endif
 #if FRAME_CONFIG == HELI_FRAME
 	report_heli();
 	report_gyro();
@@ -704,6 +713,32 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv)
 }
 */
 
+#ifdef OPTFLOW_ENABLED
+static int8_t
+setup_optflow(uint8_t argc, const Menu::arg *argv)
+{
+	if (!strcmp_P(argv[1].str, PSTR("on"))) {
+		g.optflow_enabled = true;
+		init_optflow();
+
+	} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
+		g.optflow_enabled = false;
+
+	//} else if(argv[1].i > 10){
+	//	g.optflow_fov.set_and_save(argv[1].i);
+	//	optflow.set_field_of_view(g.optflow_fov.get());
+
+	}else{
+		Serial.printf_P(PSTR("\nOptions:[on, off]\n"));
+		report_optflow();
+		return 0;
+	}
+
+	g.optflow_enabled.save();
+	report_optflow();
+	return 0;
+}
+#endif
 
 /***************************************************************************/
 // CLI reports
@@ -894,6 +929,22 @@ static void report_flight_modes()
 	print_blanks(2);
 }
 
+#ifdef OPTFLOW_ENABLED
+void report_optflow()
+{
+	Serial.printf_P(PSTR("OptFlow\n"));
+	print_divider();
+
+	print_enabled(g.optflow_enabled);
+
+	// field of view
+	//Serial.printf_P(PSTR("FOV: %4.0f\n"),
+	//						degrees(g.optflow_fov));
+
+	print_blanks(2);
+}
+#endif
+
 #if FRAME_CONFIG == HELI_FRAME
 static void report_heli()
 {

ArduCopterMega/system.pde

@@ -215,6 +215,13 @@ static void init_ardupilot()
 	}
 	#endif
 
+#ifdef OPTFLOW_ENABLED
+	// init the optical flow sensor
+	if(g.optflow_enabled) {
+		init_optflow();
+	}
+#endif
+	
 	// Logging:
 	// --------
 	// DataFlash log initialization
@@ -459,6 +466,15 @@ init_compass()
 	Vector3f junkvector = compass.get_offsets();					// load offsets to account for airframe magnetic interference
 }
 
+#ifdef OPTFLOW_ENABLED
+static void
+init_optflow()
+{
+	bool junkbool = optflow.init();
+	optflow.set_orientation(OPTFLOW_ORIENTATION);			// set optical flow sensor's orientation on aircraft
+	optflow.set_field_of_view(OPTFLOW_FOV);					// set optical flow sensor's field of view
+}
+#endif
 
 /* This function gets the current value of the heap and stack pointers.
 * The stack pointer starts at the top of RAM and grows downwards. The heap pointer

ArduCopterMega/test.pde

@@ -24,6 +24,9 @@ static int8_t	test_wp(uint8_t argc, 			const Menu::arg *argv);
 static int8_t	test_baro(uint8_t argc, 		const Menu::arg *argv);
 static int8_t	test_mag(uint8_t argc, 			const Menu::arg *argv);
 static int8_t	test_sonar(uint8_t argc, 		const Menu::arg *argv);
+#ifdef OPTFLOW_ENABLED
+static int8_t	test_optflow(uint8_t argc, 		const Menu::arg *argv);
+#endif
 static int8_t	test_xbee(uint8_t argc, 		const Menu::arg *argv);
 static int8_t	test_eedump(uint8_t argc, 		const Menu::arg *argv);
 static int8_t	test_rawgps(uint8_t argc, 		const Menu::arg *argv);
@@ -71,6 +74,9 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
 #endif
 	{"sonar",		test_sonar},
 	{"compass",		test_mag},
+#ifdef OPTFLOW_ENABLED
+	{"optflow",		test_optflow},
+#endif
 	{"xbee",		test_xbee},
 	{"eedump",		test_eedump},
 	{"rawgps",		test_rawgps},
@@ -972,6 +978,37 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
 	return (0);
 }
 
+#ifdef OPTFLOW_ENABLED
+static int8_t
+test_optflow(uint8_t argc, const Menu::arg *argv)
+{
+	if(g.optflow_enabled) {
+		Serial.printf_P(PSTR("man id: %d\t"),optflow.read_register(ADNS3080_PRODUCT_ID));
+		print_hit_enter();
+
+		while(1){
+			delay(200);
+			optflow.read();
+			Log_Write_Optflow();
+			Serial.printf_P(PSTR("x/dx: %d/%d\t y/dy %d/%d\t squal:%d\n"),
+						optflow.x,
+						optflow.dx,
+						optflow.y,
+						optflow.dy,
+						optflow.surface_quality);
+
+			if(Serial.available() > 0){
+				return (0);
+			}
+		}
+	} else {
+		Serial.printf_P(PSTR("OptFlow: "));
+		print_enabled(false);
+		return (0);
+	}
+}
+#endif
+
 static int8_t
 test_mission(uint8_t argc, const Menu::arg *argv)
 {